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); */ }