PCL点云库入门——PCL库点云特征之点云法向量（NormalEstimation）及其可视化

1、PCL点云库中点云特征综述  

1.1、点云特征综述

        点云特征描述在三维数据处理领域扮演着至关重要的角色，它直接决定了后续的识别、分类以及重建等关键任务的执行效果。在众多的特征描述方法中，我们可以看到基于几何形状的特征、基于统计信息的特征以及基于变换域的特征这三种主要类别。几何形状特征，例如法线和曲率等，能够直观地揭示点云数据中各个点的局部结构信息；而统计信息特征，如分布直方图和协方差矩阵等，从宏观层面描述了点云数据的整体特性；变换域特征，例如傅里叶变换和小波变换等，通过频域分析的方法来揭示点云数据中隐藏的深层次信息。随着深度学习技术的不断进步，基于神经网络的特征提取方法也逐渐成为研究的热点，并且在特征表达能力方面展现出了巨大的潜力。通过综合运用这些不同的特征描述方法，我们可以显著提高点云数据处理的精度和应用的广泛性。

        在PCL库它已支持多种特征描述子，其中法线特征描述子是基础且关键的一种。法线特征描述了点云中每个点的表面方向，对于后续的点云处理任务，如表面重建、分割和识别等，具有不可替代的重要作用。它为点云数据的进一步分析提供了重要的几何信息，是许多高级点云处理任务的基础。

        除了法线特征，PCL库还引入了FPFH（Fast Point Feature Histograms）特征描述子，这是一种基于点与其邻域点之间几何关系的特征描述方法。FPFH通过计算点与其邻域点之间的法线夹角和距离分布，生成一个直方图来描述点的局部特征。FPFH特征描述子因其计算速度快、特征描述能力强等优点，被广泛应用于点云的配准、识别和分类等任务中。它能够有效地捕捉点云数据中的局部结构信息，为点云数据的进一步处理提供了有力的工具。

        SHOT（Signature of Histograms of OrienTations）是另一种在点云处理领域中具有重要地位的特征描述子。SHOT特征描述子结合了点的法线信息和局部几何信息，通过计算点与其邻域点之间的法线夹角和距离分布，生成一个高维的特征向量。SHOT特征描述子具有旋转不变性和良好的描述能力，对于点云的识别和分类等任务具有较高的准确性。它在处理具有复杂几何结构的点云数据时，能够提供更为丰富和精确的特征信息。

        借助PCL库中的这些特征描述子，研究人员和工程师可以进一步实现点云数据的分类、识别和三维重建等高级任务。例如，通过计算点云中每个点的FPFH或SHOT特征，可以训练出高效的分类器对点云进行准确的分类；或者利用这些特征进行点云的配准和三维重建，实现更精确的三维模型构建。通过充分利用PCL库中的特征描述子，可以有效地捕捉点云数据的局部和全局特性，从而实现点云数据的分类、识别和三维重建等高级任务，为三维数据处理领域带来更多的可能性和应用前景。

1.2、PCL库中的特征实现

    PCL库中的特征模块在feature文件中，重要的基础模块在文件#include <pcl/features/feature.h>中，是所有点云特征描述信息的基类，其他高级特征在此类上继承扩展开。文件中包含主要内容有两个平面参数估计函数void solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,....)和特征基类class Feature、法向量特征基类class FeatureFromNormals，此基类非常重要；特征标签基类class FeatureFromLabels和局部参考系特征基类class LocalReferenceFrames。下面仅对class Feature基类从主要的成员变量和函数进行简要的说明。

        1、主要的成员变量

              1).特征名称

       std::string feature_name_;

              2).K邻域的搜索方法 

       SearchMethodSurface search_method_surface_;

              3).K邻域的搜索时的点云数据 

       PointCloudInConstPtr surface_;

              4).KDtree搜索方法对象 

       KdTreePtr tree_;

              5).搜索的参数(如半径R或者K) 

      double search_parameter_;

             6).K邻域的的半径

      double search_radius_;

             7).K邻域的的K值 

       int k_;

             8).是否提供额外构建搜索的点云数据，如果为false，这用原的点云数据

       bool fake_surface_;

           2、主要成员函数

            1).设置K邻域的的半径R值

     inline void setRadiusSearch (double radius);

             2).设置K邻域的的K值 

     inline void setKSearch (int k);

             3).设置搜索方法 

     inline void setSearchMethod ;

             4).设置额外提供的点云数据 

     inline void setSearchSurface (const PointCloudInConstPtr &cloud)

             5).计算特征的公共接口，其他子类通过继承、多态的方式来完成特征计算

     void compute (PointCloudOut &output);

             6).在特征计算前执行安全检测，如点云数据是否为空，相关参数是否初始化，如KDtree

     initCompute ();

            7).在特征计算后执行参数状态重置，如相关内存情况

     virtual bool deinitCompute ();

            8).对来自setsearch方法搜索k个最近邻域的实现 

     inline int
   searchForNeighbors (std::size_t index, double parameter,
                          std::vector<int> &indices, std::vector<float> &distances) const；

           9).为纯虚函数，子类实现具体的特征计算

     virtual void computeFeature (PointCloudOut &output) = 0;

