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.setInputSource (src);
reg.setInputTarget (tgt);
reg.setMaximumIterations (50);
reg.setTransformationEpsilon (1e-8);
// Use a correspondence estimator which needs normals
registration::CorrespondenceEstimationNormalShooting<PointT, PointT, PointT>::Ptr ce (new registration::CorrespondenceEstimationNormalShooting<PointT, PointT, PointT>);
reg.setCorrespondenceEstimation (ce);
// Add rejector
registration::CorrespondenceRejectorSurfaceNormal::Ptr rej (new registration::CorrespondenceRejectorSurfaceNormal);
rej->setThreshold (0); //Could be a lot of rotation -- just make sure they're at least within 0 degrees
reg.addCorrespondenceRejector (rej);
// Register
reg.align (output);
EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ()));
EXPECT_LT (reg.getFitnessScore (), 0.005);
// Check again, for all possible caching schemes
for (int iter = 0; iter < 4; iter++)
{
bool force_cache = (bool) iter/2;
bool force_cache_reciprocal = (bool) iter%2;
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
// Ensure that, when force_cache is not set, we are robust to the wrong input
if (force_cache)
tree->setInputCloud (tgt);
reg.setSearchMethodTarget (tree, force_cache);
pcl::search::KdTree<PointT>::Ptr tree_recip (new pcl::search::KdTree<PointT>);
if (force_cache_reciprocal)
tree_recip->setInputCloud (src);
reg.setSearchMethodSource (tree_recip, force_cache_reciprocal);
// Register
reg.align (output);
EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ()));
EXPECT_LT (reg.getFitnessScore (), 0.005);
}
// 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);
*/
}
