【SLAM】（二）Cartographer的原理探究——GraphSLAM理论基础

现在真是越来越懒了……好久没有更新过博客。上一篇博客中提到，在室外进行Cartographer实验的时候遇到了莫名的卡顿——一方面是由于小车搭载处理器的性能瓶颈，另一方面，室外场景确实消耗更多的运算资源，只要把地图分辨率降低，优化的频率降低或者缩短Lidar的max_range就可以得到实时的效果。

从寒假到现在挺长的一段时间，在Cartographer的研究方面大概做了以下几件事：

1. 研究了下使用velodyne 32E的数据（PointCloud2）跑cartographer的方法和效果，顺便学习了一下ROS的使用。ROS真是板砖利器呀233；
2. 用实验室采集的室外数据试跑了一下cartographer，发现了一些问题：在较大的室外场景下，cartographer会出现内存溢出的错误（约300*100$m^2$的范围）；由于cartographer本来就是为室内场景设计，所以将submap都放在内存里。如果想要实现室外的SLAM建图，还需要加入地图内存的管理机制才行；
3. 为了从纯粹的cartographer使用者蜕变为融会贯通的开发者，我花了些时间阅读代码，以及了解GraphSLAM理论。

【下面主要记录一下我学习GraphSLAM的过程】
Cartographer背后主要的思想是GraphSLAM。GraphSLAM又被称为Graph-based SLAM，它的基本思想是将机器人不同时刻的位姿抽象为点（pose），机器人在不同位置上的观测所产生的约束被抽象为点之间的边，或者叫约束（constraint）。
所谓的约束可以有多种多样的形式，比如机器人在A点和B点都看到同一个消防栓（我们可以认为这是固定在地图上的landmark），那么机器人在AB点观测到消防栓的相对位置，就对机器人在A点和B点的位姿产生了约束，进一步的，AB两点之间也产生了约束。
GraphSLAM就是在机器人运动的过程中构建出若干点（pose）和边（constraint）组成的图（Graph），再从全图的角度进行优化。

GraphSLAM更具体的入门示例可以参考“白巧克力亦唯心”的博文《graph slam tutorial ：从推导到应用1》：
http://blog.csdn.net/heyijia0327/article/details/47686523

看过上面这篇博文后，可以说对GraphSLAM就有了一个比较感性的认识。但是《graph slam tutorial ：从推导到应用２》中包含了一大堆莫名其妙的数学公式，缺少SLAM理论基础的同学看起来可能会比较吃力（我当时就这么觉得……）

经过一番周折后，我发现了一篇论文讲解的非常细致：
《The GraphSLAM algorithm with applications to large-scale mapping of urban structures》

这篇文章的作者是神书《Probabilistic Robotics》的作者之一，内容和书上也非常类似，建议有志于深入了解GraphSLAM的同学仔细阅读这篇paper（搭配书阅读风味更佳）

贯穿GraphSLAM中的一个非常重要的概念就是信息矩阵（information matrix）和信息向量（information vector），通常用 Ω 和 ξ 来表示。想要理解它们在GraphSLAM中的作用，我们首先得知道它们从何而来。
大家所熟悉的高斯分布，在SLAM中常常用于表示传感器噪声分布等，是SLAM算法重要的组成元素。通常情况下，我们习惯用µ（均值）和Σ（协方差）来描述一个高斯分布。而上面提到的信息矩阵和信息向量，其实是另一组描述高斯分布的参数，叫做canonical parameterization（似乎应该翻译成“典范参数”），他们的关系如下：

$$\begin{cases} Ω = Σ^{-1} \\ ξ = Σ^{-1}µ \end{cases}$$

在使用典范参数表示negative log likelihood的时候，形式比用Ω、ξ表示更加优美：$-log p(x) = const. + \frac{1}{2}x^T Ω x − x^T ξ$
这也是GraphSLAM中引入信息矩阵和信息向量的原因。

下面三张图非常生动的描述了GraphSLAM中建立information matrix的过程。

三张图的右侧就是信息矩阵，白色格点表示该处的数值为0，黑色表示正值；图中的$x_i$ 表示机器人所在的位姿，例如可以用（x, y, $\theta$）来描述的二维位姿；图中的$m_j$ 表示在某个位置$x_i$ 所观测到的路标（landmark），可以通过路标和$x_i$ 的相对距离和角度来描述。
相邻两个$x_i$ 之间的移动可以通过运动模型和里程计来产生约束，而$x_i$ 和$m_j$ 之间则通过观测模型来产生约束。图A->B->C清晰的描述了这一过程。
这些所谓的约束其实反应了运动模型/观测模型的不确定性，简单地说，如果某个传感器的误差越大，那么信息矩阵中对应的约束值就越小；反之亦然。

在GraphSLAM中，我们需要通过优化信息矩阵来求得合理的pose和map，现在信息矩阵已经得到了，剩下的工作似乎显而易见。可是，为什么很早就被提出的GraphSLAM算法理论，却在近几年才刚刚兴起呢？
我们注意到，随着地图的扩大和landmark的增多，信息矩阵中$m_j$ 项会迅速增加，而它带来的巨大运算量是系统难以承受的。因此，人们普遍认为，过大的运算量使得GraphSLAM不具备良好的实时性和运算效率。
而然近几年SLAM研究者们发现，我们构建出来的Graph其实是非常稀疏的——因为每个pose $x_i$ 只能观察到其附近有限的路标$m_j$ ，并与他们产生约束；而其他大量的$m_j$ 与当前的pose却没有发生联系。人们发现利用稀疏代数的相关技巧，可以将$m_j$ 从信息矩阵中消去，进而减少了大量的运算。
下图是这个被称为variable elimination算法的示意。可以看到，每消除一个$m_j$ ，能够观测到$m_j$ 的所有{$x_i$ }之间的约束都会被增强。例如下图中$m_3$ 的消除就增强了<$x_2$, $x_3$>,<$x_3$, $x_4$>之间的约束，并在<$x_2$, $x_4$>之间增加了一条边（约束）。经过消除算法之后的信息矩阵就只剩下所有pose之间的约束了。

至此，我们可以列出GraphSLAM算法的基本流程：

1. 选择节点和边的类型，确定他们的参数化形式，加入图中
2. 求节点pose初始的估计值µ0:t (可以用航位推算法等)，开始迭代
3. 对信息矩阵降维 (variable elimination algorithm)，求优化的梯度方向
4. 继续迭代。如果迭代结束，返回对pose和map的优化结果。

这个流程中的2、3、4步，已经有现成的工具可以使用（例如g2o, ceres等），它也被称为GraphSLAM的后端（back-end）。第1步则被称为GraphSLAM的前端（front-end），也是我们设计GraphSLAM算法的难点——如何根据不同的传感器和任务场景设计Graph中边（constraint）的参数化形式。

如果你不太理解什么是边的参数化形式，可以参考“半闲居士”的博文《深入理解图优化与g2o：图优化篇》，其中有比较详细的解释：
http://www.cnblogs.com/gaoxiang12/p/5244828.html

目前网上比较多的基本是基于视觉的GraphSLAM方法，大概是因为视觉图像中比较好定义landmark的概念。对于laser-based场景，我目前没有找到比较满意的开源方案来入手。
下面的计划：
1. 以g2o为切入点熟悉GraphSLAM的程序结构和后端优化的使用方法；
2. 继续阅读cartographer的代码，看看google怎样通过laser之间的scan matching 提取出相应的constraint。

PS:
原本实验室是没有Velodyne的，老师说：“要有（激）光”，于是就有了Velodyne （配上非常霸气的tank图，蛤蛤）