2、法向量特征估计

2.1、算法原理

        点云法向量估计的原理主要基于局部表面拟合的思想，示意如图1。首先，在点云数据中选择一个待估计法向量的点作为中心点，然后在其邻域内选取一定数量的邻近点。通过这些邻近点，可以构建一个局部坐标系，并使用最小二乘法拟合出一个局部平面。该平面的法向量即为所求的中心点的法向量。具体步骤包括邻域点的选取、局部平面的拟合以及法向量的计算和归一化。此外，为了提高估计的准确性和鲁棒性，通常还会引入权重因子，以考虑邻域点与中心点之间的距离和分布情况。通过这种方法，能够有效地从无序的点云数据中提取出表面的法向信息，为后续的点云处理和分析提供重要的基础数据。

图1 

        在PCL点云库采样的是主成分分析方法（PCA Principal Component Analysis）来拟合局部平面实现点云法向量特征信息的估计，具体的实现步骤如下： 

        1)、点的局部邻域构建

        选择点pi，搜索构建其K邻域的点云集Pj，其中j=[1,....,k] 为邻域点的索引。

        2)、计算局部邻域点云的协方差矩阵

                第1步：计算邻域点云的均值

                第2步：计算每个点相对均值的偏移量 

                第3步：构建协方差矩阵C  

          3)、求解协方差矩阵的特征值和特征向量

                特征向量表示数据分布的方向，特征值表示在方向上的分布大小。

          4)、最小特征值对应的特征向量为法向量

                在局部切平面上，PCA求解的最小特征值对应的特征向量就该点的法向量，这是因为协方差C最小特征值对应的特征向量表示点云在这个方向上的分布小，即垂直于切平面的方向，因此可以近似的表示该点的法向量。

           5)、法向量方向的确定

                通过上面求的法向量的方向是杂乱的如图2，而在实际的工作中我们常希望法向量方向统一朝外或朝内，例如在渲染时法向量的方向由为重要，因为法向量的方向了决定渲染光源的方向，不同的光源方向渲染可视化不一样。在PCL库提供了根据当前帧视点来统一法向量方向的方法，具体操作如下公式：

               如果α的值小于0，则表示与视点的方向相反，需要将法向量的方向取反。通过该方法可以对法向量方向就行统一化效果如图3，但是效果不是太好，更专业的可以用最小生成树法，具体怎么执行这里不展开。

图2 

图3 

2.2、主要成员函数和变量

        1、主要的成员变量

        1)、法向量方向的统一朝向的视点，默认为(0,0,0)

      float vpx_, vpy_, vpz_;

         2)、局部领域点的协方差矩阵

    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_;

        3)、局部领域点的中心点

    Eigen::Vector4f xyz_centroid_;

       4)、是否用传感器提供的源点作为视点

    bool use_sensor_origin_;

        2、主要成员函数

          1)、最小二乘法估计点的法向量

  inline bool
      computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
                          Eigen::Vector4f &plane_parameters, float &curvature)；

inline bool
      computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
                          float &nx, float &ny, float &nz, float &curvature)；

       2)、设置要估计法向量的点云数据

    inline void
          setInputCloud (const PointCloudConstPtr &cloud) override

      3)、设置视点值  

    inline void
          setViewPoint (float vpx, float vpy, float vpz)

2.3、主要部分代码注解

           1、法向量估计主要部分

 template <typename PointInT, typename PointOutT> void
pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
  //K邻域搜索需要参数的初始化
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);


  output.is_dense = true;
  // 如果点云为稠密，则不检查每个点的NaN/Inf值可以节省时间
  if (input_->is_dense)
  {
    // 对每一个点进行操作
    for (std::size_t idx = 0; idx < indices_->size (); ++idx)
    {
        //1.执行最近邻域搜索
        //2.当前点的法向量估计
      if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
          !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
      {
         //3.输出法向量结果
        output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();


        output.is_dense = false;
        continue;
      }
       //4.法向量方向的确定
      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
                                  output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);


    }
  }
  else
  {
    // Iterating over the entire index vector
    for (std::size_t idx = 0; idx < indices_->size (); ++idx)
    {
        //0.无效点的识别判断
        //1.执行最近邻域搜索
        //2.当前点的法向量估计
      if (!isFinite ((*input_)[(*indices_)[idx]]) ||
          this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
          !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
      {
          //3.输出法向量结果
        output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();


        output.is_dense = false;
        continue;
      }
        //4.法向量方向的确定
      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
                                  output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);


    }
  }
}

               2、单个点法向量估计

