float
Eye3D::dynamic_EMD(const PointCloudPtr target, const vector<float> &wInput,
PointCloudPtr source, const vector<float> &wOutput)
{
_viz->clear();
float radiusArg = 0.5;
IterativeClosestPoint<PointT, PointT> icp;
icp.setInputCloud(source);
icp.setInputWeight(wInput);
icp.setInputTarget(target);
icp.setOutputWeight(wOutput);
pcl::PointCloud<PointT> result;
// Set the transformation epsilon (criterion 2)
icp.setMaximumIterations (150);
icp.setMaxCorrespondenceDistance (100);
icp.setTransformationEpsilon (1e-11);
// Set the euclidean distance difference epsilon (criterion 3)
icp.setEuclideanFitnessEpsilon (1e-11);
icp.setRANSACIterations(0);
icp.align(result);
if (!icp.hasConverged()) {
std::cout << "ICP failed to converged!"<<std::endl;
return -1;
}
if (visualize_EMD) {
/// visualize the align result
int size = result.points.size();
// rank the weights
std::map<float, int> w2rank;
std::vector<float> wInputRank;
m_util::rank(wInput, &w2rank);
for(float w: wInput){
wInputRank.push_back(w2rank[w] + 1);
}
w2rank.clear();
std::vector<float> wOutputRank;
m_util::rank(wOutput, &w2rank);
for(float w: wOutput){
wOutputRank.push_back(w2rank[w] + 1);
}
int f = 50;
for (int i = 0; i < size; i++) {
_viz->add_sphere(target->points[i].x/f,target->points[i].y/f,
target->points[i].z/f, wInputRank[i]* radiusArg, 0, 255, 0);
// _viz->add_sphere(source->points[i].x/f, source->points[i].y/f,
// source->points[i].z/f, wOutputRank[i]* radiusArg, 0, 0, 255);
_viz->add_sphere(result.points[i].x/f, result.points[i].y/f,
result.points[i].z, wOutputRank[i] * radiusArg, 255, 0, 0);
}
// icp.visualize_correspondence(_viz,0);
_viz->reset_camera();
}
return icp.best_EMD_;
}
