【PCL自学：Feature2】主成分分析及积分图点云法向估计方法 (持续更新)

一、PCL点云表面法向估计理论

  在上一章中我们介绍了点云特征估计表示和示例程序，在这一章节中我们将介绍如何使用PCL对表面上的点云进行法向特征估计。
  表面法线是几何表面的重要属性，在计算机图形学应用等许多领域中被大量使用，常应用于光源阴影等视觉效果，在工业领域常用来进行物体表面分割的预处理。
  任意给定一个几何面，通常很容易推断出面上某一点的法线方向是垂直于该面。但是，由于我们获取的点云数据集代表了真实表面上的一组点样本，因此存在两种可能性：
  ①利用表面网格技术，从采集的点云数据集中获取表面，并从网格中计算出表面法线;
  ②使用近似值直接从点云数据集估计表面法线。
本文将只讨论第2种情况，即直接从点云数据估计表面法向问题。

1、表面法向估计理论基础：
  尽管存在许多不同的正态估计方法，但我们将从最简单的方法开始讨论学习，即【确定曲面上一点的法线的问题近似为估计平面切线到曲面的法线的问题】，这转而成为一个最小二乘平面拟合估计问题。其公式如下。有关表面法向估计及最小二乘方法可参考此论文

  对于上式，k是最邻近点的个数，$\bar{p}$表示最近邻点集的三维质心，${\lambda }_j$是协方差矩阵的第j个特征值，${\mathsf{v}}_j$是第j个特征向量。
  由以上公式可知，估计表面法线的解决方案可以被简化为查询点的最近邻点创建的协方差矩阵的特征向量和特征值(或PCA主成分分析)的分析。（PCA可参考博文：http://blog.csdn.net/zhongkelee/article/details/44064401）
更具体地说，对于每个查询点pi，我们都可利用其最邻近点(如何获取最邻近点参考上一章节)创建出协方差矩阵C。

  要从PCL中的一组点估计协方差矩阵，可以参考如下代码：

 // 使用eigen库创建最邻近点集的的3x3协方差矩阵
Eigen::Matrix3f covariance_matrix;
// 最邻近点集的质心（16字节）
Eigen::Vector4f xyz_centroid;

// 第一步：估计质心 cloud为输入的最邻近点集
compute3DCentroid (cloud, xyz_centroid);

// 第二部：计算协方差矩阵 
computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix);

  但是，其实仅仅通过主成分分析(PCA)计算的方向是模糊的（同一片点云可能出现正反两个方向），并且在整个点云数据集上不一致。例如下图展示了厨房环境的数据集的PCA估计法向的效果图。最右图是扩展高斯图像(Extended Gaussian Image, EGI)，也称为法线球，它描述了从点云开始的所有法线的方向。由于数据集是2.5D的，因此是从单一的视角获得的，法线应该只出现在EGI中一半的球体上。然而，由于方向的不一致性，它们分布在整个球体上。
   

如果点云的$v_p$实际上是大概已知的，那么这个问题的解决方案是很容易的。为了使所有法线$n_i$始终朝向视点，它们需要满足这个方程

显而易见，对于不满足该式子的$n_i$，将其取反即可得到同一表面上点云都指向同一方向（视角方向）的结果。如下图所示

  以上重定向法向的过程我们可以用PCL的一句指令实现。

flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Vector4f &normal);

PS：如果数据集有多个采集视点，则上述常规的重定向方法不成立，需要实现更复杂的算法。上述方法仅在一个视图内对点进行重定向（均指向视点）对于多视点的重定向算法可参考此论文

