TEST (PCL, IterativeClosestPointNonLinear)
{
typedef PointXYZRGB PointT;
PointCloud<PointT>::Ptr temp_src (new PointCloud<PointT>);
copyPointCloud (cloud_source, *temp_src);
PointCloud<PointT>::Ptr temp_tgt (new PointCloud<PointT>);
copyPointCloud (cloud_target, *temp_tgt);
PointCloud<PointT> output;
IterativeClosestPointNonLinear<PointT, PointT> reg;
reg.setInputCloud (temp_src);
reg.setInputTarget (temp_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.941755, 1e-2);
EXPECT_NEAR (transformation (0, 1), 0.147362, 1e-2);
EXPECT_NEAR (transformation (0, 2), -0.281285, 1e-2);
EXPECT_NEAR (transformation (0, 3), 0.029813, 1e-2);
EXPECT_NEAR (transformation (1, 0), -0.111655, 1e-2);
EXPECT_NEAR (transformation (1, 1), 0.990408, 1e-2);
EXPECT_NEAR (transformation (1, 2), 0.139090, 1e-2);
EXPECT_NEAR (transformation (1, 3), -0.001864, 1e-2);
EXPECT_NEAR (transformation (2, 0), 0.297271, 1e-2);
EXPECT_NEAR (transformation (2, 1), -0.092015, 1e-2);
EXPECT_NEAR (transformation (2, 2), 0.939670, 1e-2);
EXPECT_NEAR (transformation (2, 3), 0.042721, 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);
}
TEST (PCL, IterativeClosestPointNonLinear)
{
typedef PointXYZRGB PointT;
PointCloud<PointT>::Ptr temp_src (new PointCloud<PointT>);
copyPointCloud (cloud_source, *temp_src);
PointCloud<PointT>::Ptr temp_tgt (new PointCloud<PointT>);
copyPointCloud (cloud_target, *temp_tgt);
PointCloud<PointT> output;
IterativeClosestPointNonLinear<PointT, PointT> reg;
reg.setInputSource (temp_src);
reg.setInputTarget (temp_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.941755, 1e-2);
EXPECT_NEAR (transformation (0, 1), 0.147362, 1e-2);
EXPECT_NEAR (transformation (0, 2), -0.281285, 1e-2);
EXPECT_NEAR (transformation (0, 3), 0.029813, 1e-2);
EXPECT_NEAR (transformation (1, 0), -0.111655, 1e-2);
EXPECT_NEAR (transformation (1, 1), 0.990408, 1e-2);
EXPECT_NEAR (transformation (1, 2), 0.139090, 1e-2);
EXPECT_NEAR (transformation (1, 3), -0.001864, 1e-2);
EXPECT_NEAR (transformation (2, 0), 0.297271, 1e-2);
EXPECT_NEAR (transformation (2, 1), -0.092015, 1e-2);
EXPECT_NEAR (transformation (2, 2), 0.939670, 1e-2);
EXPECT_NEAR (transformation (2, 3), 0.042721, 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);
// Check again, for all possible caching schemes
for (int iter = 0; iter < 4; iter++)
{
bool force_cache = static_cast<bool> (iter / 2);
bool force_cache_reciprocal = static_cast<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 (temp_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 (temp_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.001);
}
}
