点云入门|KDTree 与平滑法线估计_平滑点云-优快云博客

本文链接：https://blog.youkuaiyun.com/qq_44691564/article/details/144248309

首先，放一个点云官网的教程的网址，本篇文章仍然是对计算机视觉life教程的学习和复盘。

点云滤波

点云滤波是处理点云数据的一种常见方法，用于去除噪声、提取特定信息或优化数据质量。有下列几种常见的滤波方法

统计滤波（Statistical Outlier Removal, SOR）
目标：移除孤立点和离群点（Outliers）。
原理：计算每个点的邻域内点的距离统计值（如均值、标准差），然后设定阈值过滤掉不符合统计特性的点。
优点：简单高效，适用于大多数点云场景。
缺点：需要手动设置参数，可能误删有效点。

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

int main() {
// Ptr 是 PCL 中常用的一个 智能指针类型，其实是 std::shared_ptr 的别名，用于管理 pcl::PointCloud<pcl::PointXYZ> 对象的生命周期。
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

    // 加载点云
    pcl::io::loadPCDFile("point_cloud.pcd", *cloud);

    // 统计滤波
    
    //创建一个 StatisticalOutlierRemoval 滤波器对象，模板类型为 pcl::PointXYZ
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud);
    sor.setMeanK(50);         // 邻域点数
    sor.setStddevMulThresh(1.0); // 标准差阈值
    sor.filter(*cloud_filtered);

    // 保存过滤后的点云
    pcl::io::savePCDFile("filtered_statistical.pcd", *cloud_filtered);

    return 0;
}

体素滤波（Voxel Grid Filter）
体素滤波将点云的空间划分为一系列大小相等的立方体网格（称为体素）。在每个体素中，通过某种策略（通常是计算点的质心或取平均值），用一个代表点替换该体素中的所有点。
目标：降低点云分辨率，简化数据。
原理：将点云划分为固定大小的三维网格（体素），然后用每个网格中的点的中心或平均值来代表该网格。
优点：减少数据量，提升后续处理效率。
缺点：会导致细节丢失。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

    // 将名为 point_cloud.pcd 的点云文件加载到 cloud 指向的点云对象中
    pcl::io::loadPCDFile("point_cloud.pcd", *cloud);

    // 体素滤波
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.1f, 0.1f, 0.1f); // 体素大小
    vg.filter(*cloud_filtered);

    // 保存过滤后的点云
    pcl::io::savePCDFile("filtered_voxel.pcd", *cloud_filtered);

    return 0;
}

直通滤波（Pass-through Filter）
目标：提取指定范围内的点。
原理：通过对点云的某一维（如 x、y 或 z 轴）设定上下限，只保留范围内的点。
优点：直观易用，适用于处理点云的某些局部区域。
缺点：仅能按简单规则过滤，无法应对复杂场景。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

    // 加载点云
    pcl::io::loadPCDFile("point_cloud.pcd", *cloud);

    // 直通滤波
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 1.0); // z轴范围
    pass.filter(*cloud_filtered);

    // 保存过滤后的点云
    pcl::io::savePCDFile("filtered_passthrough.pcd", *cloud_filtered);

    return 0;
}

条件滤波（Conditional Filter）
目标：根据自定义条件过滤点云。
原理：通过用户设定的逻辑条件（如颜色、强度等）筛选点。
优点：灵活，可适配各种复杂需求。
缺点：复杂条件可能降低效率。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

    // 加载点云
    pcl::io::loadPCDFile("point_cloud.pcd", *cloud);

    // 设置条件
    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>("x", pcl::ComparisonOps::GT, 0.0)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
        new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, 1.0)));

    // 条件滤波
    pcl::ConditionalRemoval<pcl::PointXYZ> cond_removal;
    cond_removal.setInputCloud(cloud);
    cond_removal.setCondition(range_cond);
    cond_removal.filter(*cloud_filtered);

    // 保存过滤后的点云
    pcl::io::savePCDFile("filtered_conditional.pcd", *cloud_filtered);

    return 0;
}

半径滤波（Radius Outlier Removal, ROR）
目标：移除稀疏点。
原理：计算每个点的固定半径邻域内的点数，删除点数小于指定阈值的点。
优点：有效移除孤立点。
缺点：参数设置依赖数据特性。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/radius_outlier_removal.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

    // 加载点云
    pcl::io::loadPCDFile("point_cloud.pcd", *cloud);

    // 半径滤波
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
    ror.setInputCloud(cloud);
    ror.setRadiusSearch(0.8); // 半径范围
    ror.setMinNeighborsInRadius(2); // 最小邻域点数
    ror.filter(*cloud_filtered);

    // 保存过滤后的点云
    pcl::io::savePCDFile("filtered_radius.pcd", *cloud_filtered);

    return 0;
}

