[人工智能]pcl-点云滤波

软件：Qt5.15.2 + pcl1.11

统计滤波器显示结果：白色为原始点云；彩色为处理后点云

前言：噪声点与离群点

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

一、点云滤波方式

点云滤波可以看作点云处理的第一步。对后续点云处理起到提速和优化等作用。
直通滤波器、体素滤波器（重心、中心方法）、统计滤波器、半径滤波器、条件滤波器、双边滤波器、中值滤波器、高斯滤波、均匀采样

二、点云滤波原理

• 直通滤波器
  对于在空间分布有一定空间特征的点云数据，比如使用线结构光扫描的方式采集点云，沿z向分布较广，但x,y向的分布处于有限范围内。此时可使用直 通滤波器，确定点云在x或y方向上的范围，可较快剪除离群点，达到第一步粗处理的目的。
  原理：在x、y或者z轴方向设置目标点云的范围，获取范围内或范围外的点云。
• 体素滤波器
  原理：PCL中对输入的点云创建一个三维体素栅格，然后在每个体素内用体素中所有点的重心近似显示体素中其他点，这样该体素内所有点就用一个重心点最终表示。
  用途：使用体素化网格实现下采样，减少点云中点数量。
  pcl::ApproximateVoxelGrid和pcl::VoxelGrid：
  两种区别主要在于第一种是用每个体素栅格的中心点来近似该体素内的点，提升了速度，但是也损失了原始点云的局部形态精细度。
• 统计滤波器
  原理：明显离群点的特征是在空间中分布稀疏，可以理解为：每个点都表达一定信息量，某个区域点越密集则可能信息量越大。噪声信息属于无用信息，信息量较小。所以离群点表达的信息可以忽略不计。考虑到离群点的特征，则可以定义某处点云小于某个密度，既点云无效。计算每个点到其最近的k个点平均距离，（假设得到的结果是一个高斯分布，其形状是由均值和标准差决定），那么平均距离在标准范围之外的点，可以被定义为离群点并从数据中去除。给定均值与方差，可剔除方差在3σ之外的点。
  用途：统计滤波器主要用于滤除点云中的离群点。
• 条件滤波
  原理：条件滤波器通过设定滤波条件进行滤波。条件滤波相比于直通滤波优点就是一次可以对多个轴的范围进行划定限制。

enum CompareOp {GT, GE, LT, LE, EQ}
LT：less than 小于
LE：less than or equal to 小于等于
EQ：equal to 等于
GE：greater than or equal to 大于等于
GT：greater than 大于

• 半径滤波器
  原理：半径滤波器与统计滤波器相比更加简单粗暴。以某点为中心画一个圆计算落在该圆中点的数量，当数量大于给定值时，则保留该点，数量小于给定值则剔除该点。此算法运行速度快，依序迭代留下的点一定是最密集的，但是圆的半径和圆内点的数目都需要人工指定。
• 双边滤波器
  双边滤波器
  双边滤波算法，是通过取邻近采样点的加权平均来修正当前采样点的位置，从而达到滤波效果；同时也会有选择地剔除部分于当前采样点“差异”较大的相邻采样点，从而达到保持原特征的目的。
  类 BilaternlFilter 是对双边滤波算法在点云上的实现 ， 该类的实现利用的并非XYZ 字段的数据进行，而是利用强度数据字段进行双边滤波算法的实现，所以在使用该类时点云的类型中字段必须有强度字段，否则无法进行双边滤波处理。
  时间复杂度较高，大的点云不适合；
  去噪效果效果需要根据实际点云情况，不是所有需要平滑和保留边缘的情况使用；
  快速双边滤波器
• 提出了一种新的双边滤波器的信号处理分析方法,对空间关系和强度进行下采样进行加速, 与典型双边滤波器相比，运行相同时间该方法更加准确
  在信号处理框架解释了双边滤波器：证明在高维空间下，双边滤波器可以表示为简单的非线性卷积
  证明在高维空间下，下采样对于卷积结果影响不大：这种近似技术可以在控制误差的同时使速度提高几个数量级。
  使用FSNR（signal-to-noise tatio） 方法快速递进求解
  注意：需要pcl数据为结构点云，点云类似于图像（点云宽高与位置相关），即point_cloud_ptr->height>1.不支持PointXYZI点云格式

