NDT匹配: The Normal Distributions Transform: A New Approach to Laser Scan

介绍

大多数激光匹配算法都是基于点或者线的特征匹配，该论文提出一种2D激光扫描匹配算法，方法类似于占据栅格，将2D平面分为一个个cell，对于每个cell，设定其一个正态分布，表示该网格测量到每个点的概率。则前后两帧激光转化为一些分段连续（可微）概率密度，通过牛顿法进行匹配，因此不需要建立任何点线对应。该算法在室内环境即使没有里程计数据也能表现很好。前后帧相互匹配转换为最大化前后帧对应点概率密度之和。作者认为该算法的最大特点在于不需要建立对应点的匹配。

NDT构建：

将2D空间分为一个个cell，cell大小为常数，每个cell至少包含三个点，计算cell中点的均值、方差，则在该cell内测量到一个点x的概率可以用公式(1)中的正态分布表示。则该分布也表示cell内每个位置被测量到的概率，该论文中cell的大小为100*100。则整个2D平面都可以用分段连续可微的概率密度形式表示。

有两点需要注意：为了降低离散化的影响（即密度函数是以cell内的若干个点表示的），作者采用了四个有重叠的网格：\(l*l\)、\(l*\frac{l}{2}\)、\(\frac{l}{2}*l\)、\(\frac{l}{2}*\frac{l}{2}\)。每个点其实只被算一次，计算时算上四个网格，密度为其之和。其次，对于没有噪声，协方差矩阵将会变成奇异的，导致不可逆，检验协方差较小的特征值是否不小于较大特征值的0.001倍。

scan对齐

机器人两帧之间的位姿变换由公式(2)给出，二维的比较简单，就是一个旋转和平移。扫描对齐的目的就是算出两帧之间的相对位姿，算法流程：

1.构建第一帧的NDT 2.初始化估计参数（设为0或者使用里程计数据） 3.对于第二帧激光数据，将每个点根据初始化的参数投影到第一帧坐标系下 4.确定每个点的NDT对应关系（第二帧每个点投影到哪个格子，再将投影后的点带到这个格子的正态分布，得到一个概率） 5.参数的评分为对每个地图点分布评估之和。 6. 通过优化评分得到新的参数，这里使用牛顿法 7.返回第三步直到收敛。设估计的向量为\(p\)，即二维位姿。\(x_i\)为第二帧的激光点，\(x_i^{'}\)为\(x_i\)变换到第一帧对应的点，而\(\Sigma_i\)和\(q_i\)为\(x_ i^{'}\)所在NDT的均值和方差。则评分score公式为(3)。

牛顿法优化

由于优化问题一般为最小化问题，因此这里取-score，通过牛顿法迭代寻找\(p\)，即公式(4)

\[H\Delta p = -g
\]

其中\(g\)为\(f\)对\(p_i\)的偏导，\(H_{ij}\)为\(f\)对\(p_i\)和\(p_j\)的偏导。更新公式\(p=p+\delta p\)。其中\(H\)被要求为正定的，如果不是则设定\(H^{'}=H+\lambda I\)。这里的\(f\)即为评分，即为公式3。具体的偏导求解见公式(8)-(13)。计算代价即为求解雅克比矩阵和hessian矩阵。每个点只求一次指数，三角函数仅取决于\(\phi\)，每次迭代只用求一次。

位置追踪

位置追踪时定义了关键帧，和我们理解的关键帧应该是一样的，追踪是追踪关键帧，关键帧之间距离不要太近。也是不断迭代，直到和上一关键帧有一定距离，则设定新的关键帧。这里判断远近通过两帧之间的旋转平移量的大小。

应用到SLAM

之前帧间匹配求解的是相邻两帧的位姿变化，SLAM则需要估计每一帧中机器人的位姿，\(R_i\)和\(T_i\)表示激光帧\(i\)在世界中的位姿（我理解是上一帧的位姿，这里应该是上一步求了），而当前帧的机器人位姿则为\(R\)和\(T\)，这里要估计的是\(R\)和\(T\)。则将当前帧的坐标映射到上一帧再转换到世界坐标系下：公式(14)。

由于加入了\(R_i\)和\(T_i\)，因此雅克比矩阵应该是变复杂了，具体见公式(15)和公式(16)。注意雅克比矩阵和hessian矩阵应该将四个与有重叠的区域加和，但是作者发现这四块区域对最后的导数矩阵求解的影响是独立的，但是会相互影响寻找最优解。

如果当前帧的激光和地图的重合部分太少，则地图需要加入最新的关键帧。类似于orbslam中的共视图，每个关键帧由一个节点表示，节点之间存在边则表示这两帧有一个成对匹配。每加入一个关键帧则需要优化地图，即优化所有关键帧的参数。成对匹配的帧定义一个二次误差模型：\(\Delta p\)表示分别由全局参数和成对匹配定义相对位姿的差异。则评分公式更新为公式(17)。该式通过在\(\Delta p=0\)处进行二次泰勒展开，由于在极值点处展开，一次项为0。此时分数为所有边的上的和。

如果地图太大达不到实时的要求，则维护一个子图，子图中的帧与新的帧之间距离不超过三条边。仅仅优化子图中的关键帧。但如果存在闭环，则优化所有关键帧。

问题：

四个网格那里没看懂

协方差矩阵为什么变奇异

为什么搞帧之间的配对以及重新定义score