前面介绍了基于EKF的SLAM算法。EKF算法由于状态向量，协方差矩阵的大小随着特征点(路标)的增长而迅速增长，导致其不太适合大场景的应用。本文描述基于图优化的SLAM算法。目前由于SLAM图的稀疏性得到广泛认可，这种SLAM在效果和效率上的优势非常明显。

参考文献：

[1] Probabilistic Robotics

[2] A Tutorial on Graph-Based SLAM

[3] Globally consistent range scan alignment for environment mapping. Autonomous Robots

[4] A Framework for Sparse, Non-Linear Least Squares Problems on Manifolds

[5] INTRODUCTION TO SMOOTH MANIFOLDS

Graph-based SLAM

[3]最早提出了采用全局优化的图优化SLAM算法。下图摘自[1] Probabilistic Robotics。用图G（V, E）的方式表述了SLAM问题。

上图中， $left {x_{0} , x_{1}, ..., x_{n} right }$ 表示机器人的位姿(Pose)， $left {m_{0} , m_{1}, ..., m_{n} right }$ 表示路标(Landmark)的坐标。

图优化中，顶点是优化项，而边是约束项。优化过程就是通过全局调整优化项，使约束项的和最小。在上图中，

Pose(x)与Landmark(m)构成了图的顶点集。
边(Edge)存在两种构成。第一种是Pose到Pose，另一种是Pose到Landmark。
图优化中边表示约束，Pose到Pose的边与机器人的运动模型相关，机器人从 $x_{0}$ 运动到 $x_{1}$ ，形成了这条边。同时在运动模型 $g_{(control, Pose)}$ 的作用下，可以获得机器人在 $x_{0}$ ，控制量为 $u_{1}$ 时的状态为 $g_{(u_{1}, x_{1})}$ 。该值与“真实”的Pose $x_{1}$ 的差，构成了这条边的约束。由于边的约束是标量，因此取这个差的平方。R表示为运动模型的协方差。公式如下，

Pose到Landmark的边与机器人的观测模型相关，机器人从 $x_{1}$ 观测到 $m_{1}$ ，就会形成一条边。同时在观测模型 $h_{(measure, Pose)}$ 的作用下，可以获得机器人在 $x_{1}$ 时对m1的预测 $h_{(m_{1}, x_{1})}$ 。该值与此时真正观测到的值 $z_{1}$ 的差，构成了这条边的约束。同样的，取这个差的平方作为约束值。Q表示为观测模型的协方差。公式如下，

完整的约束为前两个约束的和，

SLAM的图优化问题就是寻找合适的Pose(x), Landmark(m)，使 $J_{GraghSLAM}$ 最小。与基于EKF的SLAM算法随着机器人运动而不停的优化Pose和地图不同，Graph-based SLAM算法会首先创建图，之后再优化这个完整的图，因此也被称作"delayed" SLAM算法或"Full" SLAM。创建图的过程和传感器相关，也被称作前端(front-end)算法，优化图的算法与传感器关系不大，也称为后端(back-end)算法。

SLAM的图模型通常用信息矩阵表示。[1]第十一章Figure11.2，中给出了图模型与信息矩阵的图示。两种表达方式都包含了地图信息和路径信息。

考虑到运动模型与观测函数都不是线性的，最优化上述函数是一个非线性最小化的过程。然而直接优化上面这个函数的复杂度非常高。GraphSLAM算法进一步分解上面的信息矩阵，消去了landmark节点(m)，有效降低计算复杂度。

此时，图模型中的节点仅仅是机器人的Pose。因此也被称作Pose Graph.

Pose-Graph

Pose-Graph图中的节点定义为机器人的Pose。

边有两种，

时间上相邻的节点存在一条边 $e_{(i, i+1)}$ 。这条边通常是运动模型提供的约束（如里程计，轮速仪等）。
如果在 $x_{i}$ 和 $x_{j}$ 看到了同样的路标，那么在这两个节点之间存在一条边 $e_{(i, j)}$ 。这条边的约束由对路标的观测模型提供。

如此一来，图中的节点大大减少。这两种边可以统称为一种"虚拟观测"(virtural measurement)。

令节点 $x=left { x_{1},...,x_{n} right }$ 表示节点向量。 $z_{(i, j)}$ 是虚拟观测的均值，方差由信息矩阵 $Omega _{i,j}$ 表示。 $hat z_{i,j}(x_{i}, x_{j})$ 是已知 $x_{i}$ 和 $x_{j}$ ，对观测的预测。 $l_{(i, j)}$ 是 $z_{(i, j)}$ 似然函数的对数。

那么 $l_{(i, j)}$ 可由预测的观测与观测的实际值的差求出。

进一步简化这个公式，令 $e_{i,j}(x_{i}, x_{j}) = z_{ij}-hat z_{ij}(x_{i},x_{j})$ ，可以得到，

问题的求解变成了寻找合适的节点向量 $x^{*}$ 以使得F(x)得值最小。非线性最优化可采用最小二乘，牛顿高斯，Levenberg-Marquardt法。

首先，在初始guess $check{x_{i}}$ 一阶泰勒展开， $J_{ij}$ 为雅克比矩阵。

进一步代入，得到论文[2]的公式（8）~（11）。

F(x)是F(i,j)的和，

对这个二次方程求极值，可令公式(14)两边对 $Delta x$ 导数，并令其导数为0。问题变成了线性系统求解。

由于矩阵H是信息矩阵，本身是稀疏的。非零数等于节点数+约束边*2。可以用cholesky分解。

得到优化的结果。

牛顿高斯算法迭代公式(14), (15)，每次迭代的结果作为下次迭代的Initial Guess。

非欧几里得空间下的优化

以上是一个当x为欧式空间下的典型的非线性优化。然而~~现实总是残酷的~~，SLAM中的参数并不全都是欧式空间的。

x通常由三维空间的旋转平移构成，

$X=left {x, y, z, alpha_{x}, alpha_{y}, alpha_{z} right }$

两相邻结点的"观测"函数一般写作

$h_{i}(x) = R(alpha)p_{i}+t$

平移变量t显然是欧式空间，而旋转变量的表达式（通常是欧拉角）属于非欧式空间。简单地讲，就是旋转或者旋转平移矩阵对加法不封闭（两个旋转矩阵相加不是旋转矩阵，两个旋转平移矩阵相加不是旋转平移矩阵）。也就是说，公式(16)的加法可能无法成立。

考虑到四元数(quaternion)表达3维空间的旋转是没有奇异性的，那么就可以使用四元组替代欧拉角表示3维旋转。然而四元数的计算和对旋转的表达有些复杂，算法也需要重新修改，这时需要一个折中的方法。借用流形的概念(Manifolds），再结合四元组的表达，就可以在不改变算法的情况下，解决奇异性问题。例如，当计算两个点在流形（非欧式空间）上的距离时，首先在其中一个点的附近建立欧式空间的映射，然后把另一个点映射过去，再计算他们的距离。这样的好处是我们完全不需要重新推导公式。只需要在原算法中定义“加”，“减”两个操作（映射），将两个操作封装，而这两个封装可以使用四元组的对数实现。更详细的内容可以参考[4]。[5]给出了关于Smooth Manifold的详细说明。

流形(Manifolds)是局部欧几里得空间化的一个拓扑空间。欧式空间就是最简单的流形。地球表面这样的球面这是一个稍微复杂的例子。

平滑流形(Smooth Manifolds), 或称微分流形(Differential Manifold)，可微流形，是一个被赋予了光滑结构的拓扑流形。

坐标卡(Cooridinate chart)，简称卡(Chart)，是一个在流形的一个开子集和一个简单空间的映射，使得该映射及其逆都保持所要的结构。对于拓扑流形，该简单空间是某个欧几里得空间 $R^{n}$ 。Chart对计算及其重要，因为它使得计算可以在简单空间（欧式空间）进行，再把结果传回流形。