【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的时候，形式比用Ω、ξ表示更加优美： −logp