2、最邻近点尺度问题(k与r参数选择问题)：

  如前所述，一个点的表面法线需要从该点的周围的邻域点(也称为k邻域)来估计。但是给定一个采样点云数据集,参数k(pcl::feature::setKSearch)或者r(pcl::feature::setRadiusSearch)值决定了哪些邻近点被用来估计特征。那应该如何对这两个参数进行选择呢？
  这个问题非常重要，下图展示了选择小比例尺(即小r或k)与选择大比例尺(即大r或k)的效果。图的左侧描述了一个合理的、经过精心选择的比例尺因子，此时估计的法线与对应的平面几乎垂直。如果比例尺太大(右图),被用于估计法向的邻点数量和范围就更大，此时会导致估计出的法线不够准确，缺失了部分边缘特征，为分割平面操作埋下了隐患。

  在不深入太多细节讨论的情况下，可以认为确定一个点的邻域的尺度必须基于程序所需的细节级别来选择。简单地说，如果杯子把手和圆柱形部分之间的边缘曲率对分割杯子把手很重要，那么比例因子需要足够小以捕捉连接处法向等细节，若不需要这些细节就可以适当增大尺度。

二、PCL点云表面法向估计代码分析（主成分分析法可用于无序点云）

  虽然在第二章中已经给出了一个法向估计的例子，但为了更好地解释内部计算过程，我们将在这里修改其中一个例子。下面的代码是上一章节中估计输入数据集中所有点的表面法线代码，我们先在此回顾一下。

 #include 
 #include 
 
 {
 	// 点云指针
   	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 
   /*
   	此处读入或创建点云并链接到 cloud指针
   */
 
   	// 创建法向估计类，并传入点云cloud
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (cloud);
	// 创建空kd树，并将法向估计对象设置成树状结构
	// 此时树结构中被点云数据填满
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // 输出点云法向数据的指针
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // 设置搜索半径0.03（球状搜索）
  ne.setRadiusSearch (0.03);

  // 计算法向特征
  ne.compute (*cloud_normals);
}

pcl::NormalEstimation类实际计算调用过程中在内部做了如下操作

for each point p in cloud P

  1. get the nearest neighbors of p   // 获取最邻近点集

  2. compute the surface normal n of p // 计算表面法向n

  3. check if n is consistently oriented towards the viewpoint and flip otherwise //检查n是否始终朝向视点，否则翻转

一般情况下，视点会被默认为(0,0,0)，可以用如下指令进行设置

setViewPoint (float vpx, float vpy, float vpz);

而计算单个点的法向时，可以采用如下指令：

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

这里的cloud是指输入的点云，indices表示要计算法向的点的最邻近点集的索引集合，plane_parameters和 curvatrue 是估计法向操作的输出，plane_parameters内前三个参数是点的法线(nx, ny, nz)，第四个参数是D = nc.p_plane(此处为质心)+ p【PS：此处这个参数不知作何用途，肯可能代表的是邻近点集的质心到点p的距离】。输出该点处表面曲率被估计为协方差矩阵C(如上所示)的特征值之间的关系，为

除此之外还可以使用OpenMP库来加速表面法向估计，对于计算速度要求较高的用户，PCL提供了表面法线估计的额外实现，它使用多核/多线程范式，使用OpenMP来加速计算。这个类的名称是pcl::NormalEstimationOMP，它的API 100%兼容单线程的pcl:: normalestimate，这使得它适合作为一个临时替代。在一个有8个核的系统上，应该可以得到6-8倍的计算速度。

   此外，如果我们使用的点云是有组织的点云，即点的顺序已知(例如条纹结构光扫描仪等三维扫描设备获取的点云)计算按照如下方法进行法向估计能获得更快速稳定的表面法相估计效果。

三、PCL点云表面法向估计代码分析（积分图法 用于有序点云）

1、示例代码解析
  为了更直观的说明，我们直接上数据集和代码。
  首先，下载数据集table_scene_mug_stereo_textured.pcd并将其保存到磁盘某处。若无法从官网下载，可在文末通过网盘尝试下载，然后，在编译器中创建一个文件，比如normal_estimation_using_integral_images.cpp，并将以下内容放入其中

 #include 
 #include 
 #include 
 #include 
 #include 
 #include            // 使用VS2017+PCL1.8.1 必须加上此头文件否则报错
