paper： Real-Time Loop Closure in 2D LIDAR SLAM
code: link

1. 摘要

便携式激光测距仪，以下简称为LIDAR，以及同时定位与地图构建（SLAM）是获取现有楼层平面图的有效方法。实时生成和可视化楼层平面图有助于操作员评估捕获数据的质量和覆盖范围。构建一个便携式捕获平台需要在有限的计算资源下运行。我们介绍了在我们的背包式测绘平台上使用的方法，该方法实现了5厘米分辨率的实时映射和闭环检测。为了实现实时闭环检测，我们采用了一种分支定界方法来计算扫描与子地图匹配作为约束。我们提供了实验结果和与其他知名方法的比较，这些结果表明，就质量而言，我们的方法与已建立的技术具有竞争力。
Cartographer是Google推出的一套基于图优化的激光SLAM算法，它同时支持2D和3D激光SLAM，可以跨平台使用，支持Lidar、IMU、Odemetry、GPS、Landmark等多种传感器配置。是目前落地应用最广泛的激光SLAM算法之一。

背景：
论文中主要介绍了前后帧激光数据扫描以及后处理去除误差的相关工作，主要分为：

观测数据匹配，主要包括以下三类方法：
Scan-to-Scan: 使用前后帧激光扫描进行匹配求解相对位移，这种相对简单且直观，但定位误差会迅速累计，效果不好
Scan-to-Map：将激光数据与局部地图做匹配，使用高斯牛顿等方法可以有效控制累计误差
Pixel-accurate scan matching：可以进一步减少局部误差累计，计算量相对而言更高
进一步消除累计误差的方法有两种：

粒子滤波：需要在每个粒子中维护整个地图系统的装啊提，因此消耗资料会迅速增加
基于图的方法：图优化的方法主要是建立在一系列用于表示位姿和特征的节点，图的边则是由观测误差生成

2. Cartographer系统数据处理流程

架构图参考blog

2.1. 数据获取（Input Sensor Data）

SLAM系统的数据获取阶段涉及多个传感器的输入，包括激光雷达数据、底盘odom数据和imu数据。

• 激光雷达数据：

  • 2D扫描点云原始数据经过体素滤波器（Voxel Filter）处理，然后通过自适应体素滤波器，最终用于扫描匹配（Scan Matching）。

• 底盘Odom数据：

  • Odom位置信息（包含x, y, θ）首先传递给姿态外推器（PoseExtrapolator），然后用于扫描匹配。

• IMU数据：

  • IMU数据（包含两个方向的线加速度和角速度）经过预处理（ImuTracker）后，传递给姿态外推器，最后用于扫描匹配。

2.2 姿态外推器（PoseExtrapolator）

姿态外推器的作用是为扫描匹配提供初始位姿估计。

• 输入：Odom、IMU数据和上一次前端匹配完成的last_pose。
• 输出：下一次前端匹配的初始pose。

处理思路：

• 通过计算odom和old_odom的（x, y, θ）增量得到速度。
• IMU预积分得到速度、位移和角度变化。
• 通过last_pose和old_pose计算（x, y, θ）增量得到加速度和角速度。
• 估算下一次前端匹配的初始pose，优先使用IMU数据估算角度变化，使用Odom数据估算位移变化。如果没有这些数据，则使用last_pose计算出的数据。

2.3 局部建图（Local SLAM）

局部建图阶段涉及外推器初始pose、扫描匹配、运动滤波器（Motion Filter）和子图（submap）的插入。

• 扫描匹配（Scan Matching）：
  • 计算当前时刻相对于之前的一段时间所应存在的最优位姿。
  • 输入：外推器初始pose。
  • 输出：扫描匹配最优位姿。
  • 处理思路：通过非线性优化问题建立最小二乘问题，使用Ceres优化求解。

子图（Submap）：

