（每日一读2019.10.22）cartographer论文(Real-time loop closure in 2D LIDAR SLAM)

论文：pdf

自己看了原版论文，但是后来一些内容忘记了，因此翻译一下加上自己的一些理解。

摘要

便携式激光测距仪(也就是LIDAR)以及实时定位和绘图（slam）都是比较有效的一种建立平面图的方法。实时生成和绘制平面图能很好的评估捕获的数据的质量。因此建立一个在有限的资源下可接入的平台是非常有必要的。这篇论文提供了一种在mapping 平台后台使用的方法来实现5cm分辨率的实时绘图和闭环检验。为了达到实时闭环检测，我们使用了分支定界法来计算scan-to-map的匹配作为约束。

I 介绍

建立平面图对于各种应用都是非常有用的。SLAM并不是这篇论文所主要的研究，这篇论文的contribution在于减少了计算量，提高了计算效率，对于大平面图更加适用。

II 相关工作

1. scan-to-scan matching：会累计误差，最终误差会很大
2. scan-to-map matching：减少了误差的累计（本篇论文使用方法）
3. pixel-accurate scan matching：最后有说这个算法在后台用于将scan点集和最近的submap进行匹配，生成loop closing的约束条件。

解决局部误差的累计有两种方法：

1. 一种是粒子滤波法，
2. 一个基于图的SLAM方法（graph-based）。

这篇文章使用的是后者。讲一下关于graph-based的吧。

graph-based是一种工作于基于位姿和特征的集合的方法。图中的边表示从观察结果中生成的约束，节点表示的是位姿和特征。对于优化由约束引进的误差有很多方法，比如文中的参考文献11，12。对于室外绘图的系统使用详情见参考论文13。
对于第二部分也就不多解释了，接下来重点来了！第三，四，五部分是这篇论文的核心，基本的理论知识阐述！非常重要。

III 系统概述

谷歌的cartographer提供了一个室内地图的实时解决方案，可以生成分辨率为r=5cm的二维栅格地图。系统的操作员可以在穿过建筑物时看到正在创建的地图。

经本人实验确实可以达到上述的精度和效果

激光扫描被插入到一个最佳估计位置的submap中，该位置被认为在短时间内足够精确。扫描匹配发生在最近的子地图上，因此它只依赖于最近的扫描，对于位姿的估计误差会在全局的帧当中积累起来。

累计误差确实存在

综合硬件等条件，这篇论文没有使用粒子滤波，使用的是pose optimization来处理误差累计。

（如何使用pose optimization实现累计误差的优化???接下来讲的是关于位姿优化算法？？）

当一个子图构建完成之后，不在有其它的scan插入这个子图当中，这个已有的子图会用来作为loop closing的scan matching。所有子图和scan点集都会被用来进行loop closure。
当一个新的laser scan加入到地图中时，如果该laser scan的估计位姿与地图中某个submap的某个laser scan的位姿比较接近的话，那么通过某种 scan match策略就会找到该闭环 。

匹配的结果？指的是什么？也就是需要存储什么？

每隔几秒就进行闭环检测，按照经验来说，就是一个位置点被重复访问之后就算达到闭环了.

（这个重复访问是如何判断的，两个位姿的距离足够小？？）

这样的判断要求闭环检测必须在新的点被加入到submap中之前完成。如果不是，可能会导致失败.为了达到这样的目的（之前提高的必选检测在前的目的？？）,通过使用分支上界法以及对于每个完成的submap有对应的几个预处理的网格（several precomputed grids per finished submap）.

IV 局部二维的实时定位和绘图

我们的系统结合了单独的局部和全局方法来实现二维slam。这两种方法都优化了由(x，y)平动和转动激光雷达观测（又称扫描）组成的姿态，$ζ=(ζx，ζy，ζθ)$。在一个不稳定的平台上，IMU被用来估计重力方向，以便从水平安装的激光雷达向二维世界投射扫描。

非线性优化的submap匹配：每个连续的点集被拿来和整个地图的一部分进行匹配，就是和submap进行匹配。使用一种非线性的优化方法将submap和scan点集联系起来，这也是scan matching的过程。局部方法积累的误差在之后的第五部分的全局方法去掉。

A scans

submap构造是反复对齐扫描和submap座标帧（也称为帧）的迭代过程。当扫描原点为$0∈r2$时，我们现在将有关扫描点的信息写成$h={h k}k=1,…,k,hk∈r2$。submap帧中扫描帧的位姿$ζ$表示为变换$t_ζ$，它将扫描帧中的扫描点严格变换为submap帧，定义为

B submaps

一个submap是由连续的几个scans建立的，这些submap采用的是概率网格$M : r_Z × r_Z →[p_{min}, p_{max} ]$，这个代表着离散网格点中概率值。这个值可以作为表示这个网格是否为障碍物的概率。对于每个网格点，论文中都定义了一个相应的像素，这个像素包含了所有靠近这个网格点的scan points。

无论什么时候一个scan点集被插入到这个概率网格中，一个包含网格点的hits集合以及一个miss集合都会被重新计算。对于每一次的hit，我们将这个最近的网格点加入hit集合，对于每一次的miss，我们将每个像素关联的网格插入这个miss集合，对于已经在hit集合中的网格点不需要插入。
对于每一个之前没有观察到的网格点都会根据他们所在集合是hit还是miss赋予一个概率值$p_{hit} or p_{miss}$。
如果一个网格点已经被观察到了，我们将会更新这个概率值。通过以下的方式（hit的修改）：

对于miss的网格点的修改也是同样的。

灰色的网格代表的是一次scan点集的范围，黑色的点表示hit（是障碍物的）的网格。

C Ceres scan matching

在将一个scan点集插入submap中之前，这个scan点集的位置必须使用Ceres-based的scan matcher的方法进行优化（这个优化是相对于submap的位置）。这个scan matcher目的是为了找到一个点集位置，这个点集的位置在submap中的概率最大，将其作为一个非线性最小二乘问题。如下：

M这个函数采用了双三次插值法。这个函数的数学优化往往比网格分辨率优化有更高的准确性，因为这是一个局部的优化，所以一个好的初值很重要。IMU被用来测量scan match中的角度旋转参数theta。一个高概率的scan matcher 或者一个高的pixel-accurate scan matching方法会被用来弥补IMU不能使用的情况，尽管复杂度很高。

V 闭环的实现

因为一个submap是有少数的几个连续scan点集生成的，所以误差很小。这篇论文中优化所有点集和submap位置的方法使用的是Sparse Pose Adjustment（参见原论文的[2]）。一个scan点集呗插入submap中的这个相对位置会被存储下来，用于之后的闭环优化。除了这些位置信息，还有的包含scan点集和submap，而且这个submap不再变化的时候，都会被用来作为闭环检测。一个scan matcher一直会在后台不断的运行，当一个好的scan match被找到之后，这个相应的位置也会被加入到优化问题中。

A 优化优化问题

闭环的优化，就像scan matching 一样，也被表示成非线性最小二乘问题。每隔几秒，就会使用Ceres计算一个解决方法。计算方式如下：

损失函数，比如Huber loss等等。使用损失函数的目的是减少加入到优化问题中的离群点对于系统的影响。

B Branch-and-bound scan matching

对于优化的，pixel-accurate match还是非常感兴趣的。这个匹配算法公式如下：找到最佳的位置点:

其中的W表示的是一个窗口大小，M是之前M函数一个延伸。这里的含义以及包括接下来的算法1的含义，在我理解看来就是在一个新的scan点集的周围画一个圆（窗口），然后不断的修改x，y，以及角度来找一个最佳的位置。这个位置的修改不能超过一定的范围。这个范围是点集的最大范围。同时角度变化step也设置如下：

计算了覆盖给定线性和角度搜索窗口大小的整数步数.
比如$W_x=W_y=7m，且W_\theta=30度$。

我们可以得到一个基于估计值$ζ_0$为中心的一个w集合的搜索窗口。

一个简单的算法来寻找$ζ^*$可以很容易地公式化，见算法1，但是对于搜索窗口的大小，我们认为它太慢了。

我们说了半天，但是作者直接在论文里用了一个instead单词，说最后没有采用这个办法，而是采用了分支定界方法来寻找最佳位置。

这个算法的主要思想是将可能性子集表示为树中的节点，其中根节点表示所有可能的解决方案，在我们的例子中是$W$。每个节点的子节点构成其父节点的一个分区，因此它们一起表示相同的可能性集。叶节点是单变量的；每个节点代表一个可行解。只要内部节点C的$score(c)$是其元素得分的上界，它就提供了与朴素方法相同的解决方案。在这种情况下，每当节点有界时，在这个子树中不存在比迄今为止已知的解还要好的解。
为了得到一个具体的算法，我们必须确定节点选择、分支和上界计算的方法。

