TEST (PCL, IterativeClosestPoint)
{
IterativeClosestPoint<PointXYZ, PointXYZ> reg;
reg.setInputCloud (cloud_source.makeShared ());
reg.setInputTarget (cloud_target.makeShared ());
reg.setMaximumIterations (50);
reg.setTransformationEpsilon (1e-8);
reg.setMaxCorrespondenceDistance (0.05);
// Register
reg.align (cloud_reg);
EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ()));
Eigen::Matrix4f transformation = reg.getFinalTransformation ();
EXPECT_NEAR (transformation (0, 0), 0.8806, 1e-3);
EXPECT_NEAR (transformation (0, 1), 0.036481287330389023, 1e-2);
EXPECT_NEAR (transformation (0, 2), -0.4724, 1e-3);
EXPECT_NEAR (transformation (0, 3), 0.03453, 1e-3);
EXPECT_NEAR (transformation (1, 0), -0.02354, 1e-3);
EXPECT_NEAR (transformation (1, 1), 0.9992, 1e-3);
EXPECT_NEAR (transformation (1, 2), 0.03326, 1e-3);
EXPECT_NEAR (transformation (1, 3), -0.001519, 1e-3);
EXPECT_NEAR (transformation (2, 0), 0.4732, 1e-3);
EXPECT_NEAR (transformation (2, 1), -0.01817, 1e-3);
EXPECT_NEAR (transformation (2, 2), 0.8808, 1e-3);
EXPECT_NEAR (transformation (2, 3), 0.04116, 1e-3);
EXPECT_EQ (transformation (3, 0), 0);
EXPECT_EQ (transformation (3, 1), 0);
EXPECT_EQ (transformation (3, 2), 0);
EXPECT_EQ (transformation (3, 3), 1);
}
int main(int argc, char **argv) {
PLYReader ply_reader;
PCDWriter pcd_writer;
PointCloud<PointXYZRGB>::Ptr source_pointer (new PointCloud<PointXYZRGB>);
PointCloud<PointXYZRGB>::Ptr target_pointer (new PointCloud<PointXYZRGB>);
PointCloud<PointXYZRGB> output_cloud;
std::string source_filename = argv[1];
std::string target_filename = argv[2];
ply_reader.read(source_filename, *source_pointer);
ply_reader.read(target_filename, *target_pointer);
IterativeClosestPoint<PointXYZRGB, PointXYZRGB> icp;
#if PCL_MINOR_VERSION > 6
icp.setInputSource(source_pointer);
#else
icp.setInputCloud(source_pointer);
#endif
icp.setInputTarget(target_pointer);
icp.align(output_cloud);
pcd_writer.write("icp_test_face.pcd", output_cloud);
return 0;
}
TEST (PCL, IterativeClosestPoint_PointToPlane)
{
typedef PointNormal PointT;
PointCloud<PointT>::Ptr src (new PointCloud<PointT>);
copyPointCloud (cloud_source, *src);
PointCloud<PointT>::Ptr tgt (new PointCloud<PointT>);
copyPointCloud (cloud_target, *tgt);
PointCloud<PointT> output;
NormalEstimation<PointNormal, PointNormal> norm_est;
norm_est.setSearchMethod (search::KdTree<PointNormal>::Ptr (new search::KdTree<PointNormal>));
norm_est.setKSearch (10);
norm_est.setInputCloud (tgt);
norm_est.compute (*tgt);
IterativeClosestPoint<PointT, PointT> reg;
typedef registration::TransformationEstimationPointToPlane<PointT, PointT> PointToPlane;
boost::shared_ptr<PointToPlane> point_to_plane (new PointToPlane);
reg.setTransformationEstimation (point_to_plane);
reg.setInputCloud (src);
reg.setInputTarget (tgt);
reg.setMaximumIterations (50);
reg.setTransformationEpsilon (1e-8);
// Register
reg.align (output);
EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ()));
// We get different results on 32 vs 64-bit systems. To address this, we've removed the explicit output test
// on the transformation matrix. Instead, we're testing to make sure the algorithm converges to a sufficiently
// low error by checking the fitness score.
/*
Eigen::Matrix4f transformation = reg.getFinalTransformation ();
EXPECT_NEAR (transformation (0, 0), 0.9046, 1e-2);
EXPECT_NEAR (transformation (0, 1), 0.0609, 1e-2);
EXPECT_NEAR (transformation (0, 2), -0.4219, 1e-2);
EXPECT_NEAR (transformation (0, 3), 0.0327, 1e-2);
EXPECT_NEAR (transformation (1, 0), -0.0328, 1e-2);
EXPECT_NEAR (transformation (1, 1), 0.9968, 1e-2);
EXPECT_NEAR (transformation (1, 2), 0.0851, 1e-2);
EXPECT_NEAR (transformation (1, 3), -0.0003, 1e-2);
EXPECT_NEAR (transformation (2, 0), 0.4250, 1e-2);
EXPECT_NEAR (transformation (2, 1), -0.0641, 1e-2);
EXPECT_NEAR (transformation (2, 2), 0.9037, 1e-2);
EXPECT_NEAR (transformation (2, 3), 0.0413, 1e-2);
EXPECT_EQ (transformation (3, 0), 0);
EXPECT_EQ (transformation (3, 1), 0);
EXPECT_EQ (transformation (3, 2), 0);
EXPECT_EQ (transformation (3, 3), 1);
*/
EXPECT_LT (reg.getFitnessScore (), 0.001);
}
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_;
}
bool
IncrementalPoseEstimatorFromRgbFeatures::optimizeWithICP(
pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_source,
Pose3D& depth_pose,
int closest_view_index)
{
pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_target = m_image_data[closest_view_index].sampled_cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_reg;
Pose3D new_depth_pose = depth_pose;
ntk_dbg_print(cloud_source->points.size(), 1);
ntk_dbg_print(cloud_target->points.size(), 1);
IterativeClosestPoint<PointXYZ, PointXYZ> reg;
reg.setInputCloud (cloud_target);
reg.setInputTarget (cloud_source);
reg.setMaximumIterations (50);
reg.setTransformationEpsilon (1e-5);
reg.setMaxCorrespondenceDistance (0.5);
reg.align (cloud_reg);
if (!reg.hasConverged())
{
ntk_dbg(1) << "ICP did not converge, ignoring.";
return false;
}
ntk_dbg_print(reg.getFitnessScore(), 1);
Eigen::Matrix4f t = reg.getFinalTransformation ();
cv::Mat1f T(4,4);
//toOpencv(t,T);
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
T(r,c) = t(r,c);
Pose3D icp_pose;
icp_pose.setCameraTransform(T);
new_depth_pose.applyTransformAfter(icp_pose);
depth_pose = new_depth_pose;
return true;
}
bool RelativePoseEstimatorICP :: estimateNewPose(const RGBDImage& image)
{
if (m_ref_cloud.points.size() < 1)
{
ntk_dbg(1) << "Reference cloud was empty";
return false;
}
PointCloud<PointXYZ>::Ptr target = m_ref_cloud.makeShared();
PointCloud<PointXYZ>::Ptr source (new PointCloud<PointXYZ>());
rgbdImageToPointCloud(*source, image);
PointCloud<PointXYZ>::Ptr filtered_source (new PointCloud<PointXYZ>());
PointCloud<PointXYZ>::Ptr filtered_target (new PointCloud<PointXYZ>());
pcl::VoxelGrid<pcl::PointXYZ> grid;
grid.setLeafSize (m_voxel_leaf_size, m_voxel_leaf_size, m_voxel_leaf_size);
grid.setInputCloud(source);
grid.filter(*filtered_source);
grid.setInputCloud(target);
grid.filter(*filtered_target);
PointCloud<PointXYZ> cloud_reg;
IterativeClosestPoint<PointXYZ, PointXYZ> reg;
reg.setMaximumIterations (m_max_iterations);
reg.setTransformationEpsilon (1e-5);
reg.setMaxCorrespondenceDistance (m_distance_threshold);
reg.setInputCloud (filtered_source);
reg.setInputTarget (filtered_target);
reg.align (cloud_reg);
if (!reg.hasConverged())
{
ntk_dbg(1) << "ICP did not converge, ignoring.";
return false;
}
Eigen::Matrix4f t = reg.getFinalTransformation ();
cv::Mat1f T(4,4);
//toOpencv(t,T);
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
T(r,c) = t(r,c);
Pose3D icp_pose;
icp_pose.setCameraTransform(T);
ntk_dbg_print(icp_pose.cvTranslation(), 1);
// Pose3D stores the transformation from 3D space to image.
// So we have to invert everything so that unprojecting from
// image to 3D gives the right transformation.
// H2' = ICP * H1'
m_current_pose.resetCameraTransform();
m_current_pose = *image.calibration()->depth_pose;
m_current_pose.invert();
m_current_pose.applyTransformAfter(icp_pose);
m_current_pose.invert();
return true;
}