高斯滤波实现算法

原理

高斯滤波 (Gaussian Filter) 是一种基于高斯分布的平滑算法，通过对局部点云进行多项式拟合来实现平滑效果。

PCL中没有专门的高斯滤波的库，需要通过移动最小二乘法算法来实现处理。该方法的核心思想是：在计算域内的任意一点处，通过局部范围内的节点数据，构造带权重的最小二乘泛函，通过最小化该泛函来得到该点处的函数近似值，并且近似函数具有连续可导的特性。

上一篇文章介绍了移动最小二乘法的计算过程，感兴趣的朋友可以看一下->移动最小二乘法

如何用最小二乘函数进行点云过滤？

计算得到近似函数 u h ( x ) u^h(x) uh(x) 后，实际过滤过程如下：

步骤1：对每个点计算新的位置

对于点云中的每个点 x i x_i xi：

找到其邻域内的所有点 { x 1 , x 2 , ... , x n } \{x_1, x_2, \dots, x_n\} {x1,x2,...,xn}
用这些点计算该位置的近似函数 u h ( x i ) u^h(x_i) uh(xi)
用 u h ( x i ) u^h(x_i) uh(xi) 的值作为该点滤波后的新位置

数学表达：

滤波后的点 = u h ( x i ) = N ( x i ) u \text{滤波后的点} = u^h(x_i) = {N}(x_i) {u} 滤波后的点=uh(xi)=N(xi)u

其中：

N ( x i ) {N}(x_i) N(xi) 是形函数在 x i x_i xi 处的值
${u}$ 是原始邻域点的坐标向量

步骤2：滤波效果分析

平滑效果 ： u h ( x ) u^h(x) uh(x) 是多项式函数，比原始离散点更平滑
噪声去除：高频噪声被多项式拟合"平均"掉了
保持形状：局部多项式拟合保持了原始表面的整体形状

步骤3：实际过滤公式

对于3D点云 ( x , y , z ) (x, y, z) (x,y,z)，需要对每个坐标分量分别处理：

x new = u x h ( x i ) x_{\text{new}} = u^h_x(x_i) xnew=uxh(xi) （x坐标的近似函数）
y new = u y h ( x i ) y_{\text{new}} = u^h_y(x_i) ynew=uyh(xi) （y坐标的近似函数）
z new = u z h ( x i ) z_{\text{new}} = u^h_z(x_i) znew=uzh(xi) （z坐标的近似函数）

关键：移动最小二乘法的"移动"特性

对每个点 x i x_i xi 都要重新计算一次近似函数 u h ( x i ) u^h(x_i) uh(xi)
权函数 w ( x − x I ) w(x-x_I) w(x−xI) 的中心随着当前点 x i x_i xi 移动
这就是"移动"最小二乘法的含义

主要特点：

基于局部多项式拟合实现平滑
有效去除高频噪声
保持点云的整体形状
需要计算法线信息

关键参数：

搜索半径：拟合多项式时考虑的邻域范围
多项式阶数：拟合多项式的阶数
上采样参数：控制重采样的密度

实现算法

移动最小二乘法(MLS)实现

在PCL中，MLS算法内部自动完成了以下步骤：

对每个点：找到搜索半径内的邻域点
计算权重：根据距离计算每个邻域点的权重
拟合多项式：用加权最小二乘法拟合局部多项式曲面
投影到曲面：将原始点投影到拟合的曲面上，得到平滑后的点

cpp 复制代码

// MLS平滑（核心过滤过程）
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
mls.setInputCloud(cloud);
mls.setPolynomialOrder(2);           // 使用二次多项式拟合局部曲面
mls.setSearchMethod(tree);
mls.setSearchRadius(0.03);           // 搜索半径，决定平滑程度
mls.process(*mls_points);

参数说明

cpp 复制代码

// 多项式阶数：控制拟合精度
mls.setPolynomialOrder(2);               // 二次多项式拟合

