PCL估计点云的表面法向量

估计点云表面法向量的方法

1. 根据点云中点及其邻域信息，构建点云表面mesh网格，然后mesh网格计算曲面法线
2. 使用近似值直接从点云数据集中推断出曲面法线
   PCL中点云表面法向量的实现是基于后者的，即在给定点云数据集的情况下，直接计算点云中每个点的表面法线。

理论基础

法线确定方法

确定表面上点的法线问题可以通过估计与表面相切的平面的法线问题来近似，从而又可以转化为最小二乘平面拟合估计问题。
因为点云中的点是三维的，要估计的平面是二维的，用三维数据估计二维数据，很容易想到的方法就是降维，而一提起降维，最容易想到的方法就是PCA，关于PCA的理论及代码实现，请参考我的文章PCA（主成分分析）降维原理及其在optdigits以及点云数据集上的python实现

因此，估计表面法线问题被简化为对由待估计点的最近邻生成的协方差矩阵的特征向量和特征值的分析。 更具体地说，对于每个点p_i，我们如下构建它的协方差矩阵C：

其中k是点p_i的k个相邻点，这些相邻点可以用最近邻方法确定，也可以指定半径确定，这在文章后面还会讨论到; p_bar表示临近点的3D质心; lamda_j表示协方差矩阵的第j个特征值，vector v_j表示协方差矩阵的第j个特征向量。

可以很容易看出，p_i和p_bar的shape都是(3,1)，所以协方差矩阵C的shape是(3,3)
要从一组点中估计协方差矩阵，在PCL中实现方式为：

  // Placeholder for the 3x3 covariance matrix at each surface patch
  Eigen::Matrix3f covariance_matrix;
  // 16-bytes aligned placeholder for the XYZ centroid of a surface patch
  Eigen::Vector4f xyz_centroid;

  // Estimate the XYZ centroid
  compute3DCentroid (cloud, xyz_centroid);

  // Compute the 3x3 covariance matrix
  computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix);

法线方向确定

PCA可以很好的实现降维，找到切平面，但是并不能确定切平面的法线方向。
解决这一问题的方法是引入一个视点约束，假设引入一个视点v_p，为了使所有法线vector n_i始终朝向视点的一面，它们需要满足如下不等式约束：

若要在PCL中手动重新定向给定法线，可以用如下代码：

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

选择合适的邻域尺度

如前所述，某一点的法线估计离不开其邻域点的约束。但是，给定点云数据，要确定某一点的邻域点集，需要确定合理的k(通过pcl::Feature::setKSearch给出)或r(通过pcl::Feature:: setRadiusSearch)给出，从而使得法线估计取得理想的效果，这就是邻域尺度问题。

该问题极其重要，是默认参数点特征估计的限制因素。为了更好地说明这个问题，下图比较了合理的尺度与偏大尺度的效果。左图是合理尺度的表面法线估计结果，可以看到，算法估计的表面法线分别与两个平面大致垂直，并且在整个桌子上可以看到小边缘。右图是偏大尺度的表面法线估计结果，因为比例尺度偏大，邻近点集覆盖了更多来自相邻表面的点，估计的法线变得失真，在两个平坦表面的边缘处表面法线出现明显旋转，并且边缘出现模糊、很多细节被抑制。

尺度因子的选择需要根据应用程序所需的详细程度来确定。 简单地说，如果杯子的手柄和圆柱形部分之间的边缘处的法线很重要，则比例因子需要足够小以捕获那些细节，否则设置成较大比例因子比较合适。

PCL估计表面法线代码实现

估算输入数据集中所有点的曲面法线：

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  ... read, pass in or create a point cloud ...

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}

ne.compute()方法实际上完成了以下的工作：

for each point p in cloud P

  1. get the nearest neighbors of p

  2. compute the surface normal n of p

  3. check if n is consistently oriented towards the viewpoint and flip otherwise

默认的视点坐标是(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表示来自点云的k-最近邻集合，plane_parameters和curvature表示法线估计的输出，plane_parameters保存法线(nx，ny，nz)的前3个坐标，第四个坐标是D = nc = p_plane（这里是质心）+ p。 输出表面curvature被估计为协方差矩阵的特征值之间的关系，如下所示：

用OpenMP加速法线估计

PCL提供了表面法线估计的加速实现，基于OpenMP使用多核/多线程来加速计算。 该类的名称是pcl :: NormalEstimationOMP，其API与单线程pcl :: NormalEstimation 100％兼容。 在具有8个内核的系统上，一般计算时间可以加快6-8倍。
示例代码：

#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>

{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  ... read, pass in or create a point cloud ...

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
  ne.setNumberOfThreads(12);  // 手动设置线程数，否则提示错误
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}

参考资料

Estimating Surface Normals in a PointCloud