对cartographer源论文《Real-Time Loop Closure in 2D LIDAR SLAM》的理解

论文所讲内容主要分为三部分：子图构建、闭环检测以及后端优化。

1子图（submap）的构建

  每个子图都由一定数量的扫描点集（scan）组成，每个扫描点集在插入子图中时，都会先寻找在该子图中的最优位姿，进而插入。（道听途说：为了避免子图中插入过多的扫描点集，对于新的扫描点集，计算当前位姿与上次位姿的变化量，当位姿的变化量大于某个距离、角度或时间时，才将扫描点集插入，否则删除，这部分参考源码中的motion_filter）。

  在论文中子图是一个分辨率为5cm的栅格地图，栅格值代表被击中的概率。其中，概率更新的方式如下。

  当位姿为ξ时，扫描点集合中的某个点p转换到子图中的公式为：

  接下来记扫描点集H={hk},k=1,…,K，我们计算扫描点集在子图中的最优位姿。

  上式中，Msmooth为双三次插值（bicubic interpolation）函数。对点a进行双三次插值的过程为：取a邻域内的16个点，分别将16个点与a点的距离带入bicubic函数得到各自的对应的权值，16个点自身的值乘以对应权值后的累加和即为a点的插值结果。如果扫描点集的端点根据某个位姿映射到子图中后，端点所对应的栅格越多的是障碍栅格，则上式的值越小。可以根据高斯牛顿法求解一个最优位姿（某个栅格，某个主方向），在该位姿处上式取得最小值。

2闭环检测

  每隔一段时间进行闭环检测，对于新来的扫描点集，根据传感器数据得到的初始位姿，设定邻域作为搜索空间。在搜索空间内查找一个最优位姿，扫描点集在该位姿下的得分最高（得分越高说明两者的环境越相似），若最高得分超过阈值，则找到闭环。因此，如果遍历搜索空间，计算扫描点集在每个位姿下的得分，得到最高得分即为最优位姿。这是一种暴力的解法，能得到结果，但是计算量太大，因此文中使用了分支定界法去搜索最优位姿。分支定界法在这里的应用主要分成两部分：搜索流程、结点得分计算。

2.1搜索流程—分支定界法

  （1）将搜索空间按步长划分成多个子空间，记每个空间为一个结点，则搜索空间为根节点，每一次划分即为该结点构建孩子结点。每一次划分，树高增加1，层数越高的结点，其对应空间越小。如下图所示为步长的设定方法。

  （2）遍历刚划分出的孩子结点，计算每个结点的得分，按从大到小排序；
  （3）对第一个结点（得分最大的结点），若其得分大于best_score（初始值为阈值），则将其划分得到四个孩子结点，计算得分并按从大到小排序，否则转到（5）；
  （4）重复（3），直到结点代表的区域大小为基础栅格大小（5cm），即得到最底层的四个结点，计算它们的得分，若最大得分a大于best_score，则更新best_score=a，否则就转到（5）；
  （5）返回上一层，比较best_score与兄弟结点的得分，若某个兄弟结点的得分更高，则将该兄弟划分成四个子结点再转到（4），否则转到（5），直到返回到根节点；
  （6）返回best_score和所对应的结点（若存在的话）。
也许看伪代码会更清楚一些，论文中给出了伪代码流程如下。

2.2结点得分计算

  将扫描点集按位姿ξ映射到栅格地图中，所有激光端点所落栅格的值的和即为该位姿下的得分。但是一个结点是一个区域范围，包含有多个位姿，如何计算一个结点的得分呢？暴力的方法是计算区域内每一个位姿下的得分，记其中最大的为区域得分。不过这样做分支定界就没有意义，还是把搜索空间内的所有位姿都求解了个遍。论文中求解结点c的得分的公式如下。

  其具体含义为，对于要计算得分的结点，根据结点所处的树深h，定义一个窗口；遍历栅格地图中的每一个栅格，窗口以当前遍历的栅格为左上角，查找窗口中的最大值，赋值给当前栅格。由此我们可以得到一个新的栅格地图gridMapTemp。取结点内的任意一个位姿，将扫描点集进行映射，所有激光端点所落栅格的值的和即为结点得分。图解如下。

  上图中红框为待求得分的结点，篮框表示对左上角的栅格查找窗口下的最大值，带有黑色端点的线表示映射到栅格地图中的扫描点集所形成的光路。上述处理相当于将障碍物变厚，本来只能在某一个位姿下才能得到最高分，但修改过栅格值后的新地图中，只要在结点范围内的位姿，均能得到最高分。

3后端优化

  论文中这部分讲的没看懂。