从零开始一起学习SLAM | 点云平滑法线估计

点云滤波后为什么还需要平滑？

小白：师兄，师兄，上次你说的点云滤波我学会啦，下一步怎么把点云变成网格啊？
师兄：滤波只是第一步，在网格化前我们还需要对滤波后的点云进行平滑（smoothing）
小白：不是已经滤波了吗？怎么还要平滑啊？滤波和平滑不一样吗？
师兄：确实不太一样。我们用RGB-D，激光扫描仪等设备扫描物体，尤其是比较小的物体时，往往会有测量误差。这些误差所造成的不规则数据如果直接拿来曲面重建的话，会使得重建的曲面不光滑或者有漏洞，而且这种不规则数据很难用前面我们提到过的统计分析等滤波方法消除，所以为了建立光滑完整的模型必须对物体表面进行平滑处理和漏洞修复。
你看下面左边就是原始的扫描数据，右边就是用最小二乘法进行表面平滑后的结果

小白：从图上看，平滑确实效果很明显啊，左边杯子上黑色的是噪声吧，右边的结果来看经过平滑都消失了
师兄：对，除了上面说到的设备测量误差外，还有一种情况也需要对点云进行平滑。就是后处理过程中，比如我们对同一个物体从不同方向进行了多次扫描，然后把扫描结果进行配准，最后得到一个完整的模型，但是你配准的结果不一定准啊，比如下图中左侧就是配准后未经过处理的结果，同一面墙壁由于配准误差变成了“两面墙”，并不能完全重叠，你觉得这个数据可以直接用来进行表面重建吗？

如何通过重采样实现点云平滑？

师兄：点云重采样，我们实际上是通过一种叫做“移动最小二乘”（MLS， Moving Least Squares ）法来实现的，对应的类名叫做：pcl::MovingLeastSquares，你知道怎么用吗？
小白：不知道，不过我还记得我们上次师兄给我说的方法，在PCL API documentation http://docs.pointclouds.org/trunk/ 上查询类名称，就能看到类的定义和用法啦
师兄：活学活用啊，哈哈，那我们现在去查一下看看吧
小白：嗯，我查到了，这个MLS类的定义在这里：
http://docs.pointclouds.org/trunk/classpcl_1_1_moving_least_squares.html#a379330b0b1dacaa668d165f94930749c
成员函数好多啊
师兄：对，看着是很多，但是很多我们不常用的，比如我们常用的一个用于重采样的示例代码如下，每行代码都给你注释好了，结合上面网址看很容易理解

// 对点云重采样  
pcl::search::KdTree

小白：师兄，这个代码里的KD-Tree是干嘛的？
师兄：Kd-Tree是一种数据结构，是空间二分树的一种特殊情况，可以很方便的用于进行范围搜索。在这里用KD-Tree就是为了便于管理、搜索点云，这种结构来可以很方便的找到最近邻点。
小白：原来如此，那上面mls.setSearchRadius (0.05) 的意思是不是就是搜索当前点以5cm为半径的空间中所有的点？
师兄：对的，然后把这些点用2阶多项式拟合~

如何估计点云的表面法线？

小白：师兄，现在可以网格化了吗？
师兄：还不行。。。别急，网格化前我们还需要估计一下点云的表面法线（normal）
小白：啊，怎么又冒出来一个法线。。。
师兄：法线好像是中学就学过了，应该还记得平面的法线的定义吧，平面的法线是垂直于该平面的向量，如下图所示

你看上面右边那个图，对于曲面来说，曲面在某点P处的法线为垂直于该点切平面（tangent plane）的向量
小白：记得呢，不过这个法线有什么用？怎么就突然冒出来了
师兄：法线很有用的，尤其是在三维建模中应用非常广泛，比如在计算机图形学（computer graphics）领域里，法线决定着曲面与光源（light source）的强弱处理（Flat Shading），对于每个点光源位置，其亮度取决于曲面法线的方向。
小白：原来如此。不过好像平面或曲面的法线比较容易计算，方程 ax + by + cz = d 表示的平面，向量(a, b, c)
就是其法线。而我们这里是点云呢！怎么算呢？
师兄：确实如此。点云的法线计算是稍微麻烦点，一般有两种方法：
1、使用曲面重建方法，从点云数据中得到采样点对应的曲面，然后再用曲面模型计算其表面的法线
2、直接使用近似值直接从点云数据集推断出曲面法线
这里主要用第2种方法来近似估计点云中每个点的表面法线。
具体来说，就是把估计某个点的表面法线问题简化为：从该点最近邻计算的协方差矩阵的特征向量和特征值的分析，这里就不多做介绍了。PCL已经帮我们封装好了函数啦
我们计算出来点云的法线大概是这样的

小白：确实是这样啊，看来编程的时候要格外注意了。
师兄：法线估计的示例如下，我也给你注释好啦

// 法线估计
pcl::NormalEstimation

本文参考：PCL官网