《点云处理》 点云法向

前言

很多点云处理过程中，都需要计算法向。曲率的计算需要使用法向，平面方程求解需要求解其法向，使用区域生长进行点云分割需要计算法向。其实如何去计算点云的法向量是非常重要非常关键的。

1.CloudCompare中法向计算

CloudCompare是一个开源且非常便于使用的3D点云显示及处理软件，简称CC。CC中集成有点云法向的计算算法，可以在读取点云后进行法向计算。其具体操作步骤可以点击这篇博文查看学习。

2.使用PCL求解法向

PCL中集成有计算点云法向的算法，在计算法向的同时顺便得到了每个点的曲率。

头文件：
#include <pcl/features/normal_3d_omp.h>

代码如下：

/// <summary>
/// 使用PCL库中集成的方法进行法向估计
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="normals">输出点云法向</param>
/// <param name="raduis">设置搜索半径</param>
/// <returns>return true表示计算成功，return false 表示计算失败</returns>
bool NormalEstimate(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::Normal>::Ptr& normals, const double& raduis)
{
    if (cloud == nullptr) return false;
    if (cloud->points.size() < 10) return false;
    if (normals == nullptr) normals.reset(new pcl::PointCloud<pcl::Normal>);

    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    ne.setInputCloud(cloud);        // 输入点云
    ne.setSearchMethod(tree);       // 设置搜索方法
    ne.setRadiusSearch(raduis);     // 设置搜索半径
    ne.setNumberOfThreads(10);      // 设置线程数量
    ne.compute(*normals);           // 计算法向
    

    return true;
}

输入点云的点数是 167063，耗时0.54s，搜索半径设置的0.2。上述代码只是常规用法，有关于这个法向估计还有点其他特殊用法，请参见点云侠。

值得注意的是，与拟合直线和拟合平面不同。此时ne.setNumberOfThreads(10); 真正起到了作用，能够节省运行时间。

3.PCA主成分分析

使用PCA进行点云的法向估计也是一种非常常见的方法，大量论文中都使用了这个方法进行法向估计。

Eigen 实现：

/// <summary>
/// 使用Eigen实现PCA算法，完成点云法向估计
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="eigenVectorsPCA">输出特征向量</param>
/// <param name="eigenValuesPCA">输出特征值</param>
/// <param name="is_fix">是否矫正法向</param>
/// <returns>return true表示计算成功，return false 表示计算失败</returns>
bool NormalEigenPCA(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, Eigen::Matrix3f& eigenVectorsPCA, Eigen::Vector3f& eigenValuesPCA, const bool& is_fix)
{
    if (cloud == nullptr) return false;
    if (cloud->points.size() < 5) return false;

    Eigen::Vector4f pcaCentroid;
    pcl::compute3DCentroid(*cloud, pcaCentroid);
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    eigenVectorsPCA = eigen_solver.eigenvectors();
    eigenValuesPCA = eigen_solver.eigenvalues();
    if (is_fix)
    {
        eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
        eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
        eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
    }
    
    std::cout << "特征值va(3x1):\n" << eigenValuesPCA << std::endl;
    std::cout << "特征向量ve(3x3):\n" << eigenVectorsPCA << std::endl;
    std::cout << "质心点(4x1):\n" << pcaCentroid << std::endl;

    return true;
}

值得注意的是，上述代码得到的是整个点云的法向和特征值，并没有实际得到每个点的法向和曲率。

PCL实现：

/// <summary>
/// 使用PCL实现PCA算法，完成点云法向估计
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="eigenVectorsPCA">输出特征向量</param>
/// <param name="eigenValuesPCA">输出特征值</param>
/// <param name="is_fix">是否矫正法向</param>
/// <returns>return true表示计算成功，return false 表示计算失败</returns>
bool NormalPclPCA(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, Eigen::Matrix3f& eigenVectorsPCA, Eigen::Vector3f& eigenValuesPCA, const bool& is_fix)
{
    if (cloud == nullptr) return false;
    if (cloud->points.size() < 5) return false;

    // 另一种计算点云协方差矩阵特征值和特征向量的方式:通过pcl中的pca接口，如下，这种情况得到的特征向量相似特征向量
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCA<pcl::PointXYZ> pca;
    pca.setInputCloud(cloud);
    pca.project(*cloud, *cloudPCAprojection);
    eigenVectorsPCA = pca.getEigenVectors();
    eigenValuesPCA = pca.getEigenValues();
    // 将特征值和特征向量存储在 std::vector 中以便排序
    std::vector<std::pair<float, Eigen::Vector3f>> eigenPairs;
    for (int i = 0; i < 3; ++i) {eigenPairs.push_back(std::make_pair(eigenValuesPCA(i), eigenVectorsPCA.col(i))); }

    // 对特征值进行排序
    std::sort(eigenPairs.begin(), eigenPairs.end(),[](const std::pair<float, Eigen::Vector3f>& a, const std::pair<float, Eigen::Vector3f>& b)
        {
            return a.first < b.first;
        });

    // 更新特征值和特征向量
    for (int i = 0; i < 3; ++i) 
    {
        eigenValuesPCA(i) = eigenPairs[i].first;
        eigenVectorsPCA.col(i) = eigenPairs[i].second;
    }
    if (is_fix)
    {
        eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
        eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
        eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
    }

    std::cerr << std::endl << "EigenValues:\n " << eigenValuesPCA << std::endl;//计算特征值
    std::cerr << std::endl << "EigenVectors: \n" << eigenVectorsPCA << std::endl;//计算特征向量
  
   

    return true;
}

得到的结果如下：

从结果来看，两种方法计算得到法向和特征值是一致的，但是耗时方面，Eigen实现的显然更快一点。