[大白话] 你也可以看懂分支定界加速2D点云SLAM重定位-CSDN博客

本文链接：https://blog.csdn.net/BitSoulLab/article/details/147466491

Hi，这里是比特有灵的第2篇文章。

对于2D SLAM来说，业界已有有多种Scan To Map重定位算法：暴力全量匹配、粒子滤波、特征匹配等，常用的开源SLAM算法有AMCL、Hector slam、Gmapping、KartoSlam、Cartographer等。我们试着用大白话快速理解Cartographer中如何利用分支定界算法进行全局全量匹配加速。

我们先来设计一个小场景：

现在你有一份古代族谱，最顶端只记录着一位先人，其他所有人都是这位先人的子孙后代。这个家族每一个人，都有m个直属子辈后代。现在需要知道，这位先人的第n代子孙中，是否有出生年份小于k的人，如果有，这些人里面最早出生的是哪个？假设这份族谱可以直接查询某个人的出生年份，无需从父辈往下寻找。

不妨花5分钟尝试思考一下思路，如何能最快找到这个人（如果有符合的人选）。

最直接暴力的方法，遍历第n代的所有子孙，然后统计得到所有出生年份小于k的人（如果有）并得到最早的年份，这里需要遍历m的n次方次。

分支定界的做法，其实利用了一个隐含条件：父辈的出生年份，一定小于子辈的出生年份。换言之，如果父辈的出生年份已经大于等于k，那么他的子辈孙辈都不可能小于k，在他子孙辈中的第n代子孙，无需查询。用逻辑来表达，也就是如果我们得知第n - x代的某个人出生年份大于k，那么他的子孙辈中的第n代子孙，无需再查询，也就是这1次的查询，可以帮助我们省略了m的x次方次的查询。这就是分支定界中所谓的“剪枝”（族谱本身就是树状结构）。

如果单纯依靠剪枝，我们可能会得到一批出生年份都小于k的第n代子孙，我们再想办法从里面选出出生年份最小的那一位。那有没有办法再优化上一步的查找呢？我们这个场景可以拆分成2个小的问题，第一个是：是否存在符合条件（出生年份小于k）的第n代子孙？第二个是：如果存在符合第一个问题的人选，那么这批人里面出生年份最小的是谁。上述的“剪枝”只是帮助我们优化第一个问题。我们可以结合第二个问题，一起去优化查询的过程。

当我们寻找到第一个符合条件的第n代子孙后，假设他的出生年份为y，结合第二个问题的需求，“剪枝”的条件其实可以进行调整，从“出生年份小于k”调整为“出生年份小于y”，这样一来，接下来的查询过程中，“出生年份小于k但大于y”的候选人，也就不必进行查询了，这里又节省了一部分的查询。调整“出生年份小于某个值”的这个过程，就是分支定界中所谓的“定界”，通过更新剪枝的条件，进一步优化后续的查询过程。

回到最初的题目，我们在根据2D点云进行scan2map匹配重定位时，如果有某种设计，符合上述的“分支定界”的框架，那么匹配过程就可以非常有效地被优化。我们这里直接给出答案，这个设计是：

把目标地图中所有栅格利用起来，构建多层树状栅格。“子地图”中每个相邻的m*m栅格（m > 1），生成一个“父栅格”，“父栅格”所在的地图就是相对应的“父地图”，“父地图”的分辨率就下降为原来“子地图”的1/(m*m)。注意，此时是逻辑意义上的分辨率，但实际“父地图”地图的分辨率与“子地图”一致，如此一来，在形成树状结构的同时，不会丢失原地图的颗粒度。每个“父地图”中的栅格值，取自于“子地图”中相邻的m*m栅格值中的最大值。如此一来，相同的一帧点云，在某一个坐标(x, y)下，在“父地图”中匹配得到的分数，一定不低于在“子地图”中与(x, y)相邻的m*m个坐标下匹配得到的分数。这里构建了一个类似树状的得分结构。同时，由于一般m取2，所以这个树状结构的每个节点下，都有4个子节点。
预设一个基础匹配得分为k后，对所有符合业务场景的点云姿态进行逐一匹配，目的是找到目标地图那一层下，是否有高于基础匹配得分的点云姿态，如果有，就选取得分最高的那个点云姿态。匹配时，先从顶层“父地图”中开始匹配，若“父地图”中某姿态匹配得到的得分高于k，那么可以往下一层“子地图”中的4个子姿态中继续进行匹配。反之，若“父地图”中某姿态匹配得分已经低于k，那么“子地图”以及“孙地图”等所有的子孙节点，都不需要再匹配了，因为这些姿态进行匹配不可能得到高于k的分数。这个k就是“定界”的值，不再匹配“子地图”下的子孙节点，就是“剪枝”。至于这个k值，可以根据之前匹配到的最佳得分去动态向上更新，如此一来，剪枝的动作可以更迅速。

实际工程中，还有更多的优化空间，比如说：