cartographer学习（4）——论文阅读

cartographer学习（4）——论文阅读

代码跑通了，在学习阅读代码之前先来看看其论文是怎么写的。

论文：Real-Time Loop Closure in 2D LIDAR SLAM

1 摘要

5cm精度实时建图与回环检测

为了实现实时回环，使用分支定界算法来计算scan-to-submap作为约束

2 介绍

本文的贡献在于提供了一种新的方法：减少根据激光雷达数据计算回环约束的计算量

3 相关工作

Scan-to-scan matching：雷达数据与雷达数据进行匹配，会很快积累误差

解决方法：粒子滤波和基于图形的SLAM （graph-based）。

粒子滤波要求每个粒子中都能表示完整系统状态，对于基于网格的SLAM，随着地图变大，这很快变成资源密集型。较小维度的特征表示则不需要每个粒子的网格图。当需要最新的栅格地图时，建议计算子地图，这些子地图仅在必要时更新。

Graph-based是一种工作于基于位姿和特征的集合的方法。图中的边表示从观察结果中生成的约束，节点表示的是位姿和特征。可以使用各种优化方法来最小化由所有约束引入的误差。

4 系统概述

接下的第4、5、6部分是重点

谷歌的制图员以装有传感器的背包的形式来生成分辨率为r = 5厘米的2D网格地图。

不使用粒子滤波，使用pose optimization来处理误差累计。

当一个submap结束后，不再有新的扫描被插入其中，它再参与回环检测的扫描。

所有完成的submaps和scans都会自动被考虑到回环检测中。

5 局部2维SLAM

局部方法中,每个连续的扫描与submap M匹配,使用非线性优化将扫描与子地图对齐;这个过程进一步被称为扫描匹配。随着时间的推移，扫描匹配会累积误差，将通过第五节中介绍的全局方法消除这些误差

5.1 Scans

构建submap是重复对齐扫描和子地图坐标帧(也称为帧)的迭代过程。

5.2 submaps

几个连续的scans被用来建立一个子地图。这些子地图采取概率网格M的形式

分辨率r

values 被认为是网格点被占用的概率。

将占用和未占用的概率更新为

5.3 Ceres扫描匹配

在将扫描插入子地图之前，使用基于ceres的扫描匹配来对scan相对与当前的local submap进行优化。把它看作一个非线性最小二乘问题：

M函数使用双三次插值。

因为这是一个局部优化问题，所以需要用imu提供一个好的角度初始值。在没有惯性测量单元的情况下，可以使用更高频率的扫描匹配或像素精确的扫描匹配方法，尽管计算量更大。

6 回环检测

scans只和最近的submaps进行匹配，所以会产生累积误差。

大多是采用的是创建很多的submaps来解决。

本文中用的方法是优化所有的scans和submaps，这是根据一篇讲解2D建图中的高效稀疏矩阵图优化的文章（Efficient sparse pose adjustment for 2D mapping）来的。

6.1 优化

回环检测的优化问题也和匹配一样是一个非线性最小二乘问题，可以容易的添加残差。每隔几秒，就会使用Ceres计算一次。

其中Ξm为 submap poses，Ξs为scan poses，ξij相对位姿和∑ij相关协方差矩阵。对于一对匹配好的submap i和scan j，ξij标准scan在submap坐标框中的匹配位置。

残差E计算方式：

当扫描匹配向优化问题添加不正确的约束时，损失函数ρ（例如Huber损失）用于减少（SPA）中可能出现的异常值的影响。

6.2 分支界定扫描匹配

算法一：

通过仔细选择步长可以提高效率。选择角度步长Δθ，所以最大范围dmax处的扫描点移动不能超过一个像素的宽度r。利用余弦定律，我们推导出

算法二：

这种算法最初是在 mixed integer linear programs 文章里提出的。其主要思想是将可能的子集表示为树中的节点，其中根节点表示所有可能的解决方案，在本例中为W。每个节点的子节点构成其父节点的分区，因此它们一起表示相同的可能性集。

它提供了与朴素方法相同的解决方案，只要内部节点c的分数是其元素分数的上界。在这种情况下，每当一个节点有界时，该子树中就不存在比目前已知的解更好的解。

为了得到一个具体的算法，我们必须决定节点选择、分支和上界计算的方法。

（1）节点选择

算法使用深度优先搜索（DFS）作为默认选择方法：该算法的效率很大程度上取决于 the tree being pruned。表现在两个方面，一是好的上限和二是好的解决方案。由于我们不希望将较差的匹配添加为循环结束约束，因此我们还引入了一个分数阈值，低于该阈值，不进行优化。

关于DFS期间访问子节点的顺序，我们计算每个子节点得分的上界，首先访问具有最大界的最有希望的子节点。该方法是算法3。

(2) 分支规则

树中的每个节点由一个整型数组描述，

在算法3的公式中，包含所有可行解的根节点没有显式出现，而是在覆盖搜索窗口的固定高度h0处分支为一组初始节点c0：

在ch>1的给定节点c上，分支到最多四个高度为ch − 1的子节点：

（3）计算上界

分支定界方法的剩余部分是计算内部节点上界的有效方法，采用：

为了能够高效地计算最大值，使用预计算网格，每一个可能的高度预先计算一个网格，使我们能够以扫描点的数量的线性计算分数。

计算上界的另一种方法是计算低分辨率概率网格，将分辨率依次减半。

7 实验结果

这部分不是很重要，有需要去论文里自己看看就行。