• 一个submap由几个连续的scans创建，由5cm×5cm大小的概率栅格[Pmin, Pmax]构造而成。
• Submap在创建完成时，栅格概率小于Pmin表示该点无障碍，介于Pmin与Pmax之间表示未知，大于Pmax表示该点有障碍。
• 每一帧的scans都会生成一组称为hits的栅格点和一组称为misses的栅格点。
• Cartographer通过概率整数化和对数化优化概率表示，以提高运算性能和速度。

3. 关键模块实现

3.1 局部地图Submap

一个 submap是通过几个连续的scans创建而成的，由5cm*5cm大小的概率栅格[Pmin,Pmax]构造而成，submap在创建完成时，栅格概率小于Pmin表示该点无障碍，在Pmin与Pmax之间表示未知，大于Pmax表示该点有障碍。 每一帧的scans都会生成一组称为hits的栅格点和一组称为misses的栅格点 。其submap为栅格地图，可以认为submap为一个2维数组，其每个索引下的value值表示概率。为提高运算性能和速度，cartographer将概率表示进行优化，包括概率整数化和对数化。

Cartographer的局部SLAM中估计每一帧激光扫描数据对应的位姿 $\xi =\left({\xi }_x,{\xi }_y,{\xi }_{\theta }\right)$，包括平移分量 $(x,y)$ 和转向 ${\xi }_{\theta }$。在不稳定的平台上还会使用IMU来估计重力的指向，以将水平放置的激光扫描数据投影到2D世界平面上。下面分别从扫描、局部地图、扫描匹配来介绍这个过程。

•扫描(Scan)

扫描是构建局部地图的基本单位。局部地图的构建就是重复的将扫描（也就是帧）匹配至局部地图的迭代过程。由于是2D SLAM，作以下约定：扫描的原点在 $0\in {\mathbb{R}}^2$、激光扫描点的位置信息（在lidar坐标系下）为 H = { h k } k = 1 , … , L H=\{h_{k}\}_{k=1,\ldots,L} H={hk​}k=1,…,L​，$h_k\in {\mathbb{R}}^2$，扫描帧中的激光点在局部地图的位姿 $\xi$ 用 $T_{\xi }$ 来表示，因此将激光点在扫描坐标系下的坐标转换为局部地图坐标系下的坐标的过程为：

解释
$$T_{\xi }p=\left[\begin{array}{cc}\cos {\xi }_{\theta }& -\sin {\xi }_{\theta }\\ \sin {\xi }_{\theta }& \cos {\xi }_{\theta }\end{array}\right]p+\left[\begin{array}{c}{\xi }_x\\ {\xi }_y\end{array}\right]$$

•局部地图(Submap)

局部地图由一部分帧组成，并构建一个二维概率栅格，将一定分辨率 r 的离散栅格点映射到一个概率值，即：$M:r\mathbb{Z}\times r\mathbb{Z}\to [p_{\min },p_{\max }]$其中 概率值表示该栅格点为一个障碍物的概率，对于每一个栅格点，用一个对应的 网格像素来表示，其包含所有离该栅格点最近的点。

每当一帧扫描数据插入概率栅格地图中，首先要计算两个集合： 激光点击中的栅格集合（hits）和经过的栅格集合（misses）。
对于每一束激光束做以下处理：每一个激光点击中的位置，将其最近的栅格插入击中集合中，而对其经过（从激光束原点到击中位置）的栅格将将其经过的栅格（除了已经在当次击中集合的）放入经过集合中。

接下来进行栅格概率值的更新：对应之前没有观测过的栅格点直接分配一个击中/经过对应的概率 $p_{\text{hit}}$ 或 $p_{\text{miss}}$。如果栅格之前有观测过的话则根据下列式子进行更新：

