c++11 - 使用 PCL 迭代最近点进行点云配准

Question

我尝试用 PCL 执行 ICP，

但是 pcl::transformPointCloud不起作用。这是我的代码:

int
 main ()
{
   pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn (new pcl::PointCloud<pcl::PointXYZI>);
   pcl::PointCloud<pcl::PointXYZI>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZI>);
   pcl::PointCloud<pcl::PointXYZI> finalCloud ;
   pcl::PointCloud<pcl::PointXYZI> finalCloud1 ;
   pcl::PointCloud<pcl::PointXYZI>::Ptr cloudOut_new (new pcl::PointCloud<pcl::PointXYZI>) ;

   if(pcl::io::loadPCDFile ("/ICP_PCL/000.pcd", *cloudIn)==-1)
   {
     PCL_ERROR ("Couldn't read first file! \n");
     return (-1);
   }

   if(pcl::io::loadPCDFile ("/ICP_PCL/001.pcd", *cloudOut)==-1)
   {
     PCL_ERROR ("Couldn't read second input file! \n");
     return (-1);
   }

  pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
  icp.setInputCloud(cloudOut);
  icp.setInputTarget(cloudIn);
  icp.setMaximumIterations (500);
  icp.setTransformationEpsilon (1e-9);
  icp.setMaxCorrespondenceDistance (0.05);
  icp.setEuclideanFitnessEpsilon (1);
  icp.setRANSACOutlierRejectionThreshold (1.5);

  icp.align(finalCloud);

  if (icp.hasConverged())
  {
      std::cout << "ICP converged." << std::endl
                << "The score is " << icp.getFitnessScore() << std::endl;
      std::cout << "Transformation matrix:" << std::endl;
      std::cout << icp.getFinalTransformation() << std::endl;
  }
  else std::cout << "ICP did not converge." << std::endl;

  Eigen::Matrix4f transformationMatrix = icp.getFinalTransformation ();
  std::cout<<"trans %n"<<transformationMatrix<<std::endl;

  pcl::transformPointCloud( *cloudOut, *cloudOut_new, transformationMatrix);

  pcl::io::savePCDFileASCII ("/ICP_PCL/IcpResult3.pcd", finalCloud);

  finalCloud1=*cloudIn;
  finalCloud1+=*cloudOut_new;

   boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer("3D Viewer"));
   viewer->setBackgroundColor(0,0,0);
   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> single_color1 (cloudIn, 0, 0, 200);
   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> single_color2 (cloudOut_new, 200, 0, 0);

   viewer->addPointCloud<pcl::PointXYZI> (cloudIn,single_color1, "sample_cloud_1");
   viewer->addPointCloud<pcl::PointXYZI> (cloudOut_new, single_color2, "sample_cloud_2");

    viewer->addCoordinateSystem(1.0);
      while(!viewer->wasStopped())
      {
          viewer->spinOnce();
          boost::this_thread::sleep (boost::posix_time::microseconds(100000));
      }
 return (0);
}

这就是我得到的结果:

transformpointcloud不工作，但保存的有两朵云的 PCD 文件看起来不错。有人可以告诉我发生了什么吗？

Answer 1

问题在于算法参数的初始化不好。您为 ICP 算法选择以下参数:

icp.setInputCloud(cloudOut);
icp.setInputTarget(cloudIn);
icp.setMaximumIterations (500);
icp.setTransformationEpsilon (1e-9);
icp.setMaxCorrespondenceDistance (0.05);
icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold (1.5);

setMaximumIterations() 的值如果初始对齐很差，则应该更大，如果初始对齐已经很好，则应该较小。 500 的值迭代次数太高，应该减少到范围内的值 10 - 100 (您可以稍后在微调时增加此值)。
setRANSACOutlierRejectionThreshold() 的值通常应该略低于您的扫描仪的分辨率，例如，
setMaxCorrespondenceDistance() 的值应该设置为大约。值 setRANSACOutlierRejectionThreshold() 的 100 倍(这取决于您最初的猜测有多好，也可以进行微调)。
setTransformationEpsilon() 的值是你的收敛标准。如果当前变换与上次变换的差值之和小于该阈值，则注册成功并终止。 (1e-9) 的值看起来很合理，应该会给出很好的初步结果。

值 setEuclideanFitnessEpsilon()是发散阈值。您可以在此处定义算法停止的 ICP 循环中两个连续步骤之间的增量。这意味着如果 ICP 在某个点发散，您可以设置欧几里德距离误差障碍，这样它就不会在剩余的迭代次数中变得更糟。

可以在此处找到有关如何设置参数的更多信息:

PCL ICP presentation

Paper about registration with PCL

PCL Mailing list about ICP 1

PCL Mailing list about ICP 2

PCL ICP documentation