template <typename PointT> inline bool
  computePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
                      Eigen::Vector4f &plane_parameters, float &curvature)
  {
    // 邻域点的协方差矩阵
    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
    // 邻域点的均值
    Eigen::Vector4f xyz_centroid;
    // 1.协方差矩阵和均值的计算
    if (indices.size () < 3 ||
        computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0)
    {
      plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
      curvature = std::numeric_limits<float>::quiet_NaN ();
      return false;
    }
    // 2.切平面法向估计作为当前点的法向量
    solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
    return true;
  }

              3、法向量方向的确定

template <typename PointT, typename Scalar> inline void
  flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
                              Eigen::Matrix<Scalar, 4, 1>& normal)
  {
    Eigen::Matrix <Scalar, 4, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z, 0);


    // 计算法向量与视点之间的夹角
    float cos_theta = vp.dot (normal);


    // 夹角大小判断
    if (cos_theta < 0)
    {
     //法向量方向取反
      normal *= -1;
      normal[3] = 0.0f;
      // Hessian form (D = nc . p_plane (centroid here) + p)
      normal[3] = -1 * normal.dot (point.getVector4fMap ());
    }
  }

2.4、算法使用示例

/*****************************************************************//**
* \file   PCLFeatureNormalmain.cpp
* \brief  
*
* \author YZS
* \date   January 2025
*********************************************************************/
#include<iostream>
#include <vector>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/auto_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/normal_3d.h>//法向量计算头文件
using namespace std;
void PCLNoramls()
{
       //加载点云数据
       pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new  pcl::PointCloud<pcl::PointXYZRGB>());
      std::string fileName = "E:/PCLlearnData/12/fragment.pcd";
       pcl::io::load(fileName, *cloud);
       std::cout << "Cloud Size:" << cloud->points.size() << std::endl;
       //法向量估计器对象
       pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
       ne.setInputCloud(cloud);
       //创建搜索方法KDtree
       pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new  pcl::search::KdTree<pcl::PointXYZRGB>());
       ne.setSearchMethod(tree);
       //法向量结果
       pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new  pcl::PointCloud<pcl::Normal>);
       //设置领域搜索半径
       ne.setRadiusSearch(0.02);
       //特征计算
       ne.compute(*cloud_normals);
       std::cout << "filter Cloud Size:" << cloud_normals->points.size() << std::endl;
       //法向量结果可视化
   // PCLVisualizer对象
       pcl::visualization::PCLVisualizer viewer("FeatureVIS");
       //创建左右窗口的ID v1和v2
       int v1(0);
       int v2(1);
       //设置V1窗口尺寸和背景颜色
       viewer.createViewPort(0.0, 0.0, 0.5, 1, v1);
       viewer.setBackgroundColor(0, 0, 0, v1);
       //设置V2窗口尺寸和背景颜色
       viewer.createViewPort(0.5, 0.0, 1, 1, v2);
       viewer.setBackgroundColor(0.1, 0.1, 0.1, v2);
       // 添加2d文字标签
       viewer.addText("v1", 10, 10, 20, 1, 0, 0, "Txtv1", v1);
       viewer.addText("v2", 10, 10, 20, 0, 1, 0, "Txtv2", v2);
       //设置cloud1的渲染属性，点云的ID和指定可视化窗口v1
       viewer.addPointCloud(cloud, "cloud1", v1);
       viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,  2, "cloud1");
       //设置法向量结果的渲染到V2窗口中
       viewer.addPointCloud(cloud, "cloud2", v2);
       viewer.addPointCloudNormals<pcl::PointXYZRGB,pcl::Normal>(cloud,cloud_normals,3,0.1f,  "cloud2n", v2);
       
       viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,  2, "cloud2");
       // 可视化循环主体
       while (!viewer.wasStopped())
       {
              viewer.spinOnce();
       }
}
int main(int argc,char *argv[])
{
       PCLNoramls();
  std::cout<<"Hello PCL World!"<<std::endl;
  std::system("pause");
  return 0;
}

结果：

3、法向量信息可视化 

        代码如下：

//设置法向量结果的渲染到V2窗口中
       viewer.addPointCloud(cloud, "cloud2", v2);
       viewer.addPointCloudNormals<pcl::PointXYZRGB,pcl::Normal>(cloud,cloud_normals,3,0.1f,  "cloud2n", v2);

        结果：

至此完成第十三节PCL库点云特征之点云法向量及其可视化学习，下一节我们将进入《PCL库中点云特征之PFH特征描述》的学习。