Пример #1
0
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);
}
Пример #2
0
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);
  }

}