点云法向量估计原理及应用PCL

2019-08-25 本文已影响0人 codehory

简述

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

原理

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

平面方程
$cos\alpha·x+cos\beta·y+cos\gamma·z+p=0$

$cos\alpha,cos\beta,cos\gamma$ 为平面上点 $(x,y,z)$ 处法向量的方向余弦， $|p|$ 为原点到平面的距离。
$ax+by+cz=d(d\geq0),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\geq0),a^{2}+b^{2}+c^{2}=1$
任意点到平面的距离： $d_i=|ax+by+cz-d|$

2. 要获得最佳拟合平面，则需要满足：
$e=\sum^n_{i=1} d_i^2\to min$
因此，转化为求解极值的问题，
$f=\sum^n_{i=1} d_i^2\space-\lambda(a^{2}+b^{2}+c^{2}-1)$

3. 分别对 $d,a,b,c$ 求偏导
$\frac{\partial f}{\partial d}=-2\sum^n_{i=1} (ax_i+by_i+cz_i-d)=0$
$d=\frac{\sum ^{n}_{i=1}x_i}{n}a+\frac{\sum^{n}_{i=1}y_i}{n}b+\frac{\sum^{n}_{i=1}z_i}{n}c$
将 $d$ 带入任意点到平面的距离公式：
$\begin{equation}\begin{split} d_i&=|a(x_i-\frac{\sum ^{n}_{i=1}x_i}{n})+b(y_i-\frac{\sum ^{n}_{i=1}y_i}{n})+c(z_i-\frac{\sum ^{n}_{i=1}z_i}{n})|\\ &=|a(x_i-\overline x)+b(y_i-\overline y)+c(z_i-\overline z)|\\ \end{split}\end{equation}$
继续求偏导
$\frac{\partial f}{\partial a}=2\sum^n_{i=1} (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^n_{i=1} (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^n_{i=1} (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^n_{i=1} (a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta z_i-2\lambda c=0$
将上述三式统一：
$\begin{pmatrix}\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{pmatrix}\begin{pmatrix}a \\ b \\c \end{pmatrix}=\lambda\begin{pmatrix}a\\b\\c \end{pmatrix}$
易得：
$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^n_{i=1} 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 ()*
}

点云法向量估计原理及应用PCL

简述

原理

程序应用

猜你喜欢

热点阅读