前面介绍了基于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对计算及其重要，因为它使得计算可以在简单空间（欧式空间）进行，再把结果传回流形。

优化算法需要封装下面两个操作，

首先定义流形上两个点， $X_{0}$ , $X_{1}$ 。 $\Delta x$ 为他们在同一个Chart上的差（Chart上可以认为是欧式空间）。

已知流形上两点求距离

在 $X_{0}$ 附近创建Chart。
在该Chart上计算 $X_{1}$ 投影的位置坐标。
在该Chart上计算二者的差。

已知流形上一点，和Chart上距离，求另一点的坐标

在 $X_{0}$ 附近创建Chart
在Chart上移动 $\Delta x$
回到流形，得到 $X_{1}$ 坐标。

[2]把上面的图优化公式重写了一下，

边的约束函数，

泰勒展开，

更新，

算法中的一次迭代，

最后，简单说明一下如何具体的定义和，只摆一下结论，推导过程可以参考[4][5]。

定义以x为中心的chart为 $\varphi _{x}$ ,

如果Manifolds是李群，那么可以采用简单一点的表示，只需要定义单位元素附近的一个chart。

以三维旋转为例，

SO(3)由3*3的正交矩阵构成，行列式为1。采用三个参数（欧拉角）表示有奇异性。可以增加一个参数，使用四元组表示。SO(3)又是一个李群，因此，可以定义上文的映射函数 $\varphi (x)$ 为,

$\varphi (q) := log(q)$ , 其中， $q$ 表示四元组， $w$ 为四元组实部， $u$ 为四元组虚部。

$q=(w,u)$ , $w\in R$ ， $u\in R^3$

逆函数定义为 $\varphi ^{'} (\delta) := \exp(\delta )$ 。

计算机视觉大型攻略 —— SLAM(2) Graph-based SLAM（基于图优化的算法)相关推荐

计算机视觉大型攻略 —— 视觉里程计(1) 综述
参考文献: [1] Visual Odometry Part I: The First 30 Years and Fundamentals, Friedrich Fraundorfer and Da ...
计算机视觉大型攻略 —— 特征与匹配（3）特征描述符
接上一篇文章.这篇写特征描述符.特征匹配算法在确定角点后,还需要使用描述符来描述这些角点. 本文参考书籍:Computer Vision: Algorithms and Applications, R ...
计算机视觉大型攻略 —— CUDA(2)执行模型
Professional CUDA C Programming[1]是一本不错的入门书籍,虽说命名为"Professional",但实际上非常适合入门阅读.他几乎涵盖了所有理论部分 ...
SLAM从0到1之图优化g2o：从看懂代码到动手编写（长文）
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文由知乎作者yikang授权转载,不得擅自二次转载.原文链接:https://zhuanlan.zh ...
opencv3计算机视觉python语言实现pdf_对比《OpenCV计算机视觉编程攻略第3版》《OpenCV 3计算机视觉Python语言实现第2版》PDF代码......
OpenCV 3是一种先进的计算机视觉库,可以用于各种图像和视频处理操作,通过OpenCV 3 能很容易地实现一些有前景且功能先进的应用(比如:人脸识别或目标跟踪等).从图像处理的基本操作出发,计算机 ...
OpenCV计算机视觉编程攻略之行人检测
OpenCV计算机视觉编程攻略之行人检测,OpenCV 提供了一个基于HOG 和SVM且经过训练的行人检测器,可以用这个SVM 分类器以不同尺度的窗口扫描图像,在完整的图像中检测特定物体. 原图如下: ...
OpenCV计算机视觉编程攻略之提取图片轮廓-使用Canny函数
OpenCV计算机视觉编程攻略之提取图片轮廓-使用Canny函数,很方便..代码如下: #include <vector> #include <iostream> #inclu ...
OpenCV计算机视觉编程攻略之生成椒盐噪声实现
OpenCV计算机视觉编程攻略(第3版)P21的访问像素值,生成椒盐噪声实现. 运行结果图片,截图如下: 看书留下记录,代码如下: #include <random> #include & ...
opencv计算机视觉编程攻略第2版,OpenCV计算机视觉编程攻略（第2版）pdf
摘要 1. 50多个知识点的案例解读,全面掌握基础知识与进阶内容 2. 学习OpenCV重要的图像操作类和函数 3. 初学者和从业者即查即用的工具书 4. 掌握计算机视觉与图像处理的基础知识与概念 O ...

计算机视觉大型攻略 —— SLAM(2) Graph-based SLAM（基于图优化的算法)