7. 中值滤波器
   pcl头文件中的解释：中值滤波器是最简单、应用最广泛的图像处理滤波器之一。众所周知，它可以很好地处理“射击”/脉冲噪声(一些像素有极端值)，它不会降低函数中各个步骤的对比度(与基于平均的过滤器相比)，并且对异常值具有鲁棒性。此外，它实现简单且高效，因为它只需要对图像进行一次遍历。它由一个固定大小的移动窗口组成，该窗口用窗口内的中间值替换中心像素。
   注意：这个算法只过滤organized_和未转换(即在相机坐标中)点云的深度(z分量)。如果给类实例一个无组织的云，将输出一个错误。
8. 高斯滤波
   对整个点云进行加权平均。每一个点的值，都由其本身和邻域内其他点值经过加权平均后得到(对领域线性卷积)。是线性平滑滤波，用于消除噪声。
   注意：需要有序点云。
9. 均匀采样
   对点云数据创建一个三维体素栅格，然后，在每个体素保留一个最接近体素中心的点，代替体素中所有点。
   均匀采样滤波基本上等同于体素滤波器，但是其不改变点的位置。下采样后，其点云分布基本均匀，但是其点云的准确度要好于体素滤波，因为没有移动点的位置。
   均匀采样算法：均匀采样通过构建指定半径的球体对点云进行下采样滤波，将每一个球内距离球体中心最近的点作为下采样之后的点输出。

三、代码

#pragma once
#include <pcl/filters/approximate_voxel_grid.h>      //体素中心点方法采样
#include <pcl/filters/bilateral.h>                   //双边滤波器
#include <pcl/filters/conditional_removal.h>         //条件滤波器头文件
#include <pcl/filters/convolution.h>                 //高斯滤波
#include <pcl/filters/convolution_3d.h>              //高斯滤波
#include <pcl/filters/fast_bilateral.h>              //快速双边滤波器
#include <pcl/filters/median_filter.h>               //中值滤波器
#include <pcl/filters/passthrough.h>                 //直通滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>      //半径滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/voxel_grid.h>                  //体素滤波器头文件
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/keypoints/uniform_sampling.h> //均匀采样
#include <pcl/pcl_base.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <QDebug>

