激光SLAM之NDT算法（1）算法原理

/在激光SLAM之NDT算法（2）-建图中我会给出实测可用的建图代码,并予以解释代码结构,这里就先讲讲原理吧!!!/
无人车激光SLAM系统简单可以分为建图和定位两部分，无人车的定位问题，实际上就是要找出无人车当前在地图的那个位置（并且要求精度在厘米级别）。其中有一类方法被称为即时定位与地图构建（simultaneous localization and mapping，SLAM），它能够实现导航定位而不需要地图，然而，真正实现无人车的SLAM是非常困难的，作为交通工具，远距离的行驶会使得实时构建地图的偏差变大。所以，在已经拥有高精度地图的前提下去做无人车的定位，是更加现实，简单的。NDT就是一类利用已有的高精度地图和激光雷达实时测量数据实现高精度定位的技术。
NDT综述

正态分布变换算法是一个配准算法，他应用于三维点的统计模型，使用标准最优化技术来确定联合和点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。即类似与图像匹配一样的三维点云匹配。

NDT原理及优化

​ NDT，即是normal distribution transformation的缩写，正态分布变换！该算法的核心思想是首先将空间离散为方格，若是二维空间，则离散为栅格，若是三维空间则离散划分为立方体，这样就可以将采样的点云划分到不同的网格中，这样可以很方便的描述点云的局部特性，例如点云局部的形状（直线、平面or球体）、方向（平面法向、直线方向等）。例如现在我们可以利用统计的方法分析每一个网格的特性。

​ 正态分布不仅是一种广泛存在的数据分布形式，同时又具有良好的数学性质，均值和方差（高维情况：均值向量和协方差矩阵），因此总是能在各种SLAM中的建模中看到它。其次，高维情况下的协方差矩阵还能反映点云的局部特性。假设现在已知有N个点落在了一个格子里面，为了简化问题就说是二维情况，然后我们可以很容易的求出这NNN个点的均值μ和协方差矩阵Σ，显然对于二维情况，该协方差矩阵为2×2的矩阵。若这几个点在一条直线上，则容易知道该协方差矩阵的秩为1，因此对应的特征值其中一个为0；而当这几个点近似在一条直线上时，则协方差矩阵的一个特征值会明显大于另外一个。当大致均匀分布时，两个特征值会大小类似。因此，正态分布不仅具有良好的数学性质来描述分布，还能够反映点云的局部形态。

既然正态分布可以反映点云的局部形态，自然可以利用正态分布来表示局部点云，这样就可以利用数学公式来表示一组离散的点云。

用公式表达就是：
NDT算法原理：

1. 将空间（reference scan）划分成各个格子cell

2.将点云投票到各个格子

3.计算格子的正态分布PDF参数

4.将第二幅scan的每个点按转移矩阵T的变换

5.第二幅scan的点落于reference的哪个 格子，计算响应的概率分布函数

6.求所有点的最优值，目标函数为

注意：PDF可以当做表面的近似表达，协方差矩阵的特征向量和特征值可以表达表面信息（朝向、平整度）
格子内少于3个点，经常会协方差矩阵不存在逆矩阵，所以只计算点数大于5的cell，涉及到下采样方法。

原理解释：
NDT算法的第一步就是将参考点云网格化（对于三维地图来说，即使用一个一个小立方体对整个空间的扫描点进行划分），对于每一个网格（cell）,基于网格内的点计算其概率密度函数（probability density function, PDF）,

其中， y⃗ k=1,…,m表示一个网格内所有的扫描点。那么一个网格的概率密度函数则为：

使用正态分布来表示原本离散的点云有诸多好处，这种分块的（通过一个个cell）光滑的表示是连续可导的，每一个概率密度函数可以被认为是一个局部表面（local surface）的近似,它不但描述了这个表面在空间中的位置，同时还包含了这个表面的方向和光滑性等信息。下图是一个3D点云及其网格化效果:

上图中立方体的边长为 1 米，其中越明亮的位置表示概率越高。此外，局部表面的方向和光滑性则可以通过协方差矩阵的特征值和特征向量反映出来。我们以三维的概率密度函数为例，如果三个特征值很接近，那么这个正态分布描述的表面是一个球面，如果一个特征值远大于另外两个特征值，则这个正态分布描述的是一条线，如果一个特征值远小于另外两个特征值，则这个正态分布描述的是一个平面。下图描述了协方差矩阵特征值和表面形状之间的关系：

变换参数和最大似然

当使用NDT配准时，目标是找到当前扫描的姿态，使当前扫描的点位于参考扫描（3D地图）表面上的可能性最大化。那么我们需要优化的参数就是对当前扫描的点云的变换（旋转，平移等），我们使用一个变换参数 p⃗
来描述。当前扫描为一个点云 X={x⃗ 1,…,x⃗ n} ,给定扫描点集合 X 和变换参数 p⃗ ，令空间转换函数 T(p⃗ ,x⃗ k) 表示使用使用姿态变换 p⃗ 来移动点 x⃗ k ,结合之前的一组状态密度函数（每个网格都有一个PDF），那么最好的变换参数 p⃗

应该是最大化似然函数的姿态变换：

那么最大化似然也就相当于最小化负对数似然 −logΘ，

到这里，就到了我们最熟悉的最优化的部分了。现在的任务就是使用优化算法来调整变换参数 p⃗ 来最小化这个负对数似然。NDT算法使用牛顿法进行参数优化。我们不难看出，这里的概率密度函数 f(x⃗ ) 其实并不要求一定是正态分布，任何能够反映扫描表面的结构信息且对异常扫描点具有鲁棒性的概率密度函数都是可以的。