【PCL】——point cloud filtering

When acquiring point cloud data, due to the influence of equipment accuracy, operator experience and environmental factors, as well as the diffraction characteristics of electromagnetic waves, changes in the surface properties of the measured object, and the influence of data splicing and registration operations, point cloud data will inevitably some noise occurs.

Several situations that require point cloud filtering are as follows:

• Point cloud data density is irregular and needs to be smoothed;
• Outliers need to be removed due to problems such as occlusion;
• A large amount of data needs to be down-sampled;
• Noise data needs to be removed.

PCL conventional filtering methods are well packaged, and the filtering of point clouds is completed by calling each filter object. The main filters are pass-through filter , voxel filter , statistical filter , radius filter and so on.

Common methods are as follows:

• Filter the point cloud using a pass-through filter
• Downsampling Point Clouds Using Voxel Filtering
• Remove outliers using statistical filters
• Remove outliers using conditional filtering or radius filtering

pass filter

The pass-through filter is to set the range on the attribute of the point according to the attribute of the point cloud (attributes such as x, y, z, color value, etc.), filter the point, keep the range or keep outside the range.

Official website tutorial - https://pcl.readthedocs.io/projects/tutorials/en/latest/passthrough.html#passthrough
test case 1 (retain the point cloud within 0-1m within the z-axis range)

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>

typedef pcl::PointXYZ PointT;

int
main(int argc, char **argv) {
    
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

    //写入点云
    cloud->width = 5;
    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);
    }

    std::cerr << "Cloud before filtering: " << std::endl;
    for (size_t i = 0; i < cloud->points.size(); ++i)
        std::cerr << "    " << cloud->points[i].x << " "
                  << cloud->points[i].y << " "
                  << cloud->points[i].z << std::endl;

    // Create the filtering object
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);          // 1. 设置输入源
    pass.setFilterFieldName("z");       // 2. 设置过滤域名
    pass.setFilterLimits(0.0, 1.0);     // 3. 设置过滤范围
//    pass.setFilterLimitsNegative(true); // 设置获取Limits之外的内容
    pass.filter(*cloud_filtered);       // 4. 执行过滤，并将结果输出到cloud_filtered

    std::cerr << "Cloud after filtering: " << std::endl;
    for (size_t i = 0; i < cloud_filtered->points.size(); ++i)
        std::cerr << "    " << cloud_filtered->points[i].x << " "
                  << cloud_filtered->points[i].y << " "
                  << cloud_filtered->points[i].z << std::endl;

    pcl::visualization::CloudViewer viewer("Cloud Viewer");

    //这里会一直阻塞直到点云被渲染
    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {
    
    
    }
    return (0);
}

After executing the program, there will be the following results

Cloud before filtering:
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762
Cloud after filtering:
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304

A graphical display of the filtering process is shown below, where red is (x), green is (y), and blue is (z) in the coordinate system. Among the five points in the figure, the green points are the filtered points, and the red points are the filtered points.

Test case 2

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>

int
main(int argc, char **argv) {
    
    
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);

    // 加载pcd文件到cloud
    if (pcl::io::loadPCDFile("../data/result.pcd", *cloud) == -1)
    {
    
    
        std::cout << "Could not load pcd file!" << std::endl;
        return -1;
    }

    // Create the filtering object
    pcl::PassThrough<pcl::PointXYZI> pass;
    pass.setInputCloud(cloud);          // 1. 设置输入源
    pass.setFilterFieldName("z");       // 2. 设置过滤域名
    pass.setFilterLimits(-1.0, 1.0);     // 3. 设置过滤范围
    // pass.setFilterLimitsNegative(true); // 设置获取Limits之外的内容
    pass.filter(*cloud_filtered);       // 4. 执行过滤，并将结果输出到cloud_filtered

    pcl::visualization::PCLVisualizer viewer_filter("Cloud-Filtered Viewer");
    //背景颜色设置
    viewer_filter.setBackgroundColor(0, 0, 0);
    //按照z字段进行渲染
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor_filter(cloud_filtered, "intensity");
    //显示点云，其中fildColor为颜色显示
    viewer_filter.addPointCloud<pcl::PointXYZI>(cloud_filtered, fildColor_filter, "sample");
    //设置点云大小
    viewer_filter.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample");
    // // 循环判断是否退出
    // while (!viewer_filter.wasStopped()) {
    
    
    //     viewer_filter.spinOnce();
    // }

    pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
    //背景颜色设置
    viewer.setBackgroundColor(0, 0, 0);
    //按照z字段进行渲染
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor(cloud, "intensity");
    //显示点云，其中fildColor为颜色显示
    viewer.addPointCloud<pcl::PointXYZI>(cloud, fildColor, "sample");
    //设置点云大小
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample");
    
    // 循环判断是否退出
    while (!viewer.wasStopped() && !viewer_filter.wasStopped()) {
    
    
        viewer_filter.spinOnce();
        viewer.spinOnce();
    }
    return 0;
}

