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

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，结