$$\begin{array}{rl}\text{odds}(p)& =\frac{p}{1-p}\\ M_{\text{new}}(x)& =\text{clamp}({\text{odds}}^{-1}(\text{odds}(M_{\text{old}}(x))\cdot \text{odds}(p_{\text{hits}})))\end{array}$$
其中阴影带叉的表示hits,阴影不带叉的表示misses,每个hits中的栅格点被赋予初值Phits，每个misses中的栅格点被称为Pmis,如果该栅格点在先前已经有p值，则用下述对该栅格点的值进行更新. 其中clamp是区间限定函数。

扫描匹配(Scan matching)

在将扫描数据插入局部地图之前，首先需要用基于Ceres的优化器来将该扫描数据对应的位姿进行优化。该优化器负责找到一个位姿使得该帧数据所有扫描点在局部地图的可能性最大。具体要优化的方程如下：

$$\begin{array}{r}\arg \underset{\xi }{\min }\sum _{k=1}^K(1-M_{smooth}(T_{\xi }{h}_k){)}^2\end{array}$$

其中 $T_{\xi }$ 将 $h_k$根据估计的位姿从扫描坐标系转换到局部地图坐标系。函数 $M_{smooth}:{\mathbb{R}}^2\to \mathbb{R}$ 将信息矩阵转换为连续概率值。使用这种连续函数的数学优化通常比根据栅格的分辨率能得到更好的结果。由于这是一个局部优化问题，因此需要一个较好的初值。这里可以使用IMU来测量角速度从而进行位姿中旋转部分 $\theta$ 的估计。在没有IMU的场合则使用更好频率的扫描匹配，这样位姿之间相隔的时间更短，因此丢失的可能性也会降低。

3.2 回环检测

由于每次扫描匹配只与最近几次扫描数据匹配，因此上述的方法会逐渐累计误差。Cartographer 中使用稀疏位姿调整（Sparse Pose Adjustment） 来对所有局部地图中的所有扫描数据的位姿进行优化。内存中存储了扫描数据之间的相对位置来进行回环优化，除了这些相对位姿以外，一旦一个局部地图完成（不再变化之后），其他包含一个扫描和一个局部地图的关系对都被作为一个约束添加进回环检测中。为此，在后台会持续进行扫描匹配，一旦一个上述关系（扫描和任意一个不再变化的局部地图匹配）发现之后，相应的相对位姿会添加进优化问题中。

优化问题
回环优化，如scan matching，也可以使用非线性最小二乘问题来表示，每隔几秒，就使用Ceres对下面的优化问题进行求解：

$$\underset{{\Xi }^m,{\Xi }^s}{\mathrm{argmin}}\frac{1}{2}\sum _{ij}\rho \left(E^2({\xi }_i^m,{\xi }_j^s;{\Sigma }_{ij},{\xi }_{ij})\right)(SPA)$$

其中，世界坐标系下的子图的位姿 Ξ m = { ξ i m } i = 1 , ⋯ , m \Xi^{m}=\{\xi_{i}^{m}\}_{i=1,\cdots,m} Ξm={ξim​}i=1,⋯,m​ 以及扫描的位姿 Ξ s = { ξ j s } j = 1 , ⋯ , n \Xi^{s}=\{\xi_{j}^{s}\}_{j=1,\cdots,n} Ξs={ξjs​}j=1,⋯,n​ 将由给定的约束进行优化。

这些约束采用相对位姿 ${\xi }_{ij}$ 以及对应的协方差矩阵 ${\Sigma }_{ij}$ 组成。对于每一对{子图i，扫描j}，相对位姿 ${\xi }_{ij}$ 描述了该激光扫描在子图坐标系中的匹配位置。协方差矩阵 ${\Sigma }_{ij}$ 也可以被估计。这样一个约束的残差 (E) 可以如下表示，

$$E^2({\xi }_i^m,{\xi }_j^s;{\Sigma }_{ij},{\xi }_{ij})=e({\xi }_i^m,{\xi }_j^s;{\xi }_{ij}{)}^T{\Sigma }_{ij}^{-1}e({\xi }_i^m,{\xi }_j^s;{\xi }_{ij}),(4)$$