// 搜索半径：控制平滑程度
mls.setSearchRadius(0.03);               // 搜索半径3cm

// 上采样设置：在平滑后的曲面上生成新点，增加点云密度
// 注意：启用上采样会增加输出点数
mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::SAMPLE_LOCAL_PLANE);
mls.setUpsamplingRadius(0.005);              // 上采样搜索半径
mls.setUpsamplingStepSize(0.003);            // 上采样步长

完整代码

cpp 复制代码

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>

int main() {
    // 1. 加载点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("02.pcd", *cloud) == -1) {
        std::cerr << "错误：无法加载点云文件02.pcd" << std::endl;
        return -1;
    }
    
    std::cout << "加载点云成功，文件名: 02.pcd" << std::endl;
    
    // 2. 创建KD树用于搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    
    // 3. 执行MLS平滑（高斯滤波的核心实现）
    pcl::PointCloud<pcl::PointNormal>::Ptr mls_points(new pcl::PointCloud<pcl::PointNormal>);
    
    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
    mls.setInputCloud(cloud);                    // 设置输入点云
    mls.setPolynomialOrder(2);                   // 二次多项式拟合，平衡精度和速度
    mls.setSearchMethod(tree);                   // 使用KD树加速邻域搜索
    mls.setSearchRadius(0.03);                   // 搜索半径3cm，控制平滑程度
    
    // 上采样设置：在平滑后的曲面上生成新点，增加点云密度
    // 注意：启用上采样会增加输出点数
    // mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::SAMPLE_LOCAL_PLANE);
    // mls.setUpsamplingRadius(0.005);              // 上采样搜索半径
    // mls.setUpsamplingStepSize(0.003);            // 上采样步长
    
    mls.process(*mls_points);  // 执行MLS处理，得到平滑后的点云（含法线信息）
    
    // 4. 转换回PointXYZ类型（去除法线信息，便于可视化）
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
    filtered->points.resize(mls_points->size());
    for (size_t i = 0; i < mls_points->size(); ++i) {
        filtered->points[i].x = mls_points->points[i].x;
        filtered->points[i].y = mls_points->points[i].y;
        filtered->points[i].z = mls_points->points[i].z;
    }
    filtered->width = filtered->points.size();
    filtered->height = 1;
    
    // 输出处理结果，对比点数变化
    std::cout << "原始点云点数: " << cloud->points.size() << std::endl;
    std::cout << "高斯滤波(MLS)后点数: " << filtered->points.size() << std::endl;
    std::cout << "参数: 多项式阶数=2, 搜索半径=0.03m" << std::endl;
    
    // 5. 可视化对比原始点云和滤波结果
    pcl::visualization::PCLVisualizer viewer("MLS高斯滤波效果对比");
    
    // 原始点云：红色，半透明，小点
    viewer.addPointCloud<pcl::PointXYZ>(cloud, "original");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "original");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "original");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "original");
    
    // 滤波后点云：白色，不透明，大点
    viewer.addPointCloud<pcl::PointXYZ>(filtered, "filtered");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 1.0, "filtered");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "filtered");
    
    // 添加图例说明
    viewer.addText("红色半透明: 原始点云 (有噪声)", 10, 70, 12, 1.0, 1.0, 1.0, "original_text");
    viewer.addText("白色大点: MLS滤波后 (平滑)", 10, 50, 12, 1.0, 1.0, 1.0, "filtered_text");
    
    // 6. 运行可视化窗口
    std::cout << "打开可视化窗口，按q退出..." << std::endl;
    while (!viewer.wasStopped()) {
        viewer.spinOnce(100);  // 每100ms更新一次
    }
    
    // 7. 保存滤波结果
    pcl::io::savePCDFileASCII("filtered.pcd", *filtered);
    std::cout << "滤波结果已保存到: filtered.pcd" << std::endl;
    
    return 0;
}

运行结果

点有点多，用这种方法来过滤需要较长时间。