The Normal Distributions Transform: A New Approach to Laser Scan Matching

这篇论文一共是十一个部分，第一部分和第二部分对应简介和前人的工作，第三四五主要是介绍了算法的实现细节，分别是正态分布的建立过程、点云的匹配和位姿的优化，之后第六和第七两部分介绍了算法的实际应用，在轨迹跟踪和SLAM中的具体应用，最后八至十一则是一些后续性的内容，包括结果、后续工作、致谢和引用。所以对于整个论文而言，最重要的是算法的细节和算法的应用部分。

论文的翻译可以看下面的链接：
https://blog.csdn.net/sunqin_csdn/article/details/106183711

一、简介与前人的成果

对于移动机器人而言，SLAM是很重要的一个基础，许多的SLAM算法都是基于匹配两次扫描的结果或者去匹配扫描和地图，其中最典型的就是最近点算法，但是这个算法每次迭代都需要寻找最近点，耗时很大，存在不少缺点。论文里面提出的NDT算法，核心是利用正态分布的概率去进行匹配，利用对位姿的不断调整，让两次扫描之间的匹配程度更好。

以前关于匹配的算法中，都是通过建立一个显式的连接，去进行点之间的匹配，所以在点的数量很大的情况下，匹配所需要的时间也会很大，但是NDT算法改变了这个思路，利用概率去进行匹配，从而不存在显式的联系，大大减小了匹配的用时。

二、正态分布的建立

这一部分开始介绍算法的实现细节，对于这个算法而言，需要匹配的两个点云可以是前后帧的关系，也可以是地图与扫描的关系，这里为了方便，统一称为前一帧了。在二维的场景下，先将前一帧划分为许多的二维方格，每个方格的划分也需要根据场景进行判断，方格太小会导致计算量偏大，方格过大则会让匹配的精确度下降。划分后对每个方格做如下的操作：

这三个步骤相当于统计了所有点，计算了平均值和协方差矩阵，计算这两个值的目的是为了建立一个正态分布，这个正态分布的两个参数分别是平均值和协方差矩阵，利用这个正态分布，可以得到点落在这个方框内部的概率值，这里称为NDT值。

结果是一个函数，函数的输入是点，结果是点在方格内的概率，显然，我们的目标就是让所有点在方格内的概率最大，这就是后面的优化目标。

在这一章最后还补充了两点，一方面为了减小离散化的影响，实际的网格划分是按照下面的思路去划分的：

That is, one grid with side length l of a single cell is placed first, then a second one, shifted by l/2 horizontally, a third one, shifted by l/2 vertically and finally a fourth one, shifted by l/2 horizontally and vertically.

也就是按照下面的方式去划分：

实际上每个点都落在了四个区域内，但是在表示时并不体现这些，而是直接将四个概率密度进行整合，最后作为最大的那个方格的概率密度，后面全都是按照这种方式去计算的。
另一方面是关于协方差矩阵的一些问题，在不考虑噪声的场景下，协方差矩阵有时候是不可逆的，因此不能取反，在实际场景下也有可能出现这种情况，所以检查Σ的较小特征值是否至少是较大特征值的0.001倍，如果不是，则将其设为该值。

三、扫描的配准

根据SLAM的一些基础知识，两帧之间的转换可以用下面的公式表示：

其中，φ表示旋转，tx和ty表示平移，x和y表示当前帧的坐标，x‘和y’代表上一帧的坐标，两帧之间的坐标转换可以通过平移旋转进行转换。现在给出两帧的坐标，配准的方式按照下面七步进行：

这七步的解释下面链接的博客有记录：https://blog.csdn.net/weixin_41469272/article/details/105622447?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7Eessearch%7Evector-12.no_search_link&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7Eessearch%7Evector-12.no_search_link

对于这七个步骤，个人的理解就是先转换再检验，首先对上一帧的坐标构建正态分布并且计算出NDT值，之后初始化一个参数，这里的参数实际上就是前面的旋转和平移，这里统称为参数，之后利用初始化的平移和旋转，将第二帧的坐标转换到第一帧去，得到一个在第一帧上对应的坐标，利用上一帧的正态分布去检验当前对应点的匹配情况，我们将第二帧的点投影到了第一帧，将投影的点的信息带入到第一帧的NDT函数里面去，得到的是投影点在这个方格的概率值，概率值加在一起可以看作我们要优化的函数，不断优化参数，也就是调整变换的位姿，让整体的概率值最大，达到收敛的标准后就认为配准成功了。

在计算过程中会用到下面的一些参数：

参数p就是要估计的位姿，可以看见里面实际上是三项，tx和ty就是平移，而φ对应旋转，xi是第二帧中的点，xi’是第二帧对应到第一帧的点，∑i和qi表示的则是平均值和协方差。利用这些，可以得到衡量优秀程度的函数，这里记为score，公式如下：

结合前面的NDT函数，这个score实际上就是在p转换的情况下，所有点在对应方格内的概率值，显然我们的目标是让总的概率值最大，这就涉及后面的优化环节。

四、使用牛顿法进行优化

优化问题可以使用梯度下降法也可以使用牛顿法，论文里面用牛顿法将整个优化的过程写了出来。
一般优化问题都是优化最小值，所以这里我们对score函数加一个负号，也就是变成-score，这样优化的最小值刚好对应score的最大值，根据牛顿法我们可以写出下面的式子：

对（4）式而言，H和g都是输入，具体的表达式是（5）和（6），得到的结果是△P，将这个结果加在P上，相当于score在向着更加优秀的方向移动。（5）式中的f表示的就是要最小化的函数，即-score，它相当于一个雅可比矩阵。（6）式则是一个海森矩阵。

