void MissionSpec::observeDistance(float x, float y, float z, const std::string& name)
{
AgentHandlers::ObservationFromDistance_optional& obs = this->mission->AgentSection().front().AgentHandlers().ObservationFromDistance();
if (!obs.present())
{
ObservationFromDistance observer;
observer.Marker().push_back(NamedPoint(x, y, z, name));
obs.set(observer);
}
else
{
obs->Marker().push_back(NamedPoint(x, y, z, name));
}
}
void Deform_With_Seg::FixSef(int index)
{
vector<NamedPoint> points;
for (size_t i = 0; i < sourse_sample.size(); i++)
{
points.push_back(NamedPoint(sourse_sample[i].position[0], sourse_sample[i].position[1], sourse_sample[i].position[2], i));
}
KDTree3<NamedPoint> stree(points.begin(), points.end());
NamedPoint query(target_sample[index].position[0], target_sample[index].position[1], target_sample[index].position[2]);
BoundedSortedArray<PKDTree::Neighbor> k_closest_elems(near_point);
stree.kClosestElements<MetricL2>(query, k_closest_elems, -1);
for (size_t ind = 0; ind < total_seg_size; ind++)
{
float weight_temp = 0;
for (size_t j = 0; j < near_point; j++)
{
int nearest_id = stree.getElements()[k_closest_elems[j].getIndex()].id;
if (sourse_sample[nearest_id].face_id == ind)
{
weight_temp += 1;
}
}
weight_temp /= near_point;
target_seg_prob[index][ind] = weight_temp;
}
}
void Deform_With_Seg::SetMeanFeatureDis()
{
mean_feature_dis = 0;
var_feature_dis = 0;
int count = 0;
vector<float> distance;
distance.clear();
vector<NamedPoint> points;
for (size_t i = 0; i < target_sample.size(); i++)
{
points.push_back(NamedPoint(target_sample[i].position[0], target_sample[i].position[1], target_sample[i].position[2], i));
}
KDTree3<NamedPoint> stree(points.begin(), points.end());
int near_size = 5;
for (size_t i = 0; i < target_sample.size(); i++)
{
NamedPoint query(target_sample[i].position[0], target_sample[i].position[1], target_sample[i].position[2]);
BoundedSortedArray<PKDTree::Neighbor> k_closest_elems(near_size);
stree.kClosestElements<MetricL2>(query, k_closest_elems, -1);
for (size_t j = 0; j < near_size; j++)
{
int nearest_id = stree.getElements()[k_closest_elems[j].getIndex()].id;
float dis_temp = target_sample[nearest_id].dist_with_feature(target_sample[i]);
mean_feature_dis += dis_temp;
distance.push_back(dis_temp);
count++;
}
}
mean_feature_dis /= count;
for(size_t i =0; i < distance.size(); i++)
{
var_feature_dis += (distance[i] - mean_feature_dis) * (distance[i] - mean_feature_dis)/count;
}
cout <<"mean_feature_dis " << mean_feature_dis << " var_feature_dis "<< var_feature_dis << endl;
}
void Deform_With_Seg::ReNormal(vector<Sample> &sample_list)
{
vector<NamedPoint> points;
for (size_t i = 0; i < sample_list.size(); i++)
{
points.push_back(NamedPoint(sample_list[i].position[0], sample_list[i].position[1], sample_list[i].position[2], i));
}
PKDTree stree(points.begin(), points.end());
for (size_t i = 0; i < sample_list.size(); i++)
{
NamedPoint query(sample_list[i].position[0], sample_list[i].position[1], sample_list[i].position[2]);
BoundedSortedArray<PKDTree::Neighbor> k_closest_elems(30);
Eigen::Matrix3d local_covariance_matrix = Eigen::Matrix3d::Constant(3, 3, 0);
Eigen::Vector3d centroid(0, 0, 0);
stree.kClosestElements<MetricL2>(query, k_closest_elems, -1); //-1 means there's no limit on the maximum allowed
for (int kn = 0; kn < k_closest_elems.size(); kn++)
{
if (stree.getElements()[k_closest_elems[kn].getIndex()].id != i)
{
NamedPoint kpoint = stree.getElements()[k_closest_elems[kn].getIndex()];
Eigen::Vector3d v(kpoint.position.x, kpoint.position.y, kpoint.position.z);
centroid += v;
}
}
centroid /= (float)k_closest_elems.size();
for (int kn = 0; kn < k_closest_elems.size(); kn++)
{
if (stree.getElements()[k_closest_elems[kn].getIndex()].id != i)
{
NamedPoint kpoint = stree.getElements()[k_closest_elems[kn].getIndex()];
Eigen::Vector3d v(kpoint.position.x, kpoint.position.y, kpoint.position.z);
v = v - centroid;
local_covariance_matrix += v * v.transpose();
}
}
local_covariance_matrix /= (float)k_closest_elems.size();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>eigensolver(local_covariance_matrix);
Eigen::Vector3d n = eigensolver.eigenvectors().col(0);
vec3 normal(n(0), n(1), n(2));
if ((sample_list[i].normal DOT normal) > 0)
{
sample_list[i].normal = normal;
}
else
{
sample_list[i].normal = -normal;
}
}
}
void Deform_With_Seg::SetDisThresh()
{
vector<Sample> target_sample;
vector<Sample> sourse_sample;
if (sourse_sample.size() > target_sample.size())
{
target_sample = sourse_sample;
sourse_sample = target_sample;
}
else
{
target_sample = target_sample;
sourse_sample = sourse_sample;
}
dis_thread_hold = 0;
vector<NamedPoint> points;
for (size_t i = 0; i < target_sample.size(); i++)
{
points.push_back(NamedPoint(target_sample[i].position[0], target_sample[i].position[1], target_sample[i].position[2], i));
}
KDTree3<NamedPoint> stree(points.begin(), points.end());
for (size_t i = 0; i < sourse_sample.size(); i++)
{
vector<int> near;
near.clear();
NamedPoint query(sourse_sample[i].position[0], sourse_sample[i].position[1], sourse_sample[i].position[2]);
BoundedSortedArray<PKDTree::Neighbor> k_closest_elems(1);
stree.kClosestElements<MetricL2>(query, k_closest_elems, -1); // -1 means there's no limit on the maximum allowed
int nearest_id = stree.getElements()[k_closest_elems[0].getIndex()].id;
dis_thread_hold += dist(sourse_sample[i].position, target_sample[nearest_id].position);
}
dis_thread_hold /= sourse_sample.size();
}
void Deform_With_Seg::UpdataTargetSegnmentPro()
{
vector<NamedPoint> points;
for (size_t i = 0; i < sourse_sample.size(); i++)
{
points.push_back(NamedPoint(sourse_sample[i].position[0], sourse_sample[i].position[1], sourse_sample[i].position[2], i));
}
KDTree3<NamedPoint> stree(points.begin(), points.end());
cout << "begin UpdateProShapetoSeg" << endl;
vector<vector<float>> prob_new = target_seg_prob;
vector<float> total_weight;
total_weight.resize(target_sample.size(), 0);
for (size_t i = 0; i < target_sample.size(); i++)
{
NamedPoint query(target_sample[i].position[0], target_sample[i].position[1], target_sample[i].position[2]);
BoundedSortedArray<PKDTree::Neighbor> k_closest_elems(near_point);
stree.kClosestElements<MetricL2>(query, k_closest_elems, -1);
for (size_t ind = 0; ind < total_seg_size; ind++)
{
float weight_temp = 0.0f;
for (size_t j = 0; j < near_point; j++)
{
int nearest_id = stree.getElements()[k_closest_elems[j].getIndex()].id;
if (sourse_sample[nearest_id].face_id != ind)
{
float weight = -(target_sample[i].dist_with_feature(sourse_sample[nearest_id]) - mean_feature_dis)*(target_sample[i].dist_with_feature(sourse_sample[nearest_id]) - mean_feature_dis) / (var_feature_dis* 2.0f);
weight_temp -= exp(weight)*2.0f;
}
}
for (size_t j = 0; j < target_near_list[i].size(); j++)
{
int sample_index = target_near_list[i][j];
float theta_value = ThetaFunction(target_sample[i], target_sample[sample_index], 1);
weight_temp += ((log(theta_value) - log(1 - theta_value)) * target_seg_prob[sample_index][ind]);
}
prob_new[i][ind] = exp(weight_temp);
total_weight[i] += exp(weight_temp);
}
}
for (size_t i = 0; i < target_sample.size(); i++)
{
for (size_t ind = 0; ind < total_seg_size; ind++)
{
target_seg_prob[i][ind] = prob_new[i][ind] / total_weight[i];
}
}
/*vector<NamedPoint> points;
for (size_t i = 0; i < sourse_sample.size(); i++)
{
points.push_back(NamedPoint(sourse_sample[i].position[0], sourse_sample[i].position[1], sourse_sample[i].position[2], i));
}
KDTree3<NamedPoint> stree(points.begin(), points.end());
cout << "begin UpdateProShapetoSeg" << endl;
vector<vector<float>> prob_new = target_seg_prob;
vector<float> total_weight;
total_weight.resize(target_sample.size(), 0);
for (size_t i = 0; i < target_sample.size(); i++)
{
NamedPoint query(target_sample[i].position[0], target_sample[i].position[1], target_sample[i].position[2]);
BoundedSortedArray<PKDTree::Neighbor> k_closest_elems(near_point);
stree.kClosestElements<MetricL2>(query, k_closest_elems, -1);
for (size_t ind = 0; ind < total_seg_size; ind++)
{
float weight_temp = 0.0f;
for (size_t j = 0; j < near_point; j++)
{
int nearest_id = stree.getElements()[k_closest_elems[j].getIndex()].id;
if (sourse_sample[nearest_id].face_id == ind)
{
float weight = -(target_sample[i].dist_with_feature(sourse_sample[nearest_id]) - mean_feature_dis)*(target_sample[i].dist_with_feature(sourse_sample[nearest_id]) - mean_feature_dis)/(var_feature_dis* 2.0f);
weight_temp += exp(weight);
}
}
for (size_t j = 0; j < target_near_list[i].size(); j++)
{
int sample_index = target_near_list[i][j];
for (size_t jj = 0; jj < total_seg_size; jj++)
{
if (jj != ind)
{
weight_temp+= exp(log(ThetaFunction(target_sample[i], target_sample[sample_index], 0))*target_seg_prob[sample_index][jj]);
}
}
weight_temp += exp(log(ThetaFunction(target_sample[i], target_sample[sample_index], 1)) * target_seg_prob[sample_index][ind]);
}
prob_new[i][ind] = weight_temp;
total_weight[i] += weight_temp;
}
}
for (size_t i = 0; i < target_sample.size(); i++)
{
for (size_t ind = 0; ind < total_seg_size; ind++)
{
target_seg_prob[i][ind] = prob_new[i][ind] / total_weight[i];
}
}*/
}
void Deform_With_Seg::Find_Soft_Map()
{
SetDisThresh();
dis_thread_hold = 2.0f;
soft_map_near.clear();
cout << "get point " << target_sample.size() << endl;
for (size_t i = 0; i < target_sample.size(); i++)
{
vector<int> near_id;
near_id.clear();
soft_map_near.push_back(near_id);
}
float average_dis = 0;
int k = 50;
vector<NamedPoint> points;
for (size_t i = 0; i < sourse_sample.size(); i++)
{
points.push_back(NamedPoint(sourse_sample[i].position[0], sourse_sample[i].position[1], sourse_sample[i].position[2], i));
}
KDTree3<NamedPoint> stree(points.begin(), points.end());
for (size_t i = 0; i < target_sample.size(); i++)
{
vector<int> near;
near.clear();
NamedPoint query(target_sample[i].position[0], target_sample[i].position[1], target_sample[i].position[2]);
BoundedSortedArray<PKDTree::Neighbor> k_closest_elems(k);
stree.kClosestElements<MetricL2>(query, k_closest_elems, -1); // -1 means there's no limit on the maximum allowed
for (size_t ind = 0; ind< k; ind++)
{
int nearest_id = stree.getElements()[k_closest_elems[ind].getIndex()].id;
if ((sourse_sample[nearest_id].normal DOT target_sample[i].normal) > 0 || check_normal == 0)
{
near.push_back(nearest_id);
}
}
if (near.size() >= near_point)
{
int nearest_point_id = near[0];
average_dis += dist(sourse_sample[nearest_point_id].position, target_sample[i].position);
for (size_t ind = 0; ind< near_point/2; ind++)
{
if (dist(sourse_sample[near[ind]].position, target_sample[i].position) < 2.0f * dis_thread_hold)
{
soft_map_near[i].push_back(near[ind]);
}
}
}
else
{
if (check_normal == 0)
{
int nearest_point_id = stree.getElements()[k_closest_elems[0].getIndex()].id;
average_dis += dist(sourse_sample[nearest_point_id].position, target_sample[i].position);
for (size_t ind = 0; ind< near_point; ind++)
{
int nearest_id = stree.getElements()[k_closest_elems[ind].getIndex()].id;
if (dist(sourse_sample[nearest_id].position, target_sample[i].position) < 2.0f * dis_thread_hold)
{
soft_map_near[i].push_back(near[ind]);
}
}
}
}
}
average_dis /= target_sample.size();
float stand_dev = 0.5*average_dis;
//stand_dev = 0.5*dis_thread_hold;
cout << "dev " << stand_dev << endl;
soft_map_weight.clear();
vector<float> sum_weight;
sum_weight.resize(target_sample.size(), 0);
cout << "begin weight" << endl;
for (size_t i = 0; i< soft_map_near.size(); i++)
{
vector<float> weight_t;
weight_t.clear();
//cout << "weight " <<i <<" ";
for (size_t j = 0; j < soft_map_near[i].size(); j++)
{
float dis = dist(target_sample[i].position, sourse_sample[soft_map_near[i][j]].position);
//float dis = target_sample[i].dist_with_feature(sourse_sample[soft_map_near[i][j]]);
float weight = exp(-(dis - average_dis) * (dis - average_dis) / (2 * stand_dev * stand_dev));
//float weight = exp(-(dis - mean_feature_dis) * (dis - mean_feature_dis) / (2 *var_feature_dis));
weight *= target_seg_prob[i][sourse_sample[soft_map_near[i][j]].face_id];
//cout << weight <<" ";
sum_weight[i] += weight;
weight_t.push_back(weight);
}
//cout << endl;
soft_map_weight.push_back(weight_t);
}
cout << "end weight" << endl;
cout << "begin normalize" << endl;
// normalize
for (size_t i = 0; i < soft_map_weight.size(); i++)
{
for (size_t j = 0; j < soft_map_weight[i].size(); j++)
{
soft_map_weight[i][j] /= sum_weight[i];
if (soft_map_weight[i][j] < 0.05)
{
soft_map_weight[i][j] = 0.05;
}
}
}
cout << "end normalize" << endl;
}