激光点云法向量估计原理及数学推导

激光点云法向量估计原理及数学推导

简述

点云法向量估计这个问题，相信很多人在点云处理，曲面重建的过程中遇到过。表面法线是几何体面的重要属性。而点云数据集在真实物体的表面表现为一组定点样本。对点云数据集的每个点的法线估计，可以看作是对表面法线的近似推断。在开源库提供我们调用便利的同时，了解其实现原理也有利于我们对问题的深刻认识！格物要致知：）

原理

确定表面一点法线的问题近似于估计表面的一个相切面法线的问题，因此转换过来以后就变成一个最小二乘法平面拟合估计问题。

• 平面方程
  $$cos\alpha \cdot x+cos\beta \cdot y+cos\gamma \cdot z+p=0$$
  $cos\alpha ,cos\beta ,cos\gamma$为平面上点$(x,y,z)$处法向量的方向余弦，$|p|$为原点到平面的距离。
  $$ax+by+cz=d(d\ge 0),a^2+b^2+c^2=1$$
  求平面方程即转化为求$a,b,c,d$四个参数。

• 求解过程

1. 待拟合平面点集$(x_i,y_i,z_i),i=1,2,...,n$

待拟合的平面方程：
$$ax+by+cz=d(d\ge 0),a^2+b^2+c^2=1$$
任意点到平面的距离：
$$d_i=|ax+by+cz-d|$$

2. 要获得最佳拟合平面，则需要满足：

$$e=\sum _{i=1}^n{d}_i^2\to min$$

因此，转化为求解极值的问题，
$$f=\sum _{i=1}^n{d}_i^2-\lambda (a^2+b^2+c^2-1)$$

3. 分别对$d,a,b,c$求偏导
$$\frac{\partial f}{\partial d}=-2\sum _{i=1}^n(a{x}_i+b{y}_i+c{z}_i-d)=0$$
$$d=\frac{{\sum }_{i=1}^n{x}_i}{n}a+\frac{{\sum }_{i=1}^n{y}_i}{n}b+\frac{{\sum }_{i=1}^n{z}_i}{n}c$$
将$d$带入任意点到平面的距离公式：

$$\begin{array}{c}\begin{array}{rl}{d}_i& =|a(x_i-\frac{{\sum }_{i=1}^n{x}_i}{n})+b(y_i-\frac{{\sum }_{i=1}^n{y}_i}{n})+c(z_i-\frac{{\sum }_{i=1}^n{z}_i}{n})|\\ & =|a(x_i-\overline{x})+b(y_i-\overline{y})+c(z_i-\overline{z})|\end{array}\end{array}$$

继续求偏导
$$\frac{\partial f}{\partial a}=2\sum _{i=1}^n(a(x_i-\overline{x})+b(y_i-\overline{y})+c(z_i-\overline{z}))(x_i-\overline{x})-2\lambda a=0$$
令$\Delta x_i=x_i-\overline{x},\Delta y_i=y_i-\overline{y},\Delta z_i=z_i-\overline{z}$
则：
$$\frac{\partial f}{\partial a}=2\sum _{i=1}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta x_i-2\lambda a=0$$
同理：
$$\frac{\partial f}{\partial b}=2\sum _{i=1}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta y_i-2\lambda b=0$$
$$\frac{\partial f}{\partial c}=2\sum _{i=1}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta z_i-2\lambda c=0$$

将上述三式统一：
$$\left(\begin{array}{ccc}\sum \Delta x_i\Delta x_i& \sum \Delta x_i\Delta y_i& \sum \Delta x_i\Delta z_i\\ \sum \Delta x_i\Delta y_i& \sum \Delta y_i\Delta y_i& \sum \Delta y_i\Delta z_i\\ \sum \Delta x_i\Delta z_i& \sum \Delta y_i\Delta z_i& \sum \Delta z_i\Delta z_i\end{array}\right)\left(\begin{array}{c}a\\ b\\ c\end{array}\right)=\lambda \left(\begin{array}{c}a\\ b\\ c\end{array}\right)$$
易得：
$$Ax=\lambda x$$

即转化到了求解矩阵A的特征值与特征向量的问题，矩阵A即为n个点的协方差矩阵。$(a,b,c{)}^T$即为该矩阵的一个特征向量。

4. 求最小特征向量
如上所示，求得的特征向量可能不止一个，那么如何来选取特征向量，使得求得法向量为最佳拟合平面的法向量呢？

由$a^2+b^2+c^2=1,\Rightarrow (x,x)=1$(内积形式)，
$Ax=\lambda x,\Rightarrow (Ax,x)=(\lambda x,x),\Rightarrow \lambda =(Ax,x),$
$\Rightarrow \lambda ={\sum }_{i=0}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i{)}^2,$
$\Rightarrow \lambda ={\sum }_{i=0}^n{d}_i^2$

由$e={\sum }_{i=1}^n{d}_i^2\to min,\lambda \to min$

因此，最小特征值对应的特征向量即为法向量

程序应用

• PCL中的NormalEstimation

    #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 ()*
}

• 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 ()*
}

作者：codehory
链接：https://www.jianshu.com/p/faa9953213dd
来源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。