使用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()应设置为约.价值的100倍setRANSACOutlierRejectionThreshold()(这取决于你的初始猜测有多好,也可以微调).

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

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

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

• PCL ICP演示
• 关于PCL注册的论文
• 关于ICP 1的PCL邮件列表
• 关于ICP 2的PCL邮件列表
• PCL 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);
}