$$\begin{array}{rl}e({\xi }_i^m,{\xi }_j^s;{\xi }_{ij})& ={\xi }_{ij}-\left(\begin{array}{c}{R}_{{\xi }_i^m}^{-1}(t_{{\xi }_i^m}-t_{{\xi }_j^s})\\ {\xi }_{i;\theta }^m-{\xi }_{j;\theta }^s\end{array}\right).\end{array}(5)$$

另外，损失函数 $\rho$ 可以使用Huber损失，用来减少outliers（错误约束）带来的影响。这种情况常发生在局部特征相似的情况，如办公室的隔间。

4. 分支定界扫描匹配（分枝定界的scan matching）

回环检测中，需要找到最优匹配的位姿 ${\xi }^{\star }$，可以通过以下公式计算：
$${\xi }^{\star }=\arg \underset{\xi \in W}{\max }\sum _{k=1}^K(M_{\text{nearest}}(T_{\xi }{h}_k))$$
其中，W 是搜索窗口，$M_{\text{nearest}}$是从 M扩展至 ${\mathbb{R}}^2$ 得到的值。具体扩展的方式是：首先将输入位置四舍五入到最近的栅格点，然后使用对应的像素的概率值作为结果。

通过精心选择步长可以提高效率。这里选择使用一个角度 ${\delta }_{\theta }$ 作为步长，使得最远距离 $d_{\max }$ 的激光点不会移动超过 r，即一个像素的宽度。可以通过余弦法则计算出该步长：

$$\begin{array}{rl}{d}_{\max }& =\underset{k=1,\dots ,K}{\max }||h_k||\\ {\delta }_{\theta }& =\arccos \left(1-\frac{r^2}{2d_{\max }^2}\right)\end{array}$$

对于角度的搜索分辨率，Cartographer采用上述等式计算得出的 ${\delta }_{\theta }$，而对于线性搜索分辨率则事先指定 $r$，因此搜索窗口的尺寸可以根据分辨率以及总长度（$W_x=W_y=7$ m, $W_{\theta }=30^{\circ }$）计算得出：

$$\begin{array}{rl}{w}_x& =\frac{W_x}{r}\\ w_y& =\frac{W_y}{r}\\ w_{\theta }& =\frac{W_{\theta }}{{\delta }_{\theta }}\end{array}$$
每次搜索范围是通过以下有限集定义得出的：

W ˉ = { − w x , … , w x } × { − w y , … , w y } × { − w θ , … , w θ } W = { ξ 0 + ( r j x , r j y , δ θ j θ ) : ( j x , j y , j θ ) ∈ W ˉ } \begin{align*} \bar{\mathcal{W}} &= \{-w_x, \ldots, w_x\} \times \{-w_y, \ldots, w_y\} \times \{-w_\theta, \ldots, w_\theta\} \\ \mathcal{W} &= \{\xi_0 + (rj_x, rj_y, \delta_\theta j_\theta): (j_x, j_y, j_\theta) \in \bar{\mathcal{W}}\} \end{align*} WˉW​={−wx​,…,wx​}×{−wy​,…,wy​}×{−wθ​,…,wθ​}={ξ0​+(rjx​,rjy​,δθ​jθ​):(jx​,jy​,jθ​)∈Wˉ}​

关于怎么搜索最优位姿，论文中给出了两个方法。第一种方法是暴力递归搜索空间中所有解，并选择分数最优的。

算法1 ： Naive algorithm for (BBS)

Naive algorithm for (BBS)是一个用于进行最佳匹配搜索的暴力算法。它通过在不同的平移和旋转参数空间内搜索，找到最佳的匹配。以下是该算法的流程总结和步骤性描述：

算法流程总结：

1. 初始化：

   • best_score 初始化为负无穷大，用来记录当前找到的最佳匹配分数。

