Registration and matching is a common technique used in several different fields that consists of finding common structures or features in two datasets and using them to stitch the datasets together. In the case of point cloud processing, this can be achieved as easily as finding where one point cloud ends and where the other one starts. These techniques are very useful when obtaining point clouds from moving sources at a high rate, and we have an estimate of the movement of the source. With this algorithm, we can stitch each of those point clouds together and reduce the uncertainty in our sensor pose estimation.

PCL provides an algorithm named Iterative Closest Point (ICP) to perform registration and matching. We will use this algorithm in the following example, which can be found in the source directory of the chapter10_tutorials package, and it's named pcl_matching.cpp:

#include <ros/ros.h> 
#include <pcl/point_cloud.h> 
#include <pcl/registration/icp.h> 
#include <pcl_conversions/pcl_conversions.h> 
#include <sensor_msgs/PointCloud2.h> 
 
class cloudHandler 
{ 
public: 
    cloudHandler() 
    { 
        pcl_sub = nh.subscribe("pcl_downsampled", 10, 
         &cloudHandler::cloudCB, this);        pcl_pub = 
         nh.advertise<sensor_msgs::PointCloud2>("pcl_matched", 1); 
    } 
 
    void cloudCB(const sensor_msgs::PointCloud2 &input) 
    { 
        pcl::PointCloud<pcl::PointXYZ> cloud_in; 
        pcl::PointCloud<pcl::PointXYZ> cloud_out; 
        pcl::PointCloud<pcl::PointXYZ> cloud_aligned; 
        sensor_msgs::PointCloud2 output; 
 
        pcl::fromROSMsg(input, cloud_in); 
 
        cloud_out = cloud_in; 
 
        for (size_t i = 0; i < cloud_in.points.size (); ++i) 
        { 
            cloud_out.points[i].x = cloud_in.points[i].x + 0.7f; 
        } 
 
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> 
         icp; 
        icp.setInputSource(cloud_in.makeShared()); 
        icp.setInputTarget(cloud_out.makeShared()); 
 
        icp.setMaxCorrespondenceDistance(5); 
        icp.setMaximumIterations(100); 
        icp.setTransformationEpsilon (1e-12); 
        icp.setEuclideanFitnessEpsilon(0.1); 
 
        icp.align(cloud_aligned); 
 
        pcl::toROSMsg(cloud_aligned, output); 
        pcl_pub.publish(output); 
    } 
 
protected: 
    ros::NodeHandle nh; 
    ros::Subscriber pcl_sub; 
    ros::Publisher pcl_pub; 
}; 
 
main(int argc, char **argv) 
{ 
    ros::init(argc, argv, "pcl_matching"); 
 
    cloudHandler handler; 
 
    ros::spin(); 
 
    return 0; 
} 

This example uses the pcl_downsampled topic as the input source of point clouds in order to improve the performance of the algorithm; the end result is published in the pcl_matched topic. The algorithm used for registration and matching takes three-point clouds—the first one is the point cloud to transform, the second one is the fixed cloud to which the first one should be aligned, and the third one is the end result point cloud:

pcl::PointCloud<pcl::PointXYZ> cloud_in; 
pcl::PointCloud<pcl::PointXYZ> cloud_out; 
pcl::PointCloud<pcl::PointXYZ> cloud_aligned; 

To simplify matters and since we don't have a continuous source of point clouds, we are going to use the same original point cloud as the fixed cloud but displaced on the x axis. The expected behavior of the algorithm would then be to align both point clouds together:

cloud_out = cloud_in; 
 
for (size_t i = 0; i < cloud_in.points.size (); ++i) 
{ 
  cloud_out.points[i].x = cloud_in.points[i].x + 0.7f; 
} 

The next step is to call the ICP algorithm to perform the registration and matching. This iterative algorithm uses Singular Value Decomposition (SVD) to calculate the transformations to be done on the input point cloud towards decreasing the gap to the fixed point cloud. The algorithm has three basic stopping conditions:

• The difference between the previous and current transformations is smaller than a certain threshold. This threshold can be set through the setTransformationEpsilon function.
• The number of iterations has reached the maximum set by the user.
  This maximum can be set through the setMaximumIterations function.
• Finally, the sum of the Euclidean squared errors between two consecutive steps in the loop is below a certain threshold. This specific threshold can be set through the setEuclideanFitnessEpsilon function.

Another interesting parameter that is used to improve the accuracy of the result is the correspondence distance, which can be set through the setMaxCorrespondanceDistance function. This parameter defines the minimum distance that two correspondent points need to have between them to be considered in the alignment process.

With all of these parameters, the fixed point cloud and the input point cloud, the algorithm is capable of performing the registration and matching and returning the end result point cloud after the iterative transformations:

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; 
icp.setInputSource(cloud_in.makeShared()); 
icp.setInputTarget(cloud_out.makeShared()); 
icp.setMaxCorrespondenceDistance(5); 
icp.setMaximumIterations(100); 
icp.setTransformationEpsilon (1e-12); 
icp.setEuclideanFitnessEpsilon(0.1); 
icp.align(cloud_aligned); 

Finally, the resulting point cloud is converted into PointCloud2 and published through the corresponding topic:

pcl::toROSMsg(cloud_aligned, output); 
pcl_pub.publish(output); 

In order to run this example, we need to follow the same instructions as the filtering and downsampling example, starting with roscore in one Terminal:

    $ roscore  

In a second Terminal, we will run the pcl_read example and a source of data:

    $ roscd chapter10_tutorials/data
    $ rosrun chapter10_tutorials pcl_read  

In a third Terminal, we will run the filtering example:

    $ rosrun chapter10_tutorials pcl_filter  

In a fourth Terminal, we will run the downsampling example:

    $ rosrun chapter10_tutorials pcl_downsampling  

Finally, we will run the registration and matching node that requires the
pcl_downsampled topic, which is produced by the chain of nodes run before:

    $ rosrun chapter10_tutorials pcl_matching  

The end result can be seen in the following image, which has been obtained from rviz. The blue one is the original point cloud obtained from the PCD file, and the white point cloud is the aligned one obtained from the ICP algorithm. It has to be noted that the original point cloud was translated in the x axis, so the results are consistent with the point cloud, completely overlapping the translated image, as shown in the following screenshot: