In this first example, the reader will learn how to create PCL point clouds composed solely of pseudorandom points. The PCL point clouds will then be published periodically through a topic named /pcl_output. This example provides practical knowledge on how to generate point clouds with custom data and how to convert them to the corresponding ROS message type in order to broadcast point clouds to subscribers. The source code for this first example can be found in the chapter10_tutorials/src folder, and it is named pcl_create.cpp:

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_create");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;

    // Fill in the cloud data
    cloud.width = 100;
    cloud.height = 1;
    cloud.points.resize(cloud.width * cloud.height);

    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
        cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }

    //Convert the cloud to ROS message
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

The first step in this, and every other snippet, is including the appropriate header files; in this case, we'll include a few PCL-specific headers as well as the standard ROS header and the one that contains the declarations for the PointCloud2 message:

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

After the node initialization, a PointCloud2 ROS publisher is created and advertised; this publisher will later be used to publish the point clouds created through PCL. Once the publisher is created, two variables are defined. The first one, of the PointCloud2 type, is the message type that will be used to store the information to be sent through the publisher. The second variable, of the PointCloud<PointXYZ> type, is the native PCL type that will be used to generate the point cloud in the first place:

ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
pcl::PointCloud<pcl::PointXYZ> cloud;
sensor_msgs::PointCloud2 output;

The next step is to generate the point cloud with relevant data. In order to do so, we need to allocate the required space in the point cloud structure as well as set the appropriate field. In this case, the point cloud created will be of size 100. Since this point cloud is not meant to represent an image, the height will only be of size 1:

// Fill in the cloud data
cloud.width = 100;
cloud.height = 1;
cloud.points.resize(cloud.width * cloud.height);

With the space allocated and the appropriate fields set, the point cloud is filled with random points between 0 and 1024:

for (size_t i = 0; i < cloud.points.size (); ++i)
{
  cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
  cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
  cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}

At this point, the cloud has been created and filled with data. Since this node is meant to be a data source, the next and last step in this snippet is to convert the PCL point cloud type into a ROS message type and publish it. In order to perform the conversion, the toROSMSg function will be used, performing a deep copy of the data from the PCL point cloud type to the PointCloud2 message.

Finally, the PointCloud2 message is published periodically at a rate of one hertz in order to have a constant source of information, albeit immutable:

//Convert the cloud to ROS message
pcl::toROSMsg(cloud, output);
output.header.frame_id = "odom";

ros::Rate loop_rate(1);
while (ros::ok())
{
  pcl_pub.publish(output);
  ros::spinOnce();
  loop_rate.sleep();
}

Perhaps the reader has also noticed that the frame_id field in the message header has been set to the odom value; this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer.

In order to run this example, the first step is to open a terminal and run the roscore command:

$ roscore

In another terminal, the following command will run the example:

rosrun chapter10_tutorials pcl_create

To visualize the point cloud, the RViz visualizer must be run with the following command:

$ rosrun rviz rviz

Once rviz has been loaded, a PointCloud2 object needs to be added by clicking on Add and adding the pcl_output topic. The reader must make sure to set odom as the fixed frame in the Global Options section. If everything has worked properly, a randomly spread point cloud should be shown in the 3D view, just as in the following screenshot:

PCL provides a standard file format to load and store point clouds to the disk as it is a common practice among researchers to share interesting datasets for other people to experiment with. This format is named PCD, and it has been designed to support PCL-specific extensions.

The format is very simple; it starts with a header containing information about the point type and the number of elements in the point cloud, followed by a list of points conforming to the specified type. The following lines are an example of a PCD file header:

# .PCD v.5 - Point Cloud Data file format
FIELDS x y z intensity distance sid
SIZE 4 4 4 4 4 4
TYPE F F F F F F
COUNT 1 1 1 1 1 1
WIDTH 460400
HEIGHT 1
POINTS 460400
DATA ascii

Reading PCD files can be done through the PCL API, which makes it a very straightforward process. The following example can be found in chapter10_tutorials/src, and it is named pcl_read.cpp. The following example shows how to load a PCD file and publish the resulting point cloud as an ROS message:

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_read");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);

    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZ> cloud;

    pcl::io::loadPCDFile ("test_pcd.pcd", cloud);

    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

As always, the first step is to include the required header files. In this particular case, the only new header file that has been added is pcl/io/pcd_io.h, which contains the required definitions to load and store point clouds to PCD and other file formats.

The main difference between the previous example and this new one is simply the mechanism used to obtain the point cloud. While in the first example, we manually filled the point cloud with random points, in this case, we just load them from the disk:

pcl::io::loadPCDFile ("test_pcd.pcd", cloud);

As we can see, the process of loading a PCD file has no complexity whatsoever. Further versions of the PCD file format also allow reading and writing of the current origin and orientation of the point cloud.

In order to run the previous example, we need to access the data directory in the package provided, which includes an example PCD file containing a point cloud that will be used further in this chapter:

$ roscd chapter10_tutorials/data
$ rosrun chapter10_tutorials pcl_read

As in the previous example, the point cloud can be easily visualized through the RViz visualizer:

Obvious though it may sound, the second interesting operation when dealing with PCD files is creating them. In the following example, our goal is to subscribe to a sensor_msgs/PointCloud2 topic and store the received point clouds into a file. The code can be found in chapter10_tutorials, and it is named pcl_write.cpp:

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(input, cloud);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_write");

    ros::NodeHandle nh;
    ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);

    ros::spin();

    return 0;
}

The topic subscribed to is the same used in the two previous examples, namely pcl_output, so they can be linked together for testing:

ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);

When a message is received, the callback function is called. The first step in this callback function is to define a PCL cloud and convert PointCloud2 that is received, using the pcl_conversions function fromROSMsg. Finally, the point cloud is saved to the disk in the ASCII format, but it could also be saved in the binary format, which will generate smaller PCD files:

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(input, cloud);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}

In order to be able to run this example, it is necessary to have a publisher providing point clouds through the pcl_output topic. In this particular case, we will use the pcl_read example shown earlier, which fits this requirement. In three different terminals, we will run the roscore, pcl_read, and pcl_write node:

$ roscore
$ roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_read
$ roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_write

If everything worked properly, after the first (or second) message is produced, the pcl_write node should have created a file named write_pcd_test.pcd in the data directory of the chapter10_tutorials package.

PCL provides several ways of visualizing point clouds. The first and simplest is through the basic cloud viewer, which is capable of representing any sort of PCL point cloud in a 3D viewer, while at the same time providing a set of callbacks for user interaction. In the following example, we will create a small node that will subscribe to sensor_msgs/PointCloud2 and the node will display sensor_msgs/PointCloud2 using cloud_viewer (basic) from the library. The code for this example can be found in the chapter10_tutorials/src source directory, and it is named pcl_visualize.cpp:

#include <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
        pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this);
        viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(input, cloud);

        viewer.showCloud(cloud.makeShared());
    }

    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }

protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_visualize");

    cloudHandler handler;

    ros::spin();

    return 0;
}

The code for this particular example introduces a different pattern; in this case, all of our functionality is encapsulated in a class, which provides a clean way of sharing variables with the callback functions, as opposed to using global variables.

The constructor implicitly initializes the node handle through the default constructor, which is automatically called for the missing objects in the initializer list. The cloud handle is explicitly initialized with a very simple string, which corresponds to the window name, after everything is correctly initialized. The subscriber to the pcl_output topic is set as well as a timer, which will trigger a callback every 100 milliseconds. This timer is used to periodically check whether the window has been closed and, if this is the case, shut down the node:

cloudHandler()
: viewer("Cloud Viewer")
{
  pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this);
  viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
}

The point cloud callback function is not very different from the previous examples except that, in this particular case, the PCL point cloud is passed directly to the viewer through the showCloud function, which automatically updates the display:

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(input, cloud);

  viewer.showCloud(cloud.makeShared());
}

Since the viewer window usually comes with a close button as well as a keyboard shortcut to close the window, it is important to take into account this event and act upon it by, for example, shutting down the node. In this particular case, we are handling the current state of the window in a callback, which is called through a ROS timer every 100 milliseconds. If the viewer has been closed, our action is to simply shut down the node:

void timerCB(const ros::TimerEvent&)
{
  if (viewer.wasStopped())
  {
    ros::shutdown();
  }
}

To execute this example, and any other for that matter, the first step is to run the roscore command in a terminal:

$ roscore

In a second terminal, we will run the pcl_read example and a source of data, such as a reminder, using the following commands:

$ roscd chapter10_tutorials/data
$ rosrun chapter10_tutorials pcl_read

Finally, in a third terminal, we will run the following command:

$ rosrun chapter10_tutorials pcl_visualize

Running this code will cause a window to launch; this window will display the point cloud contained in the test PCD file provided with the examples. The following screenshot shows this:

The current example uses the simplest possible viewer, namely the PCL cloud_viewer, but the library also provides a much more complex and complete visualization component named PCLVisualizer. This visualizer is capable of displaying point clouds, meshes, and surfaces, as well as including multiple viewports and color spaces. An example of how to use this particular visualizer is provided in the chapter10_tutorials source directory named pcl_visualize2.cpp.

In general, all the visualizers provided by PCL use the same underlying functionality and work in much the same way. The mouse can be used to move around the 3D view; in combination with the shift, it allows you to translate the image, and in combination with the control, it allows you to rotate the image. Finally, upon pressing H, the help text is printed in the current terminal, which should look like the following screenshot:

The two main issues that we may face when attempting to process point clouds are excessive noise and excessive density. The former causes our algorithms to misinterpret the data and produce incorrect or inaccurate results, whereas the latter makes our algorithms take a long time to complete their operation. In this section, we will provide insight into how to reduce the amount of noise or outliers of our point clouds and how to reduce the point density without losing valuable information.

The first part is to create a node that will take care of filtering outliers from the point clouds produced in the pcl_output topic and sending them back through the pcl_filtered topic. The example can be found in the source directory of the chapter10_tutorials package, and it is named pcl_filter.cpp:

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/statistical_outlier_removal.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_filtered", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2& input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);

        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);

        pcl::toROSMsg(cloud_filtered, 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_filter");

    cloudHandler handler;

    ros::spin();

    return 0;
}

Just as with the previous example, this one uses a class that contains a publisher as a member variable that is used in the callback function. The callback function defines two PCL point clouds, one for input messages and one for the filtered point cloud. As always, the input point cloud is converted using the standard conversion functions:

pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
sensor_msgs::PointCloud2 output;

pcl::fromROSMsg(input, cloud);

Now, this is where things start getting interesting. In order to perform filtering, we will use the statistical outlier removal algorithm provided by PCL. This algorithm performs an analysis of the point cloud and removes those points that do not satisfy a specific statistical property, which, in this case, is the average distance in a neighborhood, removing all of those points that deviate too much from the average. The number of neighbors to use for the average computation can be set by the setMeanK function, and the multiplier on the standard deviation threshold can also be set through setStddevMulThresh.

The following piece of code handles the filtering and sets the cloud_filtered point cloud with our new noiseless cloud:

pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
statFilter.setInputCloud(cloud.makeShared());
statFilter.setMeanK(10);
statFilter.setStddevMulThresh(0.2);
statFilter.filter(cloud_filtered);

Finally, and as always, the filtered cloud is converted to PointCloud2 and published so that our other algorithms can make use of this new point cloud to provide more accurate results:

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

In the following screenshot, we can see the result of the previous code when it is applied on the point cloud provided in our test PCD file. The original point cloud can be seen on the left-hand side and the filtered one on the right-hand side. The results are not perfect, but we can observe how much of the noise has been removed, which means that we can now proceed with reducing the density of the filtered point cloud.

Reducing the density of a point cloud, or any other dataset for that matter, is called downsampling. There are several techniques that can be used to downsample a point cloud, but some of them are more rigorous or provide better results than others.

In general, the goal of downsampling a point cloud is to improve the performance of our algorithms; for that reason, we need our downsampling algorithms to keep the basic properties and structure of our point cloud so that the end result of our algorithms doesn't change too much.

In the following example, we are going to demonstrate how to perform downsampling on point clouds with Voxel Grid Filter. In this case, the input point clouds are going to be the filtered ones from the previous example so that we can chain both examples together to produce better results in further algorithms. The example can be found in the source directory of the chapter10_tutorials package, and it's named pcl_downsampling.cpp:

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_filtered", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_downsampled", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);

        pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
        voxelSampler.setInputCloud(cloud.makeShared());
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
        voxelSampler.filter(cloud_downsampled);

        pcl::toROSMsg(cloud_downsampled, 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_downsampling");

    cloudHandler handler;

    ros::spin();

    return 0;
}

This example is exactly the same as the previous one, with the only differences being the topics subscribed and published, which, in this case, are pcl_filtered and pcl_downsampled, and the algorithms used to perform the filtering on the point cloud.

As said earlier, the algorithm used is Voxel Grid Filter, which partitions the point cloud into voxels, or more accurately a 3D grid, and replaces all of the points contained in each voxel with the centroid of that subcloud. The size of each voxel can be specified through setLeafSize and will determine the density of our point cloud:

pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
voxelSampler.setInputCloud(cloud.makeShared());
voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
voxelSampler.filter(cloud_downsampled);

The following image shows the results of both the filtered and downsampled images when compared to the original one. You can appreciate how the structure has been kept, the density reduced, and much of the noise completely eliminated.

To execute both examples, as always we start running roscore:

$ roscore

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

$ roscd chapter10_tutorials/data
$ rosrun chapter10_tutorials pcl_read

