PCL中实现点云法向量计算

原理

法线的定义：法线是指垂直于一个平面的法向量。
对于点云（非固定平面）数据，如何计算法线？主要有两种方法：
表面网格技术：从获取的数据中提取一个基础的表面，然后从网格中计算表面的法线。
近似值推断法：直接从点数据集中推断表面法线。其核心思想是：为每个点计算其表面的切平面，这通常通过最小二乘平面拟合来估计。

那什么是最小二乘平面拟合呢？
给定一组三维数据点，找到最佳拟合平面 Z = AX + BY + C，使得所有数据点到该平面的垂直距离（残差）的平方和最小。

在点云处理中，通常使用主成分分析（PCA） 来实现平面拟合。PCA的核心目标是通过降维和去相关，找到数据变化的主要方向。

PCA的实现流程先对点云去中心化，然后再计算斜方差矩阵，最后计计算协方差矩阵的特征向量，其中最小特征值对应的特征向量就是法向量。

算法实现

协方差矩阵计算：

#include <pcl/common/centroid.h>   // 协方差矩阵计算的头文件

Eigen::Vector4f centroid;          // 存储均值 (x, y, z, 1)
Eigen::Matrix3f covariance;         // 3x3协方差矩阵
// 核心一行：计算均值和协方差矩阵
int points_used = pcl::computeMeanAndCovarianceMatrix(*cloud, covariance, centroid);

特征向量求解：

Eigen::Vector3f eigenvalues = solver.eigenvalues(); //特征值
Eigen::Matrix3f eigenvectors = solver.eigenvectors();  // 列向量对应特征值
Eigen::Vector3f normal = solver.eigenvectors().col(0); // 法向量赋值

所有点计算法向量：

for each point p in cloud:
    // 1. 找到 p 的邻域点集（半径内或 K 个最近邻）
    neighbors = tree->radiusSearch(p, radius);

    // 2. 如果邻域点数不足（如 < 3），法向量设为 NaN 或跳过
    if (neighbors.size() < 3) continue;

    // 3. 计算邻域重心
    Eigen::Vector4f centroid;
    computeCentroid(neighbors, centroid);

    // 4. 构建协方差矩阵
    Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
    for each neighbor in neighbors:
        Eigen::Vector3f centered = neighbor - centroid;
        cov += centered * centered.transpose();
    cov /= neighbors.size();   // 或除以 (n-1)

    // 5. 特征分解
    Eigen::SelfAdjointEigenSolver<Matrix3f> solver(cov);
    Eigen::Vector3f normal = solver.eigenvectors().col(0);  // 最小特征值对应特征向量

    // 6. 法向量定向（使指向传感器方向）
    if (normal.dot(p - viewpoint) < 0) normal = -normal;

    // 7. 存储法向量
    cloud_normals[i] = normal;

但在PCL中，有封装好了完整的实现函数pcl::NormalEstimation，因此只需要几行代码就能实现法向量计算

#include <pcl/features/normal_3d.h>    // 法向量估计头文件

// 创建法向量估计对象
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);   // 设置输入点云

ne.setSearchMethod(tree);   // 设置邻域搜索方式
//设置搜索半径（0.03 米）
ne.setRadiusSearch(0.03);

//创建存储法向量的点云对象，并执行计算
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
ne.compute(*cloud_normals);   // 核心计算：遍历每个点，估计法向量

完整代码

#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h> 
#include <iostream>

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 << "无法加载文件 " << std::endl;
        return -1;
    }

    // 2. 计算法向量
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(0.03); // 根据点云尺度调整

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    ne.compute(*cloud_normals);
    std::cout << "法线数量: " << cloud_normals->size() << std::endl;

    // 3. 可视化
    pcl::visualization::PCLVisualizer viewer("normal");
    viewer.setBackgroundColor(0, 0, 0); // 黑色背景

    // 添加原始点云，颜色为白色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color(cloud, 255, 255, 255);
    viewer.addPointCloud<pcl::PointXYZ>(cloud, cloud_color, "cloud");

    int level = 10;  // 每 level 个点显示一个法向量
    float scale = 0.05; // 箭头长度缩放因子，可根据点云尺寸调整
    viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, cloud_normals, level, scale, "normals");

    // 进入可视化循环
    while (!viewer.wasStopped()) {
        viewer.spinOnce(100);
    }

    return 0;
}

输出结果

注意事项

1. 搜索半径选择：太小会导致法线不稳定，太大则会平滑细节，并且增加计算量
2. 法线方向：默认方向可能不统一，需要设置视点来保持一致