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()的值应设置为约。 setTransformationEpsilon()值的100倍（这取决于您的初始猜测有多好，也可以进行微调）。

(1e-9)的值是您的收敛标准。如果当前和最后一次转换之间的差异总和小于此阈值，则注册成功并将终止。值setEuclideanFitnessEpsilon()似乎非常合理，应该会给出良好的初步结果。

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

有关如何设置参数的更多信息，请访问：

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

设置输入云时，您可以尝试setInputSource()中定义的icp.h，而不是setInputCloud()，这是一个基本功能。我知道这听起来很疯狂，但它确实对我的情况产生了影响（PCL 1.8.1）

使用PCL迭代最近点进行点云注册

4 个答案: