Point cloud registration using PCL Iterative closest point
I try to perform ICP with PCL,
but pcl::transformPointCloud doesn't work. This my code:
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);
}
and this is what I get as result:
The transformpointcloud is not working, but the saved PCD file having two clouds looks fine. Can someone please suggest me what is happening?
Answer
The problem lies in bad initialisation of the algorithmic parameters. You choose the following parameters for your ICP Algorithm:
icp.setInputCloud(cloudOut);
icp.setInputTarget(cloudIn);
icp.setMaximumIterations (500);
icp.setTransformationEpsilon (1e-9);
icp.setMaxCorrespondenceDistance (0.05);
icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold (1.5);
The value for setMaximumIterations() should be larger if the initial alignment is poor and small if the initial alignment is already quite good. A value of 500 iterations is to high and should be decreased to a value in the range of 10 - 100 (You can always increase this later when fine-tuning).
The value for setRANSACOutlierRejectionThreshold() should typically be just below the resolution of your scanner, e.g,
The value for setMaxCorrespondenceDistance() should be set to approx. 100 times the value of setRANSACOutlierRejectionThreshold() (This depends on how good your initial guess is and can also be fine-tuned).
The value for setTransformationEpsilon() is your convergence criterion. If the sum of differences between current and last transformation is smaller than this threshold, the registration succeeded and will terminated. A value of (1e-9) seems quite reasonable and should give good initial results.
The value setEuclideanFitnessEpsilon() is an divergence threshold. Here you can define the delta between two consecutive steps in the ICP loop for which the algorithm stops. That means that you can set an euclidean distance error barrier if the ICP diverges at some point, so that it does not become even worse over the remaining number of iterations.
More information about how to set the parameters can be found here: