相关性扫描匹配CSM与分支限界加速

前言

在激光SLAM中，相关性扫描匹配可是重点中的重点，而分支限界加速更是大大提高了它的实时性。本文就来详细解读一下其中的奥秘。

激光SLAM前端

既然是SLAM，那么通常都会分为前端和后端。前端充当里程计的角色，比如我们熟知的VO（视觉里程计）和VIO（视觉惯性里程计）。激光SLAM也需要前端，对于每一帧点云，通常是使用所谓的扫描匹配（scan-match）方法进行位姿估计。

简单地说，扫描匹配就是想办法把两帧点云对齐，对齐过程中的旋转和平移就是两帧之间的相对位姿。这是最最简单的方式，实际中，为了提高鲁棒性，通常把当前帧点云和地图进行扫描匹配，更不容易出错。

扫描匹配的实现方式有两种，第一种是相关性扫描匹配，第二种是基于优化的扫描匹配。第一种思路简单，但有一些复杂的细节，而第二种则是构建非线性最小二乘优化问题并求解。实际中，通常是两种方法同时用，先用相关性扫描匹配求初值，再用基于优化的扫描匹配进一步优化。

相关性扫描匹配

相关性扫描匹配的思路其实非常简单。想象给定一个栅格地图，再给定当前帧的点云，怎样才能知道激光雷达所在的位置呢？最简单的办法，把激光雷达放在地图的每个格子上，看在这个位置时，点云是否与地图重合，重合程度最高的位置就是激光雷达的真实位姿。

显然，这个方法就是暴力搜索。虽然肯定搜得出来，但速度比较慢。想要加速，就得祭出我们的算法神器了——分支限界。

分支限界加速

如果上过计算机算法课，就一定接触过分支限界法。这是一个通用的算法，一般用于搜索离散空间的最优解。我们这里的解空间显然就是一个离散的空间，而且可以很方便地按照地图分辨率构造树形结构。

举例来说，假设有一张4×4的地图，我们降低其分辨率，每2×2的格子合并成一个，得到一张2×2的地图，再降低其分辨率，得到一张1×1的地图。这样，地图被分成了三层，如下图所示。

有了多分辨率地图后，我们先在第二层的解空间中搜索，找到最优的位姿对应的格子。然后再在该格子中搜索下一层中对应的小格子，找到最优的位姿。

这么简单？当然不是。请注意，上面的做法是完全错误的。它有两个问题。首先，在第二层的格子中搜索最优的位姿，实际上是把位姿放在格子的中心，计算其点云的评分（即hit点的评分之和，下文直接称其为位姿的评分）。但是格子中心的位姿并不能代表格子其它位置的位姿，很有可能格子中心的评分低于其它位置的评分，那么第二层最优的格子中并不一定包含第三层最优的格子。怎么办呢，我们可以想办法让格子中心的评分高于其所有子格子的评分，此时格子的评分是其子格子的评分上界，这样就可以保证子格子的最高评分体现在父格子中。

但是，此时又引入了第二个问题，虽然格子的评分是其子格子的评分上界，但并非上确界。也就是说，可能存在格子的评分很高，但其所有子格子的评分都很低的情况。这样的话，我们仍然无法选取最大评分格子的同时抛弃其它格子。

剩下的选择就只有一个，绝不抛弃任何一个可能存在最优解的格子，只抛弃那些绝对不存在最优解的格子。这正是分支限界的思想。

具体实现的时候，先从根节点开始遍历，把它拆分成4个子格子。这4个子格子分别计算评分，并由高到低排序。从中选出分数最高的格子，进一步拆分成4个子格子。假设这4个子格子已经到了叶子节点，也就是达到了真实地图的分辨率。此时计算出的4个子格子的评分代表了真实的位姿评分（只有叶子节点的评分是真实评分，其它格子的评分都是上界），找出其最大值，记作best_score。以上过程，我们体会了分支是如何实现的。接下来，该轮到限界出场了。第二层的格子还有3个未曾探索，我们当然是选择最大的那个开始分支。但别急，先看一下它的评分是否大于best_score，如果是，继续分支，如果否，就可以直接剪枝了，抛弃这个格子及其所有子格子。因为格子的评分代表了其子格子评分的上界，如果上界都小于best_score，就不可能再有子格子的评分大于它了。按照如此方法，大刀阔斧地剪枝即可。注意，当遇到评分大于best_score的叶子节点时，记得更新best_score，这样可以更快地缩小搜索空间。

讲到这里，你是不是有点摸不着头脑？别怕，因为这里有个最最关键的问题我还没有解释，也就是如何能够快速计算出格子评分的上界。

计算评分上界

先说一个平凡方法。既然要算子格子评分的上界，那就把每一个子格子拿出来算一遍，求个最大值。对了，子格子还会有子格子，所以这是个递归运算，直到把叶子节点都算出来才行。那这跟直接暴力搜索就没什么两样了，不行不行。

我们还是得追求在O(1)时间复杂度内求出格子的评分。想了想，之所以叶子节点评分可以在O(1)时间内求出，是因为它是直接在地图中查表得到的（严谨地说，是通过查其所有hit点的评分再求和得到的，但由于hit点数量固定，可以认为时间复杂度是O(1)）。但我们构造的多分辨率地图能不能也维护类似的概率表格，使得格子的评分可以在对应分辨率地图中查询得到呢？

多么机智的想法啊！试想，把地图中每个格子的概率用其附近区域内的最大概率代替。这样的话，直接查对应分辨率地图就可以得到格子的评分，因为这个评分已经事先在附近区域中取了最大值。

上面提到的“某个范围”，需要与地图分辨率保持一致。也就是说，越低分辨率的地图（也就是越高层的地图），应该在越大的区域求最大值，其大小与当前分辨率下格子的大小一致。

不同分辨率的地图需要事先计算好，大概长这个样子：

可以看到，越靠后的地图分辨率越低，格子中的概率越接近1，也就代表了越高的上限。这与我们的直观理解是一致的。

总结

说实话，本文内容非常抽象，又限于笔者画图能力太差，实在画不出合适的插图，只能靠读者自己想象了。

不得不说，除非事先有所了解，否则读了之后可能还是一头雾水。所以，如果你对激光SLAM还没有整体的了解，还不知道什么是点云、什么是地图、如何计算概率。那么请关注一下文末的参考资料，相信会对你有所帮助。

相关性扫描匹配及分枝限界加速在谷歌Cartographer的论文中有详细介绍，建议大家去看一看。

参考资料

Real-Time Loop Closure in 2D LIDAR SLAM Wolfgang Hess, Damon Kohler, Holger Rapp, Daniel Andor