2. 遍历平移和旋转参数：

   • 使用三个嵌套的循环，分别遍历水平方向（j_x）、垂直方向（j_y）和旋转方向（j_θ）的参数空间，从而在各个可能的平移和旋转组合中搜索。

3. 计算匹配分数：

   • 在每个平移和旋转组合下，计算一个匹配分数 score，它是通过某个函数 M_nearest 对所有 K 个匹配点的距离求和得到的。

4. 更新最佳匹配：

   • 如果当前计算的 score 比 best_score 更好，则更新 best_score，并记录当前的匹配参数 match。

5. 返回结果：

   • 当所有平移和旋转组合都遍历完毕时，返回最佳的 best_score 和相应的 match。

步骤性描述：

1. Step 1: 初始化：

   • 将变量 best_score 设为负无穷，以确保后续任意一个分数都会比初始值好。

2. Step 2: 遍历平移和旋转参数：

   • 遍历变量 j_x 从 -w_x 到 w_x，对应水平方向的平移。
   • 对于每个 j_x，遍历变量 j_y 从 -w_y 到 w_y，对应垂直方向的平移。
   • 对于每个 j_x 和 j_y 组合，再遍历变量 j_θ 从 -w_θ 到 w_θ，对应旋转角度的变化。

3. Step 3: 计算当前平移旋转下的匹配分数：

   • 对每个 j_x, j_y, j_θ 的组合，通过 M_nearest 函数对所有 K 个点的匹配得分求和，计算出当前组合的匹配分数 score。

4. Step 4: 更新最佳分数和匹配：

   • 比较当前分数 score 与记录的 best_score。
   • 如果当前分数更高，更新 best_score 和当前的平移旋转组合 match。

5. Step 5: 返回最终结果：

   • 遍历结束后，返回最佳的 best_score 和对应的 match，即找到的最佳平移和旋转组合。

总结：

该算法属于暴力搜索（Brute Force Search），通过遍历所有可能的平移和旋转组合，寻找最佳匹配组合。每次计算匹配得分后都会与当前的最佳分数进行比较，并根据需要更新。由于它是暴力搜索，效率较低，但实现简单。

算法2： 分支定界算法 Generic Branch and Bound, BnB

这种方法在搜索窗口较大时速度很慢，因此 Cartographer 中实际使用的是一种分支定界算法，算法原理具体在 An automatic method of solving discrete programming problems 有介绍，基本算法如下所示：
图中的算法描述的是一种通用的分支定界算法（Generic Branch and Bound, BnB），该算法通过递归分裂问题空间，并利用上下界来减少计算，最终找到最优解。以下是该算法的流程总结和步骤性描述。

流程总结：

1. 初始化：设定初始的 best_score 为负无穷，初始节点集合 C。
2. 迭代处理：不断从 C 中选择节点进行扩展，直到 C 集合为空。
3. 分裂与评估：对于每个节点，如果是叶节点则评估其得分；如果不是叶节点则继续分裂。
4. 剪枝：若当前节点的得分小于当前的 best_score，则进行剪枝。
5. 返回结果：当所有节点处理完毕，返回最终的 best_score 和对应的 solution。

步骤性描述：

1. Step 1: 初始化

   • 将 best_score 设置为负无穷，表示当前尚未找到合适的解。
   • 初始化节点集合 C 为初始节点 C_0，用于存储待处理的节点。

2. Step 2: 迭代处理

   • 进入 while 循环，只要节点集合 C 不为空，算法会不断从 C 中选择一个节点 c 进行处理，并将其从 C 中移除。

3. Step 3: 判断节点类型

   • 检查节点 c 是否为叶节点：
     • 如果是叶节点：直接计算该节点的得分 score(c)。
       • 如果 score(c) 大于当前的 best_score，更新 best_score 和相应的 solution。
     • 如果不是叶节点：继续将该节点 c 分裂为若干子节点 C_c，并将子节点加入 C 中继续处理。