高程滤波（Elevation Filtering）
目标：移除低于或高于特定高度的点。
应用场景：地面物体检测、建筑物提取。
原理：设定高度范围并筛选点云。
优点：适合处理有高程特征的点云。
缺点：仅适合特定场景。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>

int main() {
    // 创建点云对象
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

    // 加载点云数据
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("point_cloud.pcd", *cloud) == -1) {
        PCL_ERROR("无法加载点云文件 point_cloud.pcd\n");
        return -1;
    }

    std::cout << "原始点云点数: " << cloud->points.size() << std::endl;

    // 设置直通滤波器
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z"); // 高程维度
    pass.setFilterLimits(0.0, 1.0); // 高程范围 [0.0, 1.0]
    pass.filter(*cloud_filtered);

    std::cout << "滤波后点云点数: " << cloud_filtered->points.size() << std::endl;

    // 保存过滤后的点云
    pcl::io::savePCDFile("filtered_elevation.pcd", *cloud_filtered);

    std::cout << "高程滤波完成，结果已保存到 filtered_elevation.pcd" << std::endl;

    return 0;
}

滤波之后还需要对点云进行平滑操作。

平滑操作

我们用RGB-D，激光扫描仪等设备扫描物体，尤其是比较小的物体时，往往会有测量误差。这些误差所造成的不规则数据如果直接拿来曲面重建的话，会使得重建的曲面不光滑或者有漏洞，而且这种不规则数据很难用统计分析等滤波方法消除，所以为了建立光滑完整的模型必须对物体表面进行平滑处理和漏洞修复。
下面左边就是原始的扫描数据，右边就是用最小二乘法进行表面平滑后的结果
对同一个物体从不同方向进行了多次扫描，然后把扫描结果进行配准，最后得到一个完整的模型，可以通过重采样方法来实现点云的平滑，来提高配准精度

点云重采样

点云重采样，我们实际上是通过一种叫做“移动最小二乘”（MLS， Moving Least Squares ）法来实现的，对应的类名叫做：pcl::MovingLeastSquares

该模板类有两个模板参数PointInT与PointOutT,分别代表输入点云与输出点云的类型
可以用于平滑点云数据并估计每个点的法线
该类还包含基于参数拟合的对结果云进行上采样的方法，可以增加点云的密度，特别是在局部区域
参数设置
- setComputeNormals(true)：是否计算法线。
- setInputCloud(cloud)：设置输入的点云。
- setPolynomialOrder(2)：设置多项式的阶数，用于拟合。
- setSearchMethod(tree)：设置搜索方法，通常使用 KD 树。
- setSearchRadius(0.03)：设置搜索半径，决定了平滑处理的局部邻域大小

pcl::search::KdTree<PointT>::Ptr treeSampling (new pcl::search::KdTree<PointT>); // 创建用于最近邻搜索的KD-Tree
pcl::PointCloud<PointT> mls_points;   //输出MLS
pcl::MovingLeastSquares<PointT, PointT> mls;  // 定义最小二乘实现的对象mls
mls.setComputeNormals (false);  //设置在最小二乘计算中是否需要存储计算的法线
mls.setInputCloud (cloud_filtered);        //设置待处理点云
mls.setPolynomialOrder(2);      // 2 表示使用二次多项式拟合（即抛物面）
mls.setPolynomialFit (false);  // 设置为false可以 加速 smooth
mls.setSearchMethod (treeSampling);    // 设置KD-Tree作为搜索方法
mls.setSearchRadius (0.05); // 单位m.设置用于拟合的K近邻半径
mls.process (mls_points);        //输出

法线

对于曲面来说，曲面在某点P处的法线为垂直于该点切平面（tangent plane）的向量，法线很有用的，尤其是在三维建模中应用非常广泛，比如在计算机图形学（computer graphics）领域里，法线决定着曲面与光源（light source）的强弱处理（Flat Shading），对于每个点光源位置，其亮度取决于曲面法线的方向。
平面或曲面的法线比较容易计算，方程 ax + by + cz = d 表示的平面，向量(a, b, c)就是其法线。
点云的法线计算是稍微麻烦点，一般有两种方法：
1、使用曲面重建方法，从点云数据中得到采样点对应的曲面，然后再用曲面模型计算其表面的法线
2、直接使用近似值直接从点云数据集推断出曲面法线

第二点的方法具体来说，就是把估计某个点的表面法线问题简化为：从该点最近邻计算的协方差矩阵的特征向量和特征值的分析，这里就不多做介绍了。PCL已经帮我们封装好了函数啦

