使用 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

Answer 2

你的指针有问题，你不需要一些行。使用此代码：

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>::Ptr finalCloud (new pcl::PointCloud<pcl::PointXYZI>);
   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(*cloudOut_new);

  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;

  *finalCloud=*cloudIn;
  *finalCloud+=*cloudOut_new;

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

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

Answer 3

我将setMaxCorrespondenceDistance 更改为 0.005（因为我的点云在这个值附近）并且工作正常。

Answer 4

当你设置输入云时，也许你应该尝试icp.h中定义的setInputSource()，而不是setInputCloud()，这是一个基本函数。我知道这听起来很疯狂，但在我的情况下确实有所不同（PCL 1.8.1）