Test effect

downsampling

Voxel filtering, that is, reducing the number of points and point cloud data while maintaining the shape characteristics of the point cloud, is very practical in improving the speed of algorithms such as registration, surface reconstruction, and shape recognition.

The VoxelGrid class implemented by PCL creates a 3D voxel grid through the input point cloud data (the voxel grid can be imagined as a collection of tiny spatial 3D cubes), and then within each voxel (ie 3D cube), use volume The barycenter of all points in the voxel is used to approximate other points in the voxel , so that all points in the voxel are finally represented by a barycenter point, and the filtered point cloud is obtained after processing for all voxels.

Official tutorial - https://pcl-tutorials.readthedocs.io/en/latest/voxel_grid.html#voxelgrid

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/cloud_viewer.h>

int main (int argc, char** argv)
{
    
    
  pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

  // Fill in the cloud data
  pcl::PCDReader reader; // 也可以用创建reader的方式来读PCD文件
  // Replace the path below with the path where you saved your file
  reader.read ("../data/result.pcd", *cloud); // Remember to download the file first!

  std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
       << " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;

  // Create the filtering object
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (0.5f, 0.5f, 0.5f);
  sor.filter (*cloud_filtered);

  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;

  pcl::PCDWriter writer;
  writer.write ("../data/result_downsampled.pcd", *cloud_filtered, 
         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

  return (0);
}

PointCloud before filtering: 98322 data points (x y z intensity).
PointCloud after filtering: 11733 data points (x y z intensity).

pcl_viewer to view the effect, the number of point clouds is significantly reduced.

Use statistical filtering (statisticalOutlierRemoval) to remove outliers

Laser scanning usually produces point cloud data sets with uneven density. In addition, errors in measurement will also produce sparse outliers. Local point cloud feature calculations (such as normal vectors or curvature change rates at sampling points) are complex, which can easily lead to point Post-processing such as cloud registration failed.

Common solution ideas : perform a statistical analysis on the neighborhood of each point, and prune out some points that do not meet certain standards. Assuming that the result obtained is a Gaussian distribution whose shape is determined by the mean and standard deviation, points whose average distance is outside the standard range can be defined as outliers and removed from the data.

The steps of statistical filtering to remove outliers are as follows:

1. Find all neighbors of each point
2. Calculate the distance dij d_{ij} of each point to its neighbor points$d_{ij}$, where $i=[1,2,...,m]$ means a total of$m$ points,$j=[1,2,...,k]$ means a total of$k$ neighborhood points
3. According to Gaussian distribution $dN(\mu ,\sigma )$ to model the distance parameter, and calculate the μ \muof all points and neighboring points$\mu$ (mean of distances),$\sigma$ (standard deviation of the distance), as follows:$$m=\frac{1}{nk}\sum _{i=1}^m\sum _{j=1}^k{d}_{ij}$$$$\sigma =\sqrt{\frac{1}{nk}\sum _{i=1}^m\sum _{j=1}^k{(d_{ij}-m)}^2}$$At this step, the average distance between each point and its neighbor points is also calculated $\sum _{j=1}^k{d}_{ij}$
   Traverse all points, if the mean value of their distance is greater than the specified confidence of the Gaussian distribution, then remove, for example: $$\sum _{j=1}^k{d}_{ij}>m+3\sigma or\sum _{j=1}^k{d}_{ij}<m-3p$$

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

int main (int argc, char** argv)
{
    
    
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZI>);

    // 从文件读取点云
    // Fill in the cloud data
    pcl::PCDReader reader;
    // Replace the path below with the path where you saved your file
    reader.read<pcl::PointXYZI> ("../data/result.pcd", *cloud);

    std::cerr << "Cloud before filtering: " << std::endl;
    std::cerr << *cloud << std::endl;

    // 创建过滤器，每个点分析计算时考虑的最近邻居个数为50个；
    // 设置标准差阈值为1，这意味着所有距离查询点的平均距离的标准偏差均大于1个标准偏差的所有点都将被标记为离群值并删除。
    // 计算输出并将其存储在cloud_filtered中

    // Create the filtering object
    pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
    sor.setInputCloud (cloud);
    // 设置平均距离估计的最近邻居的数量K
    sor.setMeanK (50);
    // 设置标准差阈值系数
    sor.setStddevMulThresh (1.0);
    // 执行过滤
    sor.filter (*cloud_filtered);

    std::cerr << "Cloud after filtering: " << std::endl;
    std::cerr << *cloud_filtered << std::endl;
    // 将留下来的点保存到后缀为_inliers.pcd的文件
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZI> ("../data/result_inliers.pcd", *cloud_filtered, false);

    // 使用个相同的过滤器，但是对输出结果取反，则得到那些被过滤掉的点，保存到_outliers.pcd文件
    sor.setNegative (true);
    sor.filter (*cloud_filtered);
    writer.write<pcl::PointXYZI> ("../data/result_outliers.pcd", *cloud_filtered, false);

    return 0;
}