分支上界法就是每个分支代表一种可能。使用DFS找到最佳的那个位置即可。他和算法1一样都是解决的相同的问题。只不过这个节点的score是可能的最大值而已。为了达到一个实际的算法，需要分为以下几步：节点选择，分支，计算上限。对于这三步，论文中有具体讲解。节点选择采用的是DFS，对于分支算法，采用的是算法2。算法3是将节点选择和分支结合到一起之后的算法。

• 1）节点选择：在没有更好的选择的情况下，我们的算法使用深度优先搜索（DFS）作为默认选择：算法的效率取决于被修剪的大部分树。这取决于两件事：一个好的上限，一个好的当前解决方案。后一部分由dfs提供帮助，它可以快速评估许多叶节点。由于我们不想将差的匹配添加为回环检测约束，因此我们还引入了一个分数阈值，低于该阈值，我们对最优解不感兴趣。由于在实践中，阈值通常不会被超过，这就降低了节点选择或寻找初始启发式解决方案的重要性。对于dfs中访问子节点的顺序，我们计算每个子节点得分的上界，先访问最有前途的子节点，然后访问最大的子节点。这种方法是算法3。

• 2）分支规则：树中的每个节点由一个整数$c=(c_x,c_y,c_θ,c_h)$的元组来描述。高度为$c_h$的节点组合最多$2 c_h×2 c_h$可能的平移，但表示特定的旋转：叶节点具有高度$c_h=0$，并且对应于可行解$w 3ζ_c=ζ_0+(rc_x，rc_y，δ_θc_θ)$。
  在我们的算法3的公式中，包含所有可行解的根节点没有显式地出现，并在覆盖搜索窗口的固定高度$h_0$分支成一组初始节点$c_0$
  
  在$c_h>1$的给定节点c处，我们将分支到至多四个子节点$c_{h-1}$
  

• 3)计算上界：计算内部节点上界是在计算工作量方面还是在定界质量方面都是非常重要的。我们用
  
  为了能够有效地计算最大值，我们使用预先计算的网格$M^{c_h}_{precomp}$。预先计算每个可能高度的一个网格$c_h$允许我们用$effor$线性计算扫描点数的分数。请注意，为了能够做到这一点，我们还计算最大WC，可以大于我们的搜索空间边界附近的WC。
  
  对于叶节点，与前面一样使用$ζ_c$。注意，$M^H_{PrimCMP}$具有与$M^n_{Enm}$相同的像素结构，但在每个像素中存储$2 h×2 h$像素的值的最大值。图3给出了这种预计算网格的一个例子。
  
  为了使构建预计算网格的计算工作量保持在较低的水平，我们等待一个概率网格不再接收到进一步的更新,然后我们计算一组预计算的网格，并开始与之匹配。
  对于每个预先计算的网格，我们计算从每个像素开始的2小时像素宽行的最大值。使用这个中间结果，然后构建下一个预计算网格。
  如果按已添加的顺序去除值，则可改变的值集合的最大值可在已摊销的$O(1)$中保持最新。连续最大值被保存在一个DEGO中，它可以递归地定义为包含当前在集合中的所有值的最大值，接着是最大值的第一次出现之后的所有值的连续最大值的列表。对于空值集合，此列表为空。使用这种方法，可以在$O(n)$中计算预计算的网格。
  其中$n$是每个预计算网格中的像素数。计算上界的另一种方法是计算低分辨率概率网格，将分辨率连续减半，见[1]。由于我们的方法的额外内存消耗是可以接受的，所以我们更喜欢使用较低分辨率的概率网格，这会导致比（15）更差的边界，从而对性能产生负面影响。(牺牲空间和部分精度节约时间)

VI 实验

VII 总结

这篇论文阐述了一个2D的SLAM的系统，这个系统采用了闭环检测的scan-to-submap matching，同时还有图优化（graph optimization）。一个submap的创建是使用的是局部的，基于网格的（grid-based）SLAM方法。在后台，所有的点集与相近的submap的匹配使用的是pixel-accurate scan matching的方法，然后建立闭环检测的约束。这个约束图（基于submap和scan pose的）都会周期性的在后台被更改。这个操作是采用GPU加速将已完成的submap和当前的submap进行结合。