//#include <pcl/filters/fast_bilateral_omp.h>
namespace filterUtil {

/**
 * brief:直通滤波器；
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, const QString& axis, float min = -0.1f,
    float max = 0.1f, bool outSide = false) {
    if (pointCloud->empty()) {
        qWarning().noquote() << PassThrough: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(pointCloud);
    pass.setFilterFieldName(axis.toStdString()); //指定沿着哪一个轴过滤点云
    pass.setFilterLimits(min, max);              //设置沿着轴过滤的范围
    pass.setNegative(outSide); //指定返回的是范围内的点云（false）or外面（true）
    pass.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:体素滤波器：重心方法
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelGrid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud,
                                              float sampleStep = 0.01f) {
    if (pointCloud->empty()) {
        qWarning().noquote() << VoxelGrid: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    pcl::VoxelGrid<pcl::PointXYZ> voxelGrid;
    voxelGrid.setInputCloud(pointCloud);
    voxelGrid.setLeafSize(sampleStep, sampleStep, sampleStep);
    voxelGrid.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:体素滤波器；中心方法,速度较快，但是损失了原始点云的局部形态精细度
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr approximateGrid(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, float sampleStep = 0.01f) {
    if (pointCloud->empty()) {
        qWarning().noquote() << ApproximateVoxelGrid: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximaterGrid;
    approximaterGrid.setInputCloud(pointCloud);
    approximaterGrid.setLeafSize(sampleStep, sampleStep, sampleStep);
    approximaterGrid.filter(*filterPointCloud);

    return filterPointCloud;
}

/*
 * brief:统计滤波器；
 * 说明：设置标准偏差乘数为1.0，意味着1个标准差以上就是离群点
 * 即那些距离大于(平均距离+或者-一个标准偏差)的点将被标记为离群值并被删除。
 * 根据高斯分布的数学表达，均值的一个标准差之内的分布可以达到总体的95%。
 * 注意:这里的平均距离和标准差、方差都可以由输入的点云数据集计算出来。
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr statisticalOutlier(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, int meanK = 50, double stdDev = 1.0) {
    if (pointCloud->empty()) {
        qWarning().noquote() << StatisticalOutlierRemoval: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }
    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(pointCloud);
    sor.setNegative(false);         // false:过滤掉噪声；true：保留噪声，去除其他点
    sor.setMeanK(meanK);            //设置KNN的k值
    sor.setStddevMulThresh(stdDev); //设置标准偏差乘数为1.0
    sor.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:条件滤波器；
 * mins\maxs: need three digital
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr conditionalRemoval(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, const std::vector<double> mins,
    const std::vector<double> maxs) {
    if (pointCloud->empty()) {
        qWarning().noquote() << ConditionalRemoval: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    if (mins.size() != maxs.size() && mins.size() != 3) {
        qWarning().noquote() << ConditionalRemoval: The input mins and maxs are not equal.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }
    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    // LT：less than 小于
    // LE：less than or equal to 小于等于
    // EQ：equal to 等于
    // GE：greater than or equal to 大于等于
    // GT：greater than 大于
    typename pcl::ConditionAnd<pcl::PointXYZ>::Ptr rangeCond(
        new pcl::ConditionAnd<pcl::PointXYZ>());

    rangeCond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
        new pcl::FieldComparison<pcl::PointXYZ>(x, pcl::ComparisonOps::GT, mins[0])));
    rangeCond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
        new pcl::FieldComparison<pcl::PointXYZ>(x, pcl::ComparisonOps::LT, maxs[0])));

    rangeCond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
        new pcl::FieldComparison<pcl::PointXYZ>(y, pcl::ComparisonOps::GT, mins[1])));
    rangeCond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
        new pcl::FieldComparison<pcl::PointXYZ>(y, pcl::ComparisonOps::LT, maxs[1])));

    rangeCond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
        new pcl::FieldComparison<pcl::PointXYZ>(z, pcl::ComparisonOps::GT, mins[2])));
    rangeCond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
        new pcl::FieldComparison<pcl::PointXYZ>(z, pcl::ComparisonOps::LT, maxs[2])));

    pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
    condrem.setCondition(rangeCond);
    condrem.setInputCloud(pointCloud);
    condrem.setKeepOrganized(true);
    condrem.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:半径滤波器；
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr radiusOutlierRemoval(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, double radius = 0.8, int minPts = 10) {
    if (pointCloud->empty()) {
        qWarning().noquote() << RadiusOutlierRemoval: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }
    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    outrem.setInputCloud(pointCloud);
    outrem.setRadiusSearch(radius);
    outrem.setMinNeighborsInRadius(minPts);
    outrem.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:双边滤波器；
 * pointCloud:支持的格式pcl::PointXYZI
 */
pcl::PointCloud<pcl::PointXYZI>::Ptr bilateraFilter(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr& pointCloud, double sigmaS = 1.0,
    double sigmaR = 0.01) {
    if (pointCloud->empty()) {
        qWarning().noquote() << BilateraFilter: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
    }
    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();

    typename pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(
        new pcl::search::KdTree<pcl::PointXYZI>); //用一个子类作为形参传入

    pcl::BilateralFilter<pcl::PointXYZI> bf;
    bf.setInputCloud(pointCloud);
    bf.setSearchMethod(tree);
    bf.setHalfSize(sigmaS); //设置高斯双边滤波器窗口的一半大小。
    bf.setStdDev(sigmaR);   //设置标准偏差参数
    bf.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:快速双边滤波器；
 * pointCloud：输入的点云需要是有序点云,不支持PointXYZI
 */
pcl::PointCloud<pcl::PointXYZRGB>::Ptr fastBilateraFilter(
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& pointCloud, double sigmaS = 1.0,
    double sigmaR = 0.01) {
    if (pointCloud->empty()) {
        qWarning().noquote() << FastBilateraFilter: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    }

    if (pointCloud->isOrganized()) {
        qWarning().noquote() << FastBilateraFilter: The input point cloud is not origanized.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    }

    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();

    //    typename pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
    //        new pcl::search::KdTree<pcl::PointXYZ>); //用一个子类作为形参传入

    pcl::FastBilateralFilter<pcl::PointXYZRGB> fbf;
    fbf.setInputCloud(pointCloud);
    // fbf.setSearchMethod(tree);
    fbf.setSigmaR(sigmaS); //设置高斯双边滤波器窗口的一半大小。
    fbf.setSigmaS(sigmaR); //设置标准偏差参数
    fbf.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:中值滤波器；
 * pointCloud：输入的点云需要是有序点云,不支持PointXYZI
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr medianFilter(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, int windowSize,
    float maxAllowedMovement) {
    if (pointCloud->empty()) {
        qWarning().noquote() << FastBilateraFilter: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    if (pointCloud->isOrganized()) {
        qWarning().noquote() << FastBilateraFilter: The input point cloud is not origanized.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    pcl::MedianFilter<pcl::PointXYZ> media;
    media.setInputCloud(pointCloud);
    media.setWindowSize(windowSize);
    media.setMaxAllowedMovement(maxAllowedMovement);
    media.filter(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:高斯滤波器1；
 * pointCloud：输入的点云需要是有序点云,不支持PointXYZI
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr convolution3DFilter(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, float sigma = 4.0f,
    float sigmaCoefficient = 4.0f, double radius = 0.2) {
    if (pointCloud->empty()) {
        qWarning().noquote() << Convolution3D: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    if (pointCloud->isOrganized()) {
        qWarning().noquote() << Convolution3D: The input point cloud is not origanized.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    // Set up the Gaussian Kernel
    auto kernel = std::make_shared<pcl::filters::GaussianKernel<pcl::PointXYZ, pcl::PointXYZ>>();
    kernel->setSigma(sigma);
    kernel->setThresholdRelativeToSigma(sigmaCoefficient);

    // Set up the KDTree
    auto kdtree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
    kdtree->setInputCloud(pointCloud);

    // Set up the Convolution Filter
    pcl::filters::Convolution3D<pcl::PointXYZ, pcl::PointXYZ,
                                pcl::filters::GaussianKernel<pcl::PointXYZ, pcl::PointXYZ>>
        convolution;
    convolution.setKernel(*kernel);
    convolution.setInputCloud(pointCloud);
    convolution.setSearchMethod(kdtree);
    convolution.setRadiusSearch(radius);
    convolution.setNumberOfThreads(10); // important! Set Thread number for openMP
    convolution.convolve(*filterPointCloud);

    return filterPointCloud;
}

/**
 * brief:均匀采样；
 * pointCloud：输入的点云需要是有序点云,不支持PointXYZI
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr uniformSampling(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, float radius = 0.05f) {
    if (pointCloud->empty()) {
        qWarning().noquote() << UniformSampling: The input point cloud is empty.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    if (pointCloud->isOrganized()) {
        qWarning().noquote() << UniformSampling: The input point cloud is not origanized.;
        return std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    }

    auto filterPointCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    pcl::UniformSampling<pcl::PointXYZ> us; //创建滤波器对象
    us.setInputCloud(pointCloud);           //设置待滤波点云
    us.setRadiusSearch(radius);             //设置滤波球体半径
    us.filter(*filterPointCloud);           //执行滤波，保存滤波结果于cloud_filtered

    return filterPointCloud;
}
} // namespace filterUtil

欢迎交流…