// 法线估计
pcl::NormalEstimation<PointT, pcl::Normal> normalEstimation;                    //创建法线估计的对象
normalEstimation.setInputCloud(cloud_smoothed);                                    //输入点云
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);          // 创建用于最近邻搜索的KD-Tree
normalEstimation.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);     // 定义输出的点云法线
// K近邻确定方法，使用k个最近点，或者确定一个以r为半径的圆内的点集来确定都可以，两者选1即可
normalEstimation.setKSearch(10);                    // 使用当前点周围最近的10个点
//normalEstimation.setRadiusSearch(0.03);            //对于每一个点都用半径为3cm的近邻搜索方式
normalEstimation.compute(*normals);                 //计算法线

题目：给定一个融合后的点云，已经对其进行下采样和滤波（代码已给）。请对其进行平滑（输出结果），然后计算法线，并讲法线显示在平滑后的点云上。

#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/mls.h>
#include <pcl/features/normal_3d.h>

typedef pcl::PointXYZRGB PointT;

int main(int argc, char** argv)
{

	// Load input file
    // cloud: 原始点云数据。
    // cloud_downSampled: 下采样后的点云。
    // cloud_filtered: 经过统计滤波的点云。
    // cloud_smoothed: 平滑处理后的点云。
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_downSampled(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_smoothed(new pcl::PointCloud<PointT>);
	// 加载点云数据
	if (pcl::io::loadPCDFile("./data/fusedCloud.pcd", *cloud) == -1)
    {
        cout << "点云数据读取失败！" << endl;
    }

    std::cout << "Orginal points number: " << cloud->points.size() << std::endl;

    // 下采样，同时保持点云形状特征
    pcl::VoxelGrid<PointT> downSampled;  //创建滤波对象
    downSampled.setInputCloud (cloud);            //设置需要过滤的点云给滤波对象
    downSampled.setLeafSize (0.01f, 0.01f, 0.01f);  //设置滤波时创建的体素体积为1cm的立方体
    downSampled.filter (*cloud_downSampled);           //执行滤波处理，存储输出

    pcl::io::savePCDFile ("./downsampledPC.pcd", *cloud_downSampled);

	// 统计滤波
	pcl::StatisticalOutlierRemoval<PointT> statisOutlierRemoval;       //创建滤波器对象
    statisOutlierRemoval.setInputCloud (cloud_downSampled);            //设置待滤波的点云
    statisOutlierRemoval.setMeanK (50);                                //设置在进行统计时考虑查询点临近点数
    statisOutlierRemoval.setStddevMulThresh (3.0);                     //设置判断是否为离群点的阀值:setStddevMulThresh: 设置点云中被认为是离群点的阈值（3倍标准差）。
    statisOutlierRemoval.filter (*cloud_filtered);                     //滤波结果存储到cloud_filtered
    pcl::io::savePCDFile ("./filteredPC.pcd", *cloud_filtered);
	// ----------------------开始你的代码--------------------------//
	// 请参考PCL官网实现以下功能
	// 对点云重采样

    // 点云平滑
    pcl::MovingLeastSquares<PointT, PointT> mls;  // 使用移动最小二乘方法对点云进行平滑。
    mls.setInputCloud(cloud_filtered);
    mls.setSearchRadius(0.03);                   // 搜索半径，设置为0.03米。
    mls.setPolynomialFit(true);                  // 启用多项式拟合来平滑点云。
    mls.setComputeNormals(false);                // 不计算法线

    pcl::PointCloud<PointT>::Ptr cloud_mls(new pcl::PointCloud<PointT>());
    mls.process(*cloud_mls);
    cloud_smoothed = cloud_mls;
    pcl::io::savePCDFile("./smoothedPC.pcd", *cloud_smoothed);   // 平滑后的点云存储在cloud_smoothed并保存为smoothedPC.pcd。

    // 法线估计
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    ne.setInputCloud(cloud_smoothed);
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());  //  设置搜索方法为KD树，快速查找邻居点。
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(0.03);    // 搜索半径为0.03米。
    ne.compute(*normals)

	// ----------------------结束你的代码--------------------------//
	// 显示结果
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("PCL Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_smoothed);
    viewer->addPointCloud<PointT> (cloud_smoothed, rgb, "smooth cloud");   // 添加点云到可视化界面。
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "smooth cloud");  //  设置点云渲染属性。
    viewer->addPointCloudNormals<PointT, pcl::Normal> (cloud_smoothed, normals, 20, 0.05, "normals");   // 将法线叠加显示。

    viewer->initCameraParameters ();

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);     // 循环更新视图直到用户关闭窗口。
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

    return 1;
}