Execute the program and output the following results

Cloud before filtering: 
header: seq: 0 stamp: 0 frame_id: 

points[]: 98322
width: 98322
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

Cloud after filtering: 
header: seq: 0 stamp: 0 frame_id: 

points[]: 90637
width: 90637
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

View with pcl_viwer

Use conditional filtering (ConditionalRemoval) or radius filtering (RadiusOutlinerRemoval) to remove outliers

Conditional filter : filter by setting filter conditions, which is a bit like a piecewise function. When the point cloud is within a certain range, it will be kept, and if it is not, it will be discarded.

Radius filter : Draw a circle with a certain point as the center to calculate the number of points falling in the middle of the circle. When the number is greater than a given value, the point will be kept, and if the number is less than the given value, the point will be eliminated. This algorithm runs fast, and the points left by sequential iterations must be the densest, but the radius of the circle and the number of points in the circle need to be manually specified.

The image below helps visualize the RadiusOutlierRemoval filter object in action. The user specifies the number of neighbors, and each point must have the specified number of neighbors within the specified radius to be kept in PointCloud. For example, if 1 neighbor is specified, only yellow points will be removed from the PointCloud. If 2 neighbors are specified, both the yellow and green points will be removed from the PointCloud.

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/visualization/pcl_visualizer.h>

void showPointClouds(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud){
    
    
    pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
    //背景颜色设置
    viewer.setBackgroundColor(0, 0, 0);
    //按照z字段进行渲染
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor(cloud, "intensity");
    //显示点云，其中fildColor为颜色显示
    viewer.addPointCloud<pcl::PointXYZI>(cloud, fildColor, "sample");
    //设置点云大小
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample");

    while (!viewer.wasStopped())
    {
    
    
        viewer.spinOnce();
    }
    
}

int main(int argc, char **argv) {
    
    
    if (argc != 2)
    {
    
    
        //选择-r为半径滤波；选择-c为条件滤波
        std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
        exit(0);
    }
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);

    // 从文件读取点云
    // Fill in the cloud data
    pcl::PCDReader reader;
    // Replace the path below with the path where you saved your file
    reader.read<pcl::PointXYZI> ("../data/result.pcd", *cloud);

    if (strcmp(argv[1], "-r") == 0) 
    {
    
    
        //半径滤波
        pcl::RadiusOutlierRemoval<pcl::PointXYZI> outrem;
        // build the filter
        outrem.setInputCloud(cloud);
        outrem.setRadiusSearch(0.4); //设置半径滤波时圆的半径
        outrem.setMinNeighborsInRadius(2); //设置圆内的点数
        // apply filter
        outrem.filter(*cloud_filtered);

        //保存相应pcd文件,以方便后续查看
        pcl::PCDWriter writer;
        writer.write<pcl::PointXYZI>("../data/result_remove_outliers_r.pcd", *cloud_filtered, false);
    } 
    else if (strcmp(argv[1], "-c") == 0) 
    {
    
    
        //条件滤波
        // build the condition
        // 过滤条件:z轴方向上,大于0.0,小于8.0
        pcl::ConditionAnd<pcl::PointXYZI>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZI>());
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                new pcl::FieldComparison<pcl::PointXYZI>("z", pcl::ComparisonOps::GT, 0.0)));
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZI>::ConstPtr(
                new pcl::FieldComparison<pcl::PointXYZI>("z", pcl::ComparisonOps::LT, 0.8)));

        // build the filter
        pcl::ConditionalRemoval<pcl::PointXYZI> condrem;
        condrem.setCondition(range_cond);
        condrem.setInputCloud(cloud);
        condrem.setKeepOrganized(false);
        // apply filter
        condrem.filter(*cloud_filtered);

        //保存相应pcd文件,以方便后续查看
        pcl::PCDWriter writer;
        writer.write<pcl::PointXYZI>("../data/result_remove_outliers_c.pcd", *cloud_filtered, false);
    } 
    else 
    {
    
    
        std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
        exit(0);
    }


    std::cerr << "Cloud before filtering: " << std::endl;
    std::cerr << *cloud << std::endl;

    // display pointcloud after filtering
    std::cerr << "Cloud after filtering: " << std::endl;
    std::cerr << *cloud_filtered << std::endl;

    showPointClouds(cloud_filtered);
    return (0);
}

radius filter

Cloud before filtering: 
header: seq: 0 stamp: 0 frame_id: 

points[]: 98322
width: 98322
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

Cloud after filtering: 
header: seq: 0 stamp: 0 frame_id: 

points[]: 95967
width: 95967
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

conditional filter

Cloud before filtering: 
header: seq: 0 stamp: 0 frame_id: 

points[]: 98322
width: 98322
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

Cloud after filtering: 
header: seq: 0 stamp: 0 frame_id: 

points[]: 9904
width: 9904
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]