《概率机器人》这本书第11章详细介绍了图优化

实际代码中 pose_2d中添加约束以及后端的优化有这一部分。

1、简介

Cartographer背后主要的思想是GraphSLAM。GraphSLAM又被称为Graph-based SLAM，它的基本思想是将机器人不同时刻的位姿抽象为点（pose），机器人在不同位置上的观测所产生的约束被抽象为点之间的边，或者叫约束（constraint）。

所谓的约束可以有多种多样的形式，比如机器人在A点和B点都看到同一个消防栓（我们可以认为这是固定在地图上的landmark），那么机器人在AB点观测到消防栓的相对位置，就对机器人在A点和B点的位姿产生了约束，进一步的，AB两点之间也产生了约束。 GraphSLAM就是在机器人运动的过程中构建出若干点（pose）和边（constraint）组成的图（Graph），再从全图的角度进行优化。

SLAM问题的处理方法主要分为滤波和图优化两类。滤波的方法中常见的是扩展卡尔曼滤波、粒子滤波、信息滤波等，熟悉滤波思想的同学应该容易知道这类SLAM问题是递增的、实时的处理数据并矫正机器人位姿。比如基于粒子滤波的SLAM的处理思路是：假设机器人知道当前时刻的位姿，利用编码器或者IMU之类的惯性导航又能够计算下一时刻的位姿，然而这类传感器有累计误差，所以再将每个粒子的激光传感器数据或者图像特征对比当前建立好的地图中的特征，挑选和地图特征匹配最好的粒子的位姿当做当前位姿，如此往复。当然在gmapping、hector_slam这类算法中，不会如此轻易的使用激光数据，激光测距这么准，当然不能只用来计算粒子权重，而是将激光数据与地图环境进行匹配（scan match）估计机器人位姿，比用编码器之流精度高出很多。

目前SLAM主流研究热点几乎都是基于图优化的，为啥都用图优化了。我想这和传感器有很大关系，以前使用激光构建二维的地图，现在研究热点都是用单目、双目、RGB-D构建地图。处理视觉SLAM如果用EKF，随着时间推移地图扩大，内存消耗，计算量都很大；而使用图优化计算在高建图精度的前提下效率还快。在图优化的方法中（graph-based slam），处理数据的方式就和滤波的方法不同了，它不是在线的纠正位姿，而是把所有数据记下来，最后一次性算账。

2、描述

图是由节点和边构成，SLAM问题怎么构成图呢？在graph-based SLAM中，机器人的位姿是一个节点（node）或顶点(vertex)，位姿之间的关系构成边(edge)。具体而言比如t+1时刻和t时刻之间的odometry关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。一旦图构建完成了，就要调整机器人的位姿去尽量满足这些边构成的约束。

所以图优化SLAM问题能够分解成两个任务：

1. 构建图，机器人位姿当做顶点，位姿间关系当做边，这一步常常被成为前端（front-end），往往是传感器信息的堆积。

2. 优化图，调整机器人位姿顶点尽量满足边的约束，这一步称为后端（back-end）。

图优化过程如下图所示：先堆积数据，机器人位姿为构建的顶点。边是位姿之间的关系，可以是编码器数据计算的位姿，也可以是通过ICP匹配计算出来的位姿，还可以是闭环检测的位姿关系。构建的图和原始未经优化的地图如下：

够建好图以后，就能调整顶点满足边的约束，最后得到的优化后的地图如下图右所示。

为了更好的理解这个过程，将用一个很好的例子作说明。如下图所示，假设一个机器人初始起点在0处，然后机器人向前移动，通过编码器测得它向前移动了1m，到达第二个地点。接着，又向后返回，编码器测得它向后移动了0.8米。但是，通过闭环检测，发现它回到了原始起点。可以看出，编码器误差导致计算的位姿和观测到有差异，那机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢？

首先构建位姿之间的关系，即图的边：

线性方程组中变量小于方程的个数，要计算出最优的结果，使出杀手锏最小二乘法。先构建残差平方和函数：

为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0.

整理得到：

接着矩阵求解线性方程组：

所以调整以后为满足这些边的条件，机器人的位姿为：

前面是用闭环检测，这次用观测的路标（landmark）来构建边。如下图所示，假设一个机器人初始起点在0处，并观测到其正前方2m处有一个路标。然后机器人向前移动，通过编码器测得它向前移动了1m，这时观测到路标在其前方0.8m。请问，机器人位姿和路标位姿的最优状态？

在这个图中，我们把路标也当作了一个顶点。构建边的关系如下:

即:

残差平方和：

求偏导数：

最后整理并计算得：

得到路标和机器人位姿：

接下来，将引入了一个重要的概念。我们知道传感器的精度是有差别的，也就是说我们对传感器的相信程度应该不同。比如假设这里编码器信息很精确，测得的路标距离不准，我们应该赋予编码器信息更高的权重，假设是10。重新得到残差平方和如下：

求偏导得：

转换为矩阵：

最后计算得到:

将这个结果和之前对比，可以看到这里的机器人位姿x1更靠近编码器测量的结果。请记住这种思想，这里的权重就是在后面将要经常提到的边的信息矩阵，在后面还将介绍。

reference：

1. Grisetti. 《A Tutorial on Graph-Based SLAM》

2. University of Alberta 很棒的机器人课程 https://webdocs.cs.ualberta.ca/~zhang/c631/主要以作业为主，例子来源于该课程。

3. Strasdat. 《Visual SLAM: Why Filter?》

4. Grisetti. 课件《SLAM Back-end》（可以直接搜到）

5. Rainer & Grisetti 《g2o: A General Framework for Graph Optimization》

-----------------------------------------------------------------------------------------------------------------------------------------------------

