PLC点云滤波

Posted 2023-02-14 QtHalcon

   在获取点云数据时，由于设备精度、操作者经验、环境因素等带来的影响，以及电磁波衍射特性、被测物体表面性质变化和数据拼接配准操作过程的影响，点云数据中将不可避免地出现一些噪声点。实际应用中除了这些测量随机误差产生的噪声点之外，由于受到外界干扰如视线遮挡、障碍物等因素的影响，点云数据中往往存在着一些离主体点云较远的离散点，即离群点。不同的获取设备点云噪声结构也有不同。

    通过滤波完成的功能还包括孔洞修复、最小信息损失的海量点云数据压缩处理等 。在点云处理流程中滤波处理作为预处理的第一步，往往对后续处理流程影响很大，只有在滤波预处理中将噪声点、离群点、孔洞、数据压缩等 按照后续需求处理，才能够更好地进行配准、特征提取、曲面重建、可视化等后续流程。

    PCL 中点云滤波模块提供了很多灵活实用的滤波处理算法，例如双边滤波、高斯滤波、条件滤波、直通滤波、基于随机采样一致性滤波RANSAC等。

1 直通滤波

    直通滤波就是，用户设置一个范围，直通滤波可以取出范围之内或之外的点。

    下面是一个示例：

 

#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>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filteredNe(new pcl::PointCloud<pcl::PointXYZ>);

    // 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 = 2 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 2 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 2 * rand() / (RAND_MAX + 1.0f);
//        std::cerr<< cloud->points[i].x<<" "<< cloud->points[i].y<<" "<< cloud->points[i].z<< std::endl;
    
    std::cerr << "Cloud before filtering: " << cloud->points.size()<< 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. 设置过滤范围，以Z方向数据未过滤范围
    pass.filter(*cloud_filtered);       // 4. 执行过滤，并将结果输出到cloud_filtered
    std::cerr << "Cloud after filtering: " << cloud_filtered->points.size()<< std::endl;

    // Create the filtering object
    pcl::PassThrough<pcl::PointXYZ> passne;
    passne.setInputCloud(cloud);          // 1. 设置输入源
    passne.setFilterFieldName("z");       // 2. 设置过滤域名
    passne.setFilterLimits(0.0, 1.0);     // 3. 设置过滤范围
    passne.setNegative(true);             // 设置后得到的点是
    passne.filter(*cloud_filteredNe);       // 4. 执行过滤，并将结果输出到cloud_filtered
    std::cerr << "Cloud after filteringNE: " << cloud_filteredNe->points.size()<< std::endl;

    //
    pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");//对应组
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_filtered, 255, 0, 0);
    viewer.addPointCloud<pcl::PointXYZ>(cloud_filtered, single_color, "scene_cloud");//过滤掉的点
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_cloud");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_colorne(cloud_filtered, 0, 255, 0);
    viewer.addPointCloud<pcl::PointXYZ>(cloud_filteredNe, single_colorne, "scene_cloudne");//剩下的点
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_cloudne");

    //
    viewer.setBackgroundColor(105.0/255, 105.0/255, 105.0/255);
    while (!viewer.wasStopped()) 
        viewer.spinOnce ();
    

    结果如下，红色表示滤掉的点，绿色表示滤掉后剩下的点

 2 体素滤波

    点云中经常会用到的一个概念是体素（Voxel）。

    介绍体素之前先介绍一下图像像素。图像一般是按行列的像素存储的，也就是通常说的图像分辨率。对于WxH大小的图片，相当于把图片划分成如下的网格，然后每个网格里的颜色或者灰度值不同。这样就称为了一幅图像

    对应到三维空间，点云是由点组成的，通常每个点由x、y、z三个坐标组成。三个坐标对应二维图像中的灰度值。类比与二维图像，三维空间有以下几个区别：

• 二维图像的网格是均匀分布的，而三维点云分布在三维空间，同时是非均匀分布的。

• 二维图像相邻像素的位置坐标相差一个1单位像素，而三维点云中相邻点的间距并不是一个固定值。

    为了让三维点云也具备和二维图像一样的均匀分布，体素将整个三维空间分成一个个立体的方格。体素可以让整个三维空间分割成一个均匀分布的空间。我们可以很容易看到：

• 如果将体素的立方体长宽高设置成最小的长度单位，这样可以让每个点都落在一个体素当中。但是这样做会导致很多体素里面并不存在一个点，并且数据量会非常大。另一方面，点云的分布没有改变，仍然是不均匀的。

• 用一个相对大的立方体作为体素单位，尽可能使所有的体素中都有点落在里面。这样会出现一个体素里面有多个点。如果每个体素都用一个点来代表这个体素中点云的特征，那么整个点云会被下采样成均匀分布的形式。显然，体素是一个常用的点云下采样方法。

    通过体素网格实现降采样，可以减少点数量的同时，保证点云的形状特征，可以提高配准、曲面重建、形状识别等算法的速度，并保证准确性。

    下面是一个示例：

    

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
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>);

    // 从文件读取点云图
    // Fill in the cloud data
    pcl::PCDReader reader;
    // Replace the path below with the path where you saved your file
    reader.read ("./table_scene_lms400.pcd", *cloud); // Remember to download the file first!

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

    // 创建一个长宽高分别是1cm的体素过滤器，cloud作为输入数据，cloud_filtered作为输出数据
    float leftSize = 0.01f;
    // Create the filtering object
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud);
    sor.setLeafSize (leftSize, leftSize, leftSize);
    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 ("./table_scene_lms400_downsampled.pcd", *cloud_filtered);

    //
    pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");//对应组
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud, "z");//按照z字段进行渲染
    viewer.addPointCloud<pcl::PointXYZ>(cloud, fildColor, "sample");//显示点云，其中fildColor为颜色显示
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample");//设置点云大小

    //
    viewer.setBackgroundColor(105.0/255, 105.0/255, 105.0/255);
    while (!viewer.wasStopped())
    
        viewer.spinOnce();
    

    对比图如下：

