我尝试用 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 文件看起来不错。有人可以告诉我发生了什么吗?
最佳答案
问题在于算法参数的初始化不好。您为 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 在某个点发散,您可以设置欧几里德距离误差障碍,这样它就不会在剩余的迭代次数中变得更糟。可以在此处找到有关如何设置参数的更多信息:
关于c++11 - 使用 PCL 迭代最近点进行点云配准,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/37853423/