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 VoxelGrid 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.