Point cloud registration using PCL Iterative closest point

pcl icp
pcl icp python
pcl icp reciprocal
icp tutorial
pcl/registration/icp h
3d point cloud registration pcl
pcl icp nonlinear
pcl colored icp

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?


Iterative Closest Point - Documentation, After that the ICP algorithm will align the transformed point cloud with the original #include <pcl/registration/icp.h> #include <pcl/visualization/pcl_visualizer.h>  @FäridAlijani finalCloud is the final point cloud with both input clouds cloudIn and cloudOut merged and alligned. cloudOut_new is cloudOut after its registration onto cloudIn . – Finfa811 Aug 22 '18 at 14:39


You have a problem with pointers and you don't need some lines. Use this 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>::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);
}

Point cloud registration using PCL Iterative closest point, The problem lies in bad initialisation of the algorithmic parameters. You choose the following parameters for your ICP Algorithm: icp.setInputCloud(cloudOut); icp​  Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) More PCL_DEPRECATED (PointCloudSourceConstPtr const getInputCloud(),"[pcl::registration::Registration::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.") Get a pointer to the input point cloud dataset target. More


I changed the setMaxCorrespondenceDistance to 0.005 (because my point cloud is around this value) and worked fine.

[PDF] Registration with the Point Cloud Library, well-known Iterative Closest Point (ICP) algorithm. The article provides an overview on registration algorithms, usage examples of their PCL implementations,  Point Cloud Library (PCL). Contribute to PointCloudLibrary/pcl development by creating an account on GitHub. pcl / tools / iterative_closest_point.cpp.


When you set the input cloud, maybe you should try setInputSource() defined in icp.h, instead of setInputCloud(), which is a base function. I know this sounds nuts, but it does make a difference in my case (PCL 1.8.1)

Registration Tutorials, This tutorial gives an example of how to use the iterative closest point algorithm to see if one PointCloud is just a rigid transformation of another PointCloud. Registration Tutorials. The PCL Registration API; How to use iterative closest point; How to incrementally register pairs of clouds; Interactive Iterative Closest Point; How to use Normal Distributions Transform; In-hand scanner for small objects; Robust pose estimation of rigid objects; Sampleconsensus Tutorials; segmentation Tutorials


pcl/interactive_icp.cpp at master · PointCloudLibrary/pcl · GitHub, pcl/doc/tutorials/content/sources/interactive_icp/interactive_icp.cpp. Find file Copy path <pcl/point_types.h>. #include <pcl/registration/icp.h> PointCloudT​::Ptr cloud_icp (new PointCloudT); // ICP output point cloud. // Checking *​cloud_tr = *cloud_icp; // We backup cloud_icp into cloud_tr for later use. // The Iterative  This document demonstrates using the Iterative Closest Point algorithm in order to incrementally register a series of point clouds two by two. The idea is to transform all the clouds in the first cloud’s frame.


Point Cloud Library (PCL) Users mailing list, Hello all; Prior to point cloud registration , I am testing the ICP algorithm. I am having For tests I am using a point cloud with 48000 points. ICP (1/6) Registration using theIterative Closest Point(ICP) Algorithm Given aninputpoint cloud and atargetpoint cloud 1.determine pairs ofcorresponding points, 2.estimate a transformation that minimizes the distances between the correspondences, 3.apply the transformation to align input and target.


Iterative closest point, Iterative closest point (ICP) is an algorithm employed to minimize the difference between two The Iterative Closest Point algorithm contrasts with the Kabsch algorithm and other PCL (Point Cloud Library) is an open-source framework for n-dimensional point clouds and 3D geometry processing. Point set registration​  * run iterative closest point, and composite the resulting ICP transformation * with the translations from demeaning to obtain a transformation between * the original point clouds.