利用这种方式不断调整p的取值，最终得到一个优化的最终结果，这个结果就是两帧之间的一个位姿变化，从而确定了第二帧相对于上一帧的变换方式，相当于进行了配准。

上面的三个式子还有优化的空间，为了更加简洁的表示以及防止混淆，下面用q直接代替xi‘-qi，那么此时的优化函数就变成了：

此时牛顿法里面的值也相应地发生变化，g变成了下面的形式：

而H变成了下面的形式：

其中二阶导的部分为：

所以这一部分实际上是讲解了整个score的优化过程，目标一直都是让总概率值最大，相当于让优化函数最小，优化函数优化的过程采用的是牛顿法，需要计算海森矩阵和雅可比矩阵。论文又对优化函数做了一定的改写，之后给出了改写后的形式。

五、关于算法的个人理解

这里对算法做一个小的简化：

我们将旋转和平移简化为只有平移，而协方差和平均值简化为两个方格内的匹配程度，第一行表示的是原始数据，也就是上一帧的情况，第三行表示变换后的数据，也就是当前帧，红色表示参数p，也就是变换情况，蓝色则表示根据变换逆推的上一帧情况。

简化之后，NDT算法实际上就是利用第三行的数据，寻找一个合适的红色箭头，让根据这个红色箭头导出的蓝色叉与第一行的数据匹配成都最好。人眼直接看的话马上就可以匹配出来是第一个红色箭头，但是计算机不会这样认为，计算机需要一点一点尝试，以第二个红色箭头为例，根据这个箭头产生的上一帧数据是蓝色的部分，将蓝色部分与第一行做比较，可以发现第二个格子虽然都有点，但是点的排布是不同的，所以相性实际上是不足够好的，所以就要换一个方向继续变换红色箭头，变换的方法就是牛顿法，通过牛顿法得到一个变化的方向，向着这个方向移动，最终找到了是第一个箭头的相性最好。从而完成了匹配，找到了最合适的位姿变化，从而也确定了两帧之间的变化关系。

更新于2021.11
目前看了ndt的代码之后，感觉整个ndt就是一个最大似然估计，对于两帧点云，先初始化一个猜测的变换矩阵，再利用这个猜测的变换矩阵，将后一帧投影到上一帧，利用上一帧的概率公式也就是NDT值，得到一个投影到前一帧之后的概率值，让这个概率值尽可能大，从而优化变换矩阵，最后得到两帧点云之间的变换关系。

六、用于轨迹追踪

下面的两部分主要是介绍了NDT算法在实际中的两个应用，一个是用于轨迹追踪，另一个是用于SLAM。

在轨迹追踪中，一个非常重要的概念叫做关键帧，这个关键帧本质上还是一些时刻的扫描数据，但是不同之处在于这些关键帧很具有代表性，所以被挑选出来作为一些代表性的帧，关于关键帧keyframe的详细信息可以看下面的链接：
https://www.cnblogs.com/lihanwen/p/11344315.html

扫描的过程主要按照下面的步骤进行：

利用б表示两帧之间的位置变换，利用这个信息，将k时刻的位置产生一个映射，映射到k-1时刻，利用映射和k-1时刻的正态分布，进行优化算法的执行，最终得到一个优化后的位姿，利用这个优化后的位姿，去更新k时刻的位置，之后判断k时刻的位置与关键帧的位置是否足够的“近”，这个近没有给出明确的定义，网上的资料显示的是这个近可以用变换的位置和角度去计算，总之就是两帧位置的偏离程度，如果偏离程度不大，则进入下一个时刻的计算，否则将关键帧更新为刚刚匹配成功的扫描信息。这样不断重复这个过程，就可以得到位置信息，而且这个位置信息是经过校准的。

七、在SLAM中的应用

在SLAM中定位需要发生一定的改变，但算法使用的依然是前面的三个部分叙述的步骤，所以需要完成的就是将坐标系的转换：

其中T表示的是机器人自身的坐标系，利用全局的变换，得到在全局坐标系中的位置信息，也就是T’。将原本计算过程中的第i次扫描的结果换为T’，雅可比矩阵变为：

海森矩阵中的二次导变成：

之后继续按照前面的算法计算即可。

另外，在SLAM地图中，使用的是图结构去保存地图，图中的点表示的是一个个的关键帧，点之间的边表示的就是两个关键帧之间的位姿关系。地图元素就是2D 点云的正态分布(均值看作2D点云的位置，方差可看作2D点云覆盖范围)，并且这些分布根据每个顶点的全局坐标，也转换到全局坐标下了。
在这种存储方式之下，每次新增一个关键帧，就相当于增加了一个图的节点，理论上所有的节点都需要进行一次全局优化，但是随着关键帧增加，全局优化的顶点越来越多，无法实时计算。因此每次新增顶点，只优化三条连接边以内的子图。优化的公式如下：

其中，Δp为全局参数定义的相对位姿与成对匹配结果定义的相对位姿之间的差。

这一块在SLAM中的应用说实话有些看不懂，主要是在最后这部分的优化函数。个人的理解是在SLAM中的应用主要还是一个地图的构建，在机器人移动过程中实时地去构建地图，每次将实时扫描得到的点云与上一个时刻去做配准，修正之后判断与关键帧的距离，如果认为距离足够近，那么不是关键帧，如果距离已经变得有些大了，那就将这个匹配成功的帧作为新的关键帧，添加到SLAM的图中，新增加一个节点并对三条边以内的子图进行全局优化。相当于每走一部分就更新一下观察点，观察点代表一定范围内的情况，这样每一个关键帧都存在一部分点与上一个关键帧是重合的，在增加关键帧的过程中不断加入新的特征，最终构建出完整的地图。