【cartographer】（二）分支定界

上一篇：cartographer（一）占据栅格地图更新策略

---

分支定界将2D ICP问题转换成了DFS问题. 随着分支层数的增加，ICP的步长越小. 定界的关键是保证父分支的ICP得分都比其子分支的得分要高.

---

1. DFS

直接使用暴力法求解ICP问题无疑复杂度极高，如果我们看过一些ICP求解代码，即可发现其本质是一个先粗后精遍历的问题，这与DFS深度优先搜索很像，这里的深度在ICP中叫做精度，即遍历的步长，下图是一个DFS搜索树示例，

---

2. 分支（Branching rule）

cartographer中设置的DFS树共有8层，分别表示从顶层（depth=0）到底层/叶子（depth=7），树的深度depth每增加1，ICP的步长step就减半，表示查找的分辨率越高. 如果栅格地图分辨率是0.05m（一个栅格的物理长度），step与depth的关系如下：
$$step=2^{7-depth}\ast 0.05$$

可知，树的顶层步长最大（step=0.05*2^7=0.64m），底层步长最小（step=0.05*2^0=0.05m）.

对x和y都进行减半操作，相当于“分田”，在空间坐标上将搜索空间划分为四个更小的区域，

---

3. 预算图（precomputed grids）

预算图又叫膨胀图，从上面我们知道了树的每一层都对应一个ICP步长，根据这个步长，生成一个对应的膨胀图，用于这一层的ICP中，下图是四种不同步长得到膨胀图，

可知，步长越大，膨胀级别越高，反之亦然. 需要注意的是膨胀不会改变栅格图的分辨率，改变的仅仅是栅格图中像素值而已.

---

4. 定界（Computing upper bounds）

precomputed grids这一步得到了几种膨胀级别不同的膨胀图，同样一帧scan数据在膨胀级别越高的栅格图上做ICP的得分肯定是越高的，所以一个node的得分一定是所有的孩子得分的上限.

有了这个前提，在做DFS的时候就可以根据当前已经遍历得到的最高得分与回退分支的得分进行比较，来决定是否需要对这个分支做更深一步的搜索

---

5. 根据激光包络图减小搜索空间（ShrinkToFit）

线性搜索窗口枚举的平移量不应该使所有的scan数据都跑到栅格图像外，这样是没有意义的，而部分跑到栅格图像外是可以接受的，初始的linear_bounds是从(-7m,-7m)到(7m,7m)，对搜索空间进行shrink，如下:

可见，shrink后搜索范围只会小于等于初始的linear_bounds.

---

@leatherwang

二零二零年十二月二十五日

---