在上一部分中通过一个例子大致了解了graph based slam的优化过程。在本篇博客中将提升一个层次，对图优化的求解过程进行推导。由于博文关注的在图构建好以后，如何调整机器人位姿使误差最下。因此，本文主要涉及的是图优化的后端（back-end）。

我们已经知道图优化问题转变成了一个最小二乘问题。根据上篇博客最后一个例子，求机器人SLAM过程中最优轨迹可以表示成求解机器人位姿使得下面误差平方函数最小。

其中， ${\color{Red} X_{i}}$ 表示图顶点的参数向量，如机器人位姿。 ${\color{Red} Z_{ij}}$ 表示测量值， ${\color{Red} \Omega _{ij}}$ 表示该误差所占权重的矩阵。 ${\color{Red} e\left ( X_{i},Y_{i},Z_{ij} \right )}$ 是一个向量误差函数表示 ${\color{Red} X_{i}}$ 和 ${\color{Red} X_{j}}$ 之间的关系与测量 ${\color{Red} Z_{ij}}$ 之间有多吻合。后面为了简化，将误差函数写成下面的形式：

下图为图优化的一个简单例子

对于上述最小二乘问题还可以从最大似然的角度解释。这里也作简单介绍，主要是为了更进一步的理解误差的权重矩阵。对于传感器的测量，我们可以假设它受高斯白噪声的影响。所以每一个测量值的分布可以看作是以真值为中心的高斯分布。如果测量是多变量的，那就是多元高斯分布。

多元高斯分布的协方差矩阵的某一维越大，高斯曲线越矮胖，表示在这个方向上越不确定。并且高斯分布中，均值部位概率最大。所以，对于某个测量，我们应该使它出现在概率最大的地方，这就是最大似然概率。可以得到似然概率的log形式的计算公式：

上式和前面的误差平方和函数很像，只不过这里显式的指明了误差函数的形式。所以我们发现，误差的权重矩阵（正式名称为信息矩阵）等于协防差矩阵的逆。由于图优化里每一条边代表一个测量值，如表示相邻位姿关系的编码器测量值或者图像（激光）匹配得到的位姿变换矩阵。所以图优化里每一条边的信息矩阵就是这些测量协防差矩阵的逆。如果协防差越小，表示这次测量越准越值得相信，信息权重就越大。

我们看到图优化问题变成了求解最小二乘问题，然而机器人位姿之间的变化函数不是线性的，所以是个非线性最小二乘问题，得通过迭代法进行求解。如果迭代开始时有一个好的初始假设值，那我们就能用Guass-Newton法或者 Levenberg-Marquardt法了。

假设我们已经有了好的初始假设值 ${\color{Red}\breve{a} }$ ，需要在这个值附近迭代寻求最优解。求解的方法是把误差函数在该初始值附近进行一节泰勒展开

其中 ${\color{Red}J_{ij} }$ 是误差函数 ${\color{Red}e_{ij}\left ( x \right ) }$ 在 ${\color{Red}\breve{a} }$ 附近的雅克比矩阵。并且为了书写方便，使用 ${\color{Red}e_{ij} }$ 代替了 ${\color{Red}e_{ij}\left ( \breve{x} \right ) }$ 。

将上面的(5)式代入误差平方和中的某一项可以得到

注意，这里 ${\color{Red}b_{ij} }$ 是列向量， ${\color{Red}\Delta x }$ 是列向量， ${\color{Red}e_{ij} }$ 也是列向量， ${\color{Red}c_{ij} }$ 是一个数值。将（10）式代入，可以重写误差平方和函数如下：

其中 $c = \sum c_{ij},\mathbf{b} = \sum b_{ij},\mathbf{H} = \sum H_{ij}$ 。为了求解上式，使得其最小，还是求其一阶导数并使其等于0，可以得

其中H是系统的信息矩阵（注意与边的信息矩阵 ${\color{Red} \Omega _{ij}}$ 区分），系统的解就是在初始值上叠加这个增量：

Guass-Newton迭代法就是不断重复这个过程直到收敛。LM法引入了一个松弛因子来控制迭代速度：

然而，一个值得注意的问题是，上面的步进迭代是用的两个向量直接相加，这是在欧式空间中的做法。而机器人SLAM问题中涉及到平移和旋转，平移是在欧氏空间中，可是旋转就是在非欧空间了。在航迹推演的公式中我们也能看到相邻时刻间位姿的递增由于航向角的存在已经不是简单的相加了。所以得重新定义一个非线性算子来代表增量。

将这个非线性算子用在移动机器人相邻时刻的航迹推演，能够得到：

这里可能有点抽象，别急，在后面2d slam的图优化例子中将具体介绍。

上面的求解过程看起来简单，计算b，H，然后计算增量迭代直到收敛。然而，实际计算的时候，b,H,雅克比矩阵到底是啥得弄清楚。接下来，具体解析它们的结构。

误差函数 ${\color{Red}e_{ij} }$ 只和 ${\color{Red}x_{i} }$ 、 ${\color{Red}x_{j} }$ 有关，因此它的雅克比矩阵和 ${\color{Red}x_{i} }$ 、 ${\color{Red}x_{j} }$ 无关的列都为0，有如下结构:

清楚了雅克比矩阵的结构，再来看b和H。我们已经知道 ${\color{Red}b_{ij} }$ 是列向量，可以推算出结构如下，注意下图中 ${\color{Red} \Omega _{ij}}$ 和 ${\color{Red}e_{ij} }$ 下面的色块代表矩阵和列向量，并且蓝色块代表0，红色块非0。可以看到只有和 ${\color{Red}x_{i} }$ 、 ${\color{Red}x_{j} }$ 有关的区域非零。