VTK_MODULE_INIT(vtkRenderingOpenGL);// 使用VS2017+PCL1.8.1 必须加上此条指令否则报错
// 除此之外还要在连接器里添加vtkRenderingOpenGL-8.0-gd.lib(以debug模式为例)否则报错

 int main ()
{
    // load point cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("table_scene_mug_stereo_textured.pcd", *cloud);
    
    // estimate normals
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);

    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
    ne.setMaxDepthChangeFactor(0.02f);
    ne.setNormalSmoothingSize(10.0f);
    ne.setInputCloud(cloud);
    ne.compute(*normals);

    // visualize normals
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor (0.0, 0.0, 0.5);
    viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, normals);
    
    while (!viewer.wasStopped ())
    {
      viewer.spinOnce ();
    }
    return 0;
}

以上代码可以直接复制到编译器使用(官网示例不行，这里做了一些修改)，使用VS2017+PCL1.8.1的运行效果如图：

  现在我们来逐条分析以上代码：

  以下代码功能为读入pcd格式点云文件

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("table_scene_mug_stereo_textured.pcd", *cloud);

  然后时创建点云法向估计对象并进行法向估计

	// 创建点云法向结果指针
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	// 创建基于积分图的法向估计对象
    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    // 设置法向估计方法
    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
    // 设置最大深度
    ne.setMaxDepthChangeFactor(0.02f);
    // 设置法向平滑尺寸
    ne.setNormalSmoothingSize(10.0f);
    // 填入点云
    ne.setInputCloud(cloud);
    // 法向估计计算
    ne.compute(*normals);

  后面的代码主要为为了显示，在此不再详解，后续写到点云显示部分再来详述。

  我们主要观察设置法向估计方法这步代码，内部参数其实是可选的。

    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);

AVERAGE_3D_GRADIENT模式：创建6个积分图像来计算水平和垂直的三维梯度的平滑版本，并使用这两个梯度之间的交叉乘积来计算法线。
COVARIANCE_MATRIX模式：创建9个积分图像，从其局部邻域的协方差矩阵计算特定点的法线。
AVERAGE_DEPTH_CHANGE模式：只创建一个积分图像，并从平均深度变化计算法线。
  
  
  
2、什么是积分图

  将一张图片中的每一个像素点的取值替换为其（在原始图片中）左上角区域的所有像素值之和，就是积分图。公式如下：

相关原理较为抽象，具体想了解其中奥妙的同学可参考[这篇论文] 。
若不想看长篇大论，可以参考[此篇博文]
总的来说，使用积分图进行法向计算的效率远高于主成分分析(PCA)的方法，但是这种方法要求点云是有序的，而且与PCA相比，法向估计误差略有损失。对于效率要求较高的场合更建议使用积分图点云法向估计方法来做。

[参考资料]
[1] 主成分分析：http://blog.csdn.net/zhongkelee/article/details/44064401
[2] PCL官方指导文档：https://pcl.readthedocs.io/projects/tutorials/en/master/normal_estimation.html#normal-estimation
[3]积分图法向估计算法论文：S. Holzer, R. B. Rusu, M. Dixon, S. Gedikli and N. Navab, “Adaptive neighborhood selection for real-time surface normal estimation from organized point cloud data using integral images,” 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2012, pp. 2684-2689, doi: 10.1109/IROS.2012.6385999.
[4]积分图法向估计简要概述博文：https://blog.csdn.net/OTZ_2333/article/details/107917018

[附录]
table_scene_mug_stereo_textured.pcd 的点云数据 网盘下载链接
链接：https://pan.baidu.com/s/11tPGn-DrDbzULCQfc1PDPA?pwd=TJUM
提取码：TJUM

---

【博主简介】
  斯坦福的兔子，男，95，天津大学机械工程工学硕士。21年毕业至今从事光学三维成像及点云处理相关工作。因工作中使用的三维处理库为公司内部库，不具有普遍适用性，遂自学开源PCL库及其相关数学知识以备使用。谨此将自学过程与君共享。
博主才疏学浅，尚不具有指导能力，如有问题还请各位在评论处留言供大家共同讨论。
若前辈们有工作机会介绍欢迎私信。