# Cartographer 原理

Google Cartographer是一種基於圖的SLAM方法,其算法重點如下:

1. 前端:採用子圖方式, 每個子圖包含少量連續的掃描,並採用Ceres scan matcher進行優化以獲得掃描在子圖中的位姿。
2. 後端:關閉環路偵測使用分支定界法進行快速像素級匹配,得到loop closure constraints。

3. 圖優化:使用Google Ceres以稀疏方式優化所有子圖和掃描的位姿,最小化回環誤差。

4. 地圖生成:即時渲染子圖的occupancy grid以給操作員實時回饋。

5. 定位模式:已有地圖的情況下,只進行掃描匹配不改變地圖。

6. 多session:可以序列化和反序列化子圖,實現多次建圖。

7. 效果:實時性能好,可以建立大尺度地圖,並具有競爭力的定位精度。

8. 缺點:依賴良好的里程計,難以修改代碼,已停止維護。

Cartographer總體上達到了實時、大尺度、精確的SLAM效果,但易用性和可擴展性有限。提供了一種高效的基於圖的SLAM思路。

大致可分以下兩個重點
1. Local SLAM(前端)
利用里程计(Odometry)和IMU数据进行轨迹推算，给出小车位姿估计值将位姿估计值作为初值，对雷达数据进行匹配，并更新位姿估计器的值雷达一帧帧数据经过运动滤波后，进行叠加，形成子图(submap)

2. Global SLAM(後端)
後端優化地圖，全部子图形成一张完整可用的地图

## Local 2D SLAM
Cartographer中的Local SLAM，一帧帧scan matching（ceres非線性優化）變成一個個Submap的過程,cartographer结合了 local SLAM 和 global SLAM，两个SLAM过程都需要優化的就是lidar的掃出scans點（x，y，θ），这些 scans（因为是2D，所以x和y是平移，θ是旋转角）。

作者把雷达装在背包上，所以他们的平台是不稳的（晃晃悠悠的意思）。他们因此在背包上装了一个IMU（Inertial measurement unit， 惯性测量单元）。

IMU会给出重力方向，用于把雷达数据投影到2D平面（就是告诉你雷达的姿态，有了姿态你就能知道雷达究竟扫描的是3D世界的哪个平面，再将雷达数据变为投影数据就是了）。

在Local SLAM中，每一帧有效掃描结果都会匹配至Submap（子圖）中，在由多張submap組成地圖。

使用ceres非線性優化将 Scan 與 Submap 對齊，这个過程進一步稱为Scan Matching。
Local scan matching 會隨時間存在累计誤差，Global SLAM 会消除这些误差。

### 掃描Scan
**Local SLAM第一部分，拿到了每帧雷达数据（一堆(x,y)点），是依靠变换矩阵T将Scan的点变换到Submap下的**Submap的构建就是将Scan不断与Submap的坐标系对齐的迭代过程。

### 子圖Submaps
**Local SLAM第二部分，submap是概率网格地图，地图每个点都是该点是否被占用的概率值**，通过计算Hit点集合Miss点集以及公式更新每个点的值连续的几帧Scan会组成一个Submap，Submap实际是一个概率网格地图

### Ceres scan matching
**Local SLAM第三部分，新的Scan是通过变换矩阵T插入到Submap中。变换矩阵T是经过IMU的初值，并通过最小二乘法求出的**一帧Scan插入Submap之前，首先要对这帧Scan的位姿进行优化。

作者使用的是基于Ceres库的Scan matcher。Scan Matcher的目标就是寻找一个最优位姿，这个位姿能让Scan中的点最大概率地匹配到Submap上。

## Closing loops
**Cartographer中的Global SLAM，Submap之间存在累计误差，那么这些Submap如何形成一整张地图，靠的是回环检测和全局优化**

由于Scans仅与Submap中最近的一些Scan进行匹配，会有累计误差。 如果对于仅几十个连续的Scans，累积误差很小。 但子图多了累计误差就有影响了。累计误差不可避免，且解决累计误差十分重要

作者的方法是优化所有Scan和Submap的位姿，遵循稀疏姿态调整。 Scan插入时的相对姿态（相对于Submap的）存储在内存中，用于闭环优化。 除了这些相对姿态外，一Submap不再更改了，则将考虑由Scan和Submap组成的对进行循环闭合。Scan Matcher在后台运行，如果找到匹配的对象，则将对应的相对姿态添加到优化问题中。

**每一个scan在cartographer程序中被写成一个node，每一个node（也就是每一帧Scan）将和一个submap存在一个位姿变换。submap可以是自己所处的submap，也可以是其他submap。能够建立constraint的位姿将被优化。**

### Optimization problem
闭合优化问题也是一个非线性最小二乘问题，这些被优化的submap位姿和Scan位姿给出了一些constraint（约束）。constraint的表现形式就是位姿 ξ i j 和协方差矩阵 ∑ i j 。位姿 ξ i j 代表 j 帧Scan 在 Submap i 下的位姿。
协方差矩阵 ∑ i j 可以被估计，残差 E EE 的计算公式是
当Scan matcher 将不正确的constraint添加到优化问题时，损失函数ρ （例如Huber损失），可以用于减少异常值的影响，而异常值可能会出现在局部对称的环境（包含隔间的办公室）中

### Branch-and-bound scan matching
分支界定的scan matching，给出当前帧scan对应Submap的最优位姿。
作者使用的是分支定界方法。核心思想就是将可能性的子集表示为树中的节点，其中根节点表示所有可能的解，也就是搜索窗口W WW。每个叶子节点代表一个可能的解，全部叶子节点也就代表了全部的解。

算法分为三个部分：节点选择，分支规则，计算上限

#### 1. 节点选择：采用 “深度优先搜索”（DFS） 算法
算法的效率依赖于两件事：好的上限和好的当前解。 由于我们不想添加差的匹配项作为constraint，因此我们引入了Score阈值，不采纳低于该阈值的解。 由于在实践中通常不会超过这个阈值，这降低了节点选择或寻找初值解的重要性。

关于DFS算法访问子节点的顺序，我们计算每个子节点的分数上限，先访问“最有前途”的子节点，其上限最大。详见算法3。

#### 2. 分支规则
树中的每个节点是一组整数，c = ( c x , c y , c θ , c h ) ∈ Z 4 。高度为c h 的节点可以表示2 c h × 2 c h 全部可能的平移

很好理解，简单来说就是用另一种方式定义了搜索时的步长。每个参数的含义如下：
一个高度为c h （ c h > 1 ） 的节点，可以分成高度为c h − 1 的四个子节点

#### 3. 计算上限
分支界定算法另外一个部分就是计算内节点的上限了
为了能够有效地计算上式中的最大值，我们使用了预先计算好的网格M p r e c o m p c h 。对每个可能的高度c h 预先计算一个网格，使我们能够以Score与Scan点的数量所成的线性关系，来计算Score。请注意，为了能够做到这一点，我们还计算了W c ‾ ‾ 上的最大值，该最大值大于我们搜索空间边界附近的W c ‾ 。
请注意，M p r e c o m p c h 具有与M n e a r e s t 相同的像素结构，但是在每个像素存储2 h × 2 h 像素框的最大值
为了使构建预计算网格的计算量保持较低，我们等到概率网格不再更新，然后才计算出一组预先计算的网格，并开始对其进行匹配。

对于每个预先计算的网格，我们从每个像素开始，计算2 h 像素宽的行中的最大值。 使用该中间结果，然后构造下一个预计算网格。