In the third terminal, we will run the filtering example, which will produce the pcl_filtered image for the downsampling example:

$ rosrun chapter10_tutorials pcl_filter

Finally, in the fourth terminal, we will run the downsampling example:

$ rosrun chapter10_tutorials pcl_downsampling

As always, the results can be seen on rviz, but in this case, the pcl_visualizer2 example provided in the package can also be used although you might need to tweak the subscribed topics.

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:

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:

Often times, when processing our point clouds, we might need to perform operations that require accessing a local region of a point cloud or manipulating the neighborhood of specific points. Since point clouds store data in a one-dimensional data structure, these kinds of operations are inherently complex. In order to solve this issue, PCL provides two spatial data structures, named the kd-tree and the octree, which can provide an alternative and more structured representation of any point cloud.

As the name suggests, an octree is basically a tree structure in which each node has eight children, and which can be used to partition the 3D space. In contrast, the kd-tree is a binary tree in which nodes represent k-dimensional points. Both data structures are very interesting, but, in this particular example, we are going to learn how to use the octree to search and retrieve all the points surrounding a specific point. The example can be found in the source directory of the chapter10_tutorials package, and it's named pcl_partitioning.cpp:

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/octree/octree.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_downsampled", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_partitioned", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_partitioned;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);

        float resolution = 128.0f;
        pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);

        octree.setInputCloud (cloud.makeShared());
        octree.addPointsFromInputCloud ();

        pcl::PointXYZ center_point;
        center_point.x = 0 ;
        center_point.y = 0.4;
        center_point.z = -1.4;

        float radius = 0.5;
        std::vector<int> radiusIdx;
        std::vector<float> radiusSQDist;
        if (octree.radiusSearch (center_point, radius, radiusIdx, radiusSQDist) > 0)
        {
            for (size_t i = 0; i < radiusIdx.size (); ++i)
            {
                cloud_partitioned.points.push_back (cloud.points[radiusIdx[i]]);
            }
        }

        pcl::toROSMsg(cloud_partitioned, output);
        output.header.frame_id = "odom";
        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_partitioning");

    cloudHandler handler;

    ros::spin();

    return 0;
}

As usual, this example uses the pcl_downsampled topic as an input source of point clouds and publishes the resulting partitioned point cloud to the pcl_partitioned topic. The handler function starts by converting the input point cloud to a PCL point cloud. The next step is to create an octree-searching algorithm, which requires passing a resolution value that will determine the size of the voxels at the lowest level of the tree and, consequently, other properties such as the tree's depth. The algorithm also requires to be given the point cloud to explicitly load the points:

float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);

octree.setInputCloud (cloud.makeShared());
octree.addPointsFromInputCloud ();

The next step is to define the center point of the partition; in this case, it has been handpicked to be close to the top of the point cloud:

pcl::PointXYZ center_point;
center_point.x = 0;
center_point.y = 0.4;
center_point.z = -1.4;

We can now perform a search in a radius around that specific point using the radiusSearch function from the octree search algorithm. This particular function is used to output arguments that return the indices of the points that fall in that radius and the squared distance from those points to the center point provided. With those indices, we can then create a new point cloud containing only the points belonging to the partition:

float radius = 0.5;
std::vector<int> radiusIdx;
std::vector<float> radiusSQDist;
if (octree.radiusSearch (center_point, radius, radiusIdx, radiusSQDist) > 0)
{
  for (size_t i = 0; i < radiusIdx.size (); ++i)
  {
    cloud_partitioned.points.push_back (cloud.points[radiusIdx[i]]);
  }
}

Finally, the point cloud is converted to the PointCloud2 message type and published in the output topic:

pcl::toROSMsg( cloud_partitioned, output );
output.header.frame_id = "odom";
pcl_pub.publish( output );

In order to run this example, we need to run the usual chain of nodes, starting with roscore:

$ roscore

In the second terminal, we can run the pcl_read example and a source of data:

$ roscd chapter10_tutorials/data
$ rosrun chapter10_tutorials pcl_read

In the third terminal, we will run the filtering example:

$ rosrun chapter10_tutorials pcl_filter

In the fourth terminal, we will run the downsampling example:

$ rosrun chapter10_tutorials pcl_downsampling

Finally, we will run this example:

$ rosrun chapter10_tutorials pcl_partitioning

In the following image, we can see the end result of the partitioning process. Since we handpicked the point to be close to the top of the point cloud, we managed to extract part of the cup and the table. This example only shows a tiny fraction of the potential of the octree data structure, but it's a good starting point to further your understanding.