4. Step 4: 剪枝

   • 在分裂之前，首先比较当前节点的得分 score(c) 和 best_score：
     • 如果 score(c) 小于或等于 best_score，直接舍弃该节点及其子节点，称为剪枝（Bound）。
     • 否则，分裂节点 c 进行进一步处理。

5. Step 5: 继续处理或终止

   • 如果节点集合 C 中仍有未处理的节点，继续步骤 2 直至所有节点都被处理完。

6. Step 6: 返回结果

   • 最终，算法返回当前找到的最佳得分 best_score 和相应的解 solution。

总结：

该分支定界算法通过在解空间中寻找最优解，并使用上下界进行剪枝（减少不必要的计算），从而优化搜索效率。每当找到一个更好的解时，会更新最佳解。

算法的主要思想是将包含不同数量解的子集表示为树中的不同节点，根节点表示所有可能解（即上文定义的 $mathcalW$。每个节点的子节点表示该节点包含的可能解的不同部分，所有子节点包含的解与该节点本身包含的解一致。树中每个根节点都表示一个特定的解。同时，当每个内部节点的分数 $score(c)$ 是它对应元素的分数上界时，算法的结果是确定的，即得出的解与 Algorithm 1 得到的解相同。在这种情况下，只要一个节点是有界的，在该子树上就不存在比当前最优解更优的解。

为了完善这种算法，论文主要介绍了以下三个方面的方法：节点选择、分支以及计算上界。

算法2.1：节点选择

算法中使用了深度优先搜索（DFS）作为默认选项来进行节点选择。算法的效率依赖于树的最大深度，即依赖于两个参数：一个好的上界（有助于最大程度进行路径的筛选）以及一个好的当前解。后者由于 DFS 可以快速评估叶节点所以有所改善。并且由于作者不希望将不那么好的匹配也加入约束中，因此在筛选出的最优解中还设置了一个阈值，如果匹配度低于该阈值则不将其加入回环约束中。在实际中这个阈值不常被超过，因此降低了节点选择的重要性。在 DFS 过程遍历叶节点的顺序上，它们计算每个子节点的上界，并从中选择最高上界的子节点进行优先遍历，如 Algorithm 3 所示。

算法2.2：分支以及计算上界

以下是根据您提供的内容，对DFS分支定界扫描匹配（BBS）算法的流程总结：

1. 初始化：

   • 设定最佳分数 best_score 为一个初始阈值，通常是一个较低的分数。
   • 计算初始节点集 ${\mathcal{C}}_0$中每个元素的分数，并将这些元素放入一个栈 C 中，按照分数从高到低排序。

2. 循环处理：

   • 当栈 C 不为空时，执行循环体内的操作。
   • 从栈 C 中弹出一个节点 c。

3. 评估节点：

   • 如果节点 c 的分数 score(c) 大于当前最佳分数 best_score，则进行以下操作：
     • 如果 c 是一个叶节点（即没有子节点），则将当前节点的位姿 $\xi_{c}$ 设为匹配结果 match，并将 best_score 更新为 score(c)。
     • 如果 c 不是叶节点，进行分支操作：将 c 分解为子节点集 $\mathcal{C}_{c}$。
     • 计算子节点集 ${\mathcal{C}}_c$ 中每个元素的分数，并将这些子节点按分数从高到低排序后推入栈 C。

4. 循环结束条件：

   • 如果节点 c 的分数 score(c) 不大于 best_score，则不进行任何操作，继续下一次循环。

5. 输出结果：

   • 当栈 C 为空时，循环结束。
   • 返回最佳分数 best_score 和匹配结果 match（如果已设定）。

这个算法通过深度优先搜索结合分支定界策略，有效地在解空间树中搜索最优解，同时通过维护一个分数栈来优化搜索过程，减少不必要的计算。

算法2.3：分支规则

