PCL实现点云法线计算

原理

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

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

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

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

协方差矩阵的计算公式：

对于一个包含 \(n\) 个三维数据点 \(\{ \mathbf{p}_i = (x_i, y_i, z_i) \}_{i=1}^{n}\) 的点集，其协方差矩阵 \(\mathbf{C}\) 的计算步骤如下：

1. 计算点集质心（均值）：

\[ \bar{\mathbf{p}} = \frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_i = (\bar{x}, \bar{y}, \bar{z}) \]

3. 计算协方差矩阵：

   \[\mathbf{C} = \frac{1}{n} \sum_{i=1}^{n} (\mathbf{p}_i - \bar{\mathbf{p}})(\mathbf{p}_i - \bar{\mathbf{p}})^T \]

   将其展开为一个 \(3 \times 3\) 的对称矩阵：

   \[\mathbf{C} = \begin{bmatrix} \text{cov}(x, x) & \text{cov}(x, y) & \text{cov}(x, z) \\ \text{cov}(y, x) & \text{cov}(y, y) & \text{cov}(y, z) \\ \text{cov}(z, x) & \text{cov}(z, y) & \text{cov}(z, z) \end{bmatrix} \]

   其中，每个元素的计算公式为：

   \[\begin{aligned} \text{cov}(x, x) &= \frac{1}{n} \sum_{i=1}^{n} (x_i - \bar{x})^2 \\ \text{cov}(y, y) &= \frac{1}{n} \sum_{i=1}^{n} (y_i - \bar{y})^2 \\ \text{cov}(z, z) &= \frac{1}{n} \sum_{i=1}^{n} (z_i - \bar{z})^2 \\ \text{cov}(x, y) = \text{cov}(y, x) &= \frac{1}{n} \sum_{i=1}^{n} (x_i - \bar{x})(y_i - \bar{y}) \\ \text{cov}(x, z) = \text{cov}(z, x) &= \frac{1}{n} \sum_{i=1}^{n} (x_i - \bar{x})(z_i - \bar{z}) \\ \text{cov}(y, z) = \text{cov}(z, y) &= \frac{1}{n} \sum_{i=1}^{n} (y_i - \bar{y})(z_i - \bar{z}) \end{aligned} \]

公式解读：

• 对角线元素是各坐标轴\(（X, Y, Z）\)自身的方差，衡量数据在该维度上的离散程度。
• 非对角线元素是不同坐标轴之间的协方差，衡量两个维度变化的关联性。协方差为零表示两个维度线性无关。
• 该矩阵描述了给定点集邻域内点的空间分布特征，是后续通过PCA求解法线的基础。

协方差矩阵的特征值分解

协方差矩阵满足特征方程：

\[\mathcal{C} \cdot \vec{v}_{j} = \lambda_{j} \cdot \vec{v}_{j}, \quad j \in \{0, 1, 2\} \]

其中：

• \(\lambda_j\) 是协方差矩阵的第 \(j\) 个特征值
• \(\vec{v}_j\) 是协方差矩阵的第 \(j\) 个特征向量
  特征值分解的目标是找到对应的特征值和特征向量。
  它们满足方程：

1. 求解特征值
   • 计算特征方程：det(C - λI) = 0
   • 特征方程展开后的具体形式为：λ³ - tr(C)λ² + (C₁₁C₂₂ + C₁₁C₃₃ + C₂₂C₃₃ - C₁₂² - C₁₃² - C₂₃²)λ - det(C) = 0
   • 解方程得到三个根，并按大小排序：λ₁ ≥ λ₂ ≥ λ₃
2. 求解特征向量
   • 对每个特征值 λᵢ，求解线性方程组：(C - λᵢI) vᵢ = 0
   • 得到的非零解即为对应的特征向量 vᵢ
     在点云处理中，通常取最小特征值 λ₃ 对应的特征向量 v₃ 作为法向量。

算法实现

协方差矩阵计算：

#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. 法线方向：默认方向可能不统一，需要设置视点来保持一致