3 离群点移除

    激光扫描通常会生成不同点密度的点云数据集。此外，测量误差会导致稀疏的异常值，从而进一步破坏结果。这会使局部点云特征（例如表面法线或曲率变化）的估计复杂化，从而导致错误的值，进而可能导致点云配准失败。通过对每个点的邻域进行统计分析，并对不符合特定条件的部分进行修整，可以解决其中一些不规则现象。

    稀疏离群值的消除基于输入数据集中点到邻居距离的分布的计算。对于每个点，我们计算从它到所有相邻点的平均距离。通过假设结果分布是具有均值和标准差的高斯分布，可以将其平均距离在由全局距离均值和标准差定义的区间之外的所有点视为离群值并从数据集中进行修剪。下图显示了稀疏离群值分析和删除的效果：原始数据集显示在左侧，结果数据集显示在右侧。数据集图显示了滤波前后每个点的邻域中平均K最近邻距离。

    实现步骤：

    查找每一个点的所有邻域点

    计算每个点到其邻居的距离dijdij，其中i=[1,...,m]i=[1,...,m]表示共m个点，j=[1,...,k]j=[1,...,k]每个点有k个邻居

    根据高斯分布d∼N(μ,σ)d∼N(μ,σ)模型化距离参数，计算所有点与邻居的μμ（距离的均值），σσ（距离的标准差），如下：

    为每一个点，计算其与邻居的距离均值∑kj=1dij

    遍历所有点，如果其距离的均值大于高斯分布的指定置信度，则移除，比如

∑kj=1dij>μ+3σ∑j=1kdij>μ+3σ or ∑kj=1dij<μ−3σ

    下面是一个示例：

 

#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::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filteredne (new pcl::PointCloud<pcl::PointXYZ>);

    // 从文件读取点云
    // Fill in the cloud data
    pcl::PCDReader reader;
    // Replace the path below with the path where you saved your file
    reader.read<pcl::PointXYZ> ("./table_scene_lms400.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::PointXYZ> sor;
    sor.setInputCloud (cloud);
    // 设置平均距离估计的最近邻居的数量K
    sor.setMeanK (50);
    // 设置标准差阈值系数
    sor.setStddevMulThresh (1.0);
    // 执行过滤
    sor.filter (*cloud_filtered);
    sor.setNegative (true);
    sor.filter (*cloud_filteredne);

    //
    pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");//对应组
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_filtered, 255, 0, 0);
    viewer.addPointCloud<pcl::PointXYZ>(cloud_filtered, single_color, "scene_cloud");//过滤掉的点
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "scene_cloud");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_colorne(cloud_filteredne, 0, 255, 0);
    viewer.addPointCloud<pcl::PointXYZ>(cloud_filteredne, single_colorne, "scene_cloudne");//剩下的点
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "scene_cloudne");

    //
    viewer.setBackgroundColor(105.0/255, 105.0/255, 105.0/255);
    while (!viewer.wasStopped()) 
        viewer.spinOnce ();
    
    return (0);

 结果如下，绿色的点就是被滤掉的点

4 条件滤波

    条件滤波是设置不同维度滤波规则进行滤波，如半径离群值滤波。

    半径离群值滤波是用户指定邻居的个数，要每个点必须在指定半径内具有指定个邻居才能保留在PointCloud中。例如，如果指定了1个邻居，则只会从PointCloud中删除黄点。如果指定了2个邻居，则黄色和绿色的点都将从PointCloud中删除。

    下面是一个示例：

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

typedef pcl::PointXYZ PointType;
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>);

    // 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 = 2 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 2 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 2 * rand() / (RAND_MAX + 1.0f);
    

    int argt=1;
    if (argt == 0) 
        pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
        // build the filter
        outrem.setInputCloud(cloud);
        outrem.setRadiusSearch(0.4);
        outrem.setMinNeighborsInRadius(2);
        // apply filter
        outrem.filter(*cloud_filtered);
     else if (argt == 1) 
        // build the condition
        pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
                new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
                new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8)));
        // build the filter
        pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
        condrem.setCondition(range_cond);
        condrem.setInputCloud(cloud);
        condrem.setKeepOrganized(true);
        // apply filter
        condrem.filter(*cloud_filtered);
    
    std::cerr << "Cloud before filtering: " << cloud->points.size()<<std::endl;
    std::cerr << "Cloud after filtering: " << cloud_filtered->points.size()<< std::endl;

    //
    pcl::visualization::PCLVisualizer viewer ("3D Viewer");
    pcl::visualization::PointCloudColorHandlerCustom<PointType> single_color(cloud, 0, 255, 0);
    viewer.addPointCloud<PointType>(cloud, single_color, "sample cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");

    pcl::visualization::PointCloudColorHandlerCustom<PointType> single_color2(cloud_filtered, 255, 0, 0);
    viewer.addPointCloud<PointType>(cloud_filtered, single_color2, "sample cloud 2");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud 2");

    viewer.setBackgroundColor(105.0/255, 105.0/255, 105.0/255);
    while (!viewer.wasStopped()) 
        viewer.spinOnce();
    
    return (0);

  条件滤波结果如下，绿色是被滤掉的点，红色是剩下的点

    半径离群值滤波结果如下，绿色是被滤掉的点，红色是剩下的点