分支规则描述了树中每个节点如何表示和分支：
$$

• 每个节点由一个整数元组$c=(c_x,c_y,c_{\theta },c_h)\in {\mathbb{Z}}^4$ 表示，其中高度 $c_h$ 的节点包含最多 $2^{c_h} \times 2^{c_h}$可能的平移情况（转向固定）。
• 对于高度 $c_h$ 的节点，可能的平移集合定义为：
  W ˉ ˉ c = ( { ( j x , j y ) ∈ Z 2 : c x ≤ j x ≤ c x + 2 c h , c y ≤ j y ≤ c y + 2 c h } × { c θ } ) \bar{\bar{\mathcal{W}}}_c = (\{(j_x, j_y) \in \mathbb{Z}^2: c_x \leq j_x \leq c_x + 2^{c_h}, c_y \leq j_y \leq c_y + 2^{c_h}\} \times \{c_\theta\}) Wˉˉc​=({(jx​,jy​)∈Z2:cx​≤jx​≤cx​+2ch​,cy​≤jy​≤cy​+2ch​}×{cθ​})
  $${\bar{\mathcal{W}}}_c={\overline{\stackrel{ˉ}{\mathcal{W}}}}_c\cap \bar{\mathcal{W}}$$
• 叶节点的高度 $c_h=0$，因此只关联一个特定解：
  $${\xi }_c={\xi }_0+(r{c}_x,r{c}_y,{\delta }_{\theta }{c}_{\theta })\in \mathcal{W}$$

根节点包含所有解，并不是显式出现，而是分支为一系列固定高度 $h_0$ 的初始节点 ${\mathcal{C}}_0$ 的集合，该集合包含了如下的搜索窗口：
W ˉ 0 , x = { − w x + 2 h 0 j x : j x ∈ Z , 0 ≤ 2 h 0 j x ≤ 2 w x } , W ˉ 0 , y = { − w y + 2 h 0 j y : j y ∈ Z , 0 ≤ 2 h 0 j y ≤ 2 w y } , W ˉ 0 , θ = { j θ ∈ Z : − w θ ≤ j θ ≤ w θ } , C 0 = W ˉ 0 , x × W ˉ 0 , y × W ˉ 0 , θ \begin{align*} \bar{\mathcal{W}}_{0,x} &= \{-w_x + 2^{h_0}j_x: j_x \in \mathbb{Z}, 0 \leq 2^{h_0}j_x \leq 2w_x\}, \\ \bar{\mathcal{W}}_{0,y} &= \{-w_y + 2^{h_0}j_y: j_y \in \mathbb{Z}, 0 \leq 2^{h_0}j_y \leq 2w_y\}, \\ \bar{\mathcal{W}}_{0,\theta} &= \{j_\theta \in \mathbb{Z}: -w_\theta \leq j_\theta \leq w_\theta\}, \\ \mathcal{C}_0 &= \bar{\mathcal{W}}_{0,x} \times \bar{\mathcal{W}}_{0,y} \times \bar{\mathcal{W}}_{0,\theta} \end{align*} Wˉ0,x​Wˉ0,y​Wˉ0,θ​C0​​={−wx​+2h0​jx​:jx​∈Z,0≤2h0​jx​≤2wx​},={−wy​+2h0​jy​:jy​∈Z,0≤2h0​jy​≤2wy​},={jθ​∈Z:−wθ​≤jθ​≤wθ​},=Wˉ0,x​×Wˉ0,y​×Wˉ0,θ​​

在任意给定高度$c_h>1$的节点中，我们将其分支为高度为 $c_h-1$ 的四个子节点：
C c = ( ( { c x , c x + 2 c h − 1 } × { c y , c y + 2 c h − 1 } × { c θ } ) ∩ W ˉ ) × { c h − 1 } \mathcal{C}_c = ((\{c_x, c_x + 2^{c_h - 1}\} \times \{c_y, c_y + 2^{c_h - 1}\} \times \{c_\theta\}) \cap \bar{\mathcal{W}}) \times \{c_h - 1\} Cc​=(({cx​,cx​+2ch​−1}×{cy​,cy​+2ch​−1}×{cθ​})∩Wˉ)×{ch​−1}

算法2.4：计算上界

作者使用下述方法来表示节点的上界：
为了计算节点的分数 $score(c)$，使用了以下方法来表示节点的上界：

$$\begin{array}{rl}score(c)& =\sum _{k=1}^K\underset{j\in {\bar{\mathcal{W}}}_c}{\max }{M}_{\text{nearest}}(T_{{\xi }_j}{h}_k)\\ & \ge \sum _{k=1}^K\underset{j\in {\bar{\mathcal{W}}}_c}{\max }{M}_{\text{nearest}}(T_{{\xi }_j}{h}_k)\\ & \ge \underset{j\in {\bar{\mathcal{W}}}_c}{\max }{M}_{\text{nearest}}(T_{{\xi }_j}{h}_k)\end{array}$$

为了提高计算最大值的效率，使用了预先计算的栅格$M_{\text{precomp}}^{c_h}$。在每一个可能的高度 $c_h$ 预先计算一个栅格，可以让我们在扫描激光点的线性时间内计算分数。需要注意的是，为了完成这个目的，还需要计算${\bar{\mathcal{W}}}_c$ 的最大值，因为其有可能在搜索范围的边界上大于 ${\bar{\mathcal{W}}}_c$：

$$\begin{array}{rl}score(c)& =\sum _{k=1}^K{M}_{\text{precomp}}^{c_h}(T_{{\xi }_c}{h}_k)\\ M_{\text{precomp}}^{c_h}(x,y)& =\underset{\begin{array}{c}{x}^{\prime }\in [x,x+r(2^h-1)]\\ y^{\prime }\in [y,y+r(2^h-1)]\end{array}}{\max }{M}_{\text{nearest}}(x^{\prime },y^{\prime })\end{array}$$

其中 ${\xi }_c$ 表示根节点。注意这里 $M_{\text{precomp}}^h$ 的像素结构和 $M_{\text{nearest}}$ 一样，但在每一个像素中存储的是以该位置为中心的$(2^h\times 2^h)$个最大值。该栅格的例子如下所示。

为了降低构建栅格的成本，Cartographer会等待直到概率栅格不再更新后才进行构建，此时会计算一系列高度的预计算栅格以进行匹配。

对于任何一个预计算栅格，算法会计算以该 栅格为起点的宽度为 $2^h$的像素点的最大值，并且使用这个结果来构造下一个预计算栅格。

当一系列值发生增减时，其最大值可以在$O(1)$时间内进行更新，只要值的删除顺序和增加顺序保持一致（先进先出）。这个算法可以用一个双端队列（deque）实现，通过这种算法可以在$O(n)$复杂度内计算预计算该栅格，其中 $n$ 为该预计算栅格的像素个数。

另一种 替代的方法是计算较低分辨率的概率栅格，但Cartographer中没有使用这种方法，因此不进行介绍。

4.实验结果

这里论文展示了 Cartographer 在一些数据集上的表现：

Real-World Experiment: Deutsches Museum
传感器数据时长及范围：1913 s, 2253 m
计算匹配：Intel Xeon E5-1650 @ 3.2 GHz
算法结果
消耗资源：1018 s CPU 时间，2.2 GB 内存，4线程，实际上在 360 s 时钟时间内完成（多线程的原因），所以实际完成速度是实时速度的 5.3 倍
构建回环优化图包含 11456 个节点和 35300 条边，每一个解通常使用 3 个循环求得，耗时 0.3 s
结果如图：

Real-World Experiment: Neato’s Revo LDS
传感器数据时长及范围：Revo LDS 传感器，2 Hz
算法结果
结果如图：