视觉SLAM十四讲笔记（一）：第一讲+第二讲

2019/08/08

第一讲是对本书内容的概述，第二讲是对SLAM的概述

第一讲 前言

全书的内容主要分为两个部分：
1. 第一部分为数学基础篇，我们会以浅显易懂的方式，铺垫与视觉 SLAM 相关的数学知识，包括：

• 第一讲是前言，介绍这本书的基本信息，习题部分主要包括一些自测题。
• 第二讲为 SLAM 系统概述，介绍一个 SLAM 系统由哪些模块组成，各模块的具体工作是什么。实践部分介绍编程环境的搭建过程以及 IDE 的使用。
• 第三讲介绍三维空间运动，你将接触旋转矩阵、四元数、欧拉角的相关知识，并且在 Eigen 当中使用它们。
• 第四讲为李群和李代数。如果你现在不懂李代数为何物，也没有关系。你将学习李代数的定义和使用方式，然后通过 Sophus 操作它们。
• 第五讲介绍针孔相机模型以及图像在计算机中的表达。你将用 OpenCV 来调取相机的内外参数。
• 第六讲介绍非线性优化，包括状态估计理论基础、最小二乘问题、梯度下降方法。你会完成一个使用 Ceres 和 g2o 进行曲线拟合的实验。

这些就是我们所有的数学了，当然还有你以前学过的高等数学和线性代数。我保证它们看起来都不会很难——当然若你想进一步深入挖掘，我们会提供一些参考资料供你阅读，那些材料可能会比正文里讲的那些难一点。

2. 第二部分为 SLAM 技术篇。我们会使用第一部分所介绍的理论，讲述视觉 SLAM中各个模块的工作原理。

• 第七讲为特征点法的视觉里程计。该讲内容比较多，包括特征点的提取与匹配、对极几何约束的计算、 PnP 和 ICP 等。在实践中，你将用这些方法去估计两个图像之间的运动。
• 第八讲为直接法的视觉里程计。你将学习光流和直接法的原理，然后利用 g2o实现一个简单的 RGB-D 直接法。
• 第九讲为视觉里程计的实践章，你将搭建一个视觉里程计框架，综合应用先前学过的知识，实现它的基本功能。从中你会碰到一些问题，例如优化的必要性、关键帧的选择等。
• 第十讲为后端优化，主要为 Bundle Adjustment 的深入讨论，包括基本的 BA以及如何利用稀疏性加速求解过程。你将用 Ceres 和 g2o 分别书写一个 BA 程序。
• 第十一讲主要讲后端优化中的位姿图。位姿图是表达关键帧之间约束的一种更紧凑的形式。你将用 g2o 和 gtsam 对一个位姿球进行优化。
• 第十二讲为回环检测，我们主要介绍以词袋方法为主的回环检测。你将使用dbow3 书写字典训练程序和回环检测程序。
• 第十三讲为地图构建。我们会讨论如何使用单目进行稠密深度图的估计（以及这是多么不可靠），然后讨论 RGB-D 的稠密地图构建过程。你会书写极线搜索与块匹配的程序，然后在 RGB-D 中遇到点云地图和八叉树地图的构建问题。
• 第十四讲主要介绍当前的开源 SLAM 项目以及未来的发展方向。相信阅读了前面的知识之后，你会更容易理解它们的原理，实现自己的新想法。

最后，如果你完全看不懂上面在说什么，恭喜你！这本书很适合你！加油！

本书所有源代码均托管到 github 上：https://github.com/gaoxiang12/slambook

 

第二讲 初识 SLAM

2.1 SLAM中的相机

SLAM中使用的相机，我们把相机分为单目（Monocular）、双目（Stereo）和深度相机（RGB-D）三个大类。此外， SLAM 中还有全景相机、 Event 相机等特殊或新兴的种类。虽然偶尔能看到它们在 SLAM 中的应用，不过到目前为止还没有成为主流。这三类相机的缺点：

• 单目相机：平移之后才能计算深度，以及无法确定真实尺度。
• 双目以及多目相机：配置与标定均较为复杂，其深度量程和精度受双目的基线与分辨率限制，而且视差的计算非常消耗计算资源，需要使用 GPU 和 FPGA 设备加速后，才能实时输出整张图像的距离信息。因此在现有的条件下，计算量是双目的主要问题之一。
• RGBD相机：测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题，在 SLAM 方面，主要用于室内 SLAM，室外则较难应用。
   

2.2 经典视觉SLAM框架
 

视觉 SLAM 流程图

我们把整个视觉 SLAM 流程分为以下几步：

1. 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
2. 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动，以及局部地图的样子。 VO 又称为前端（Front End）。
3. 后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在 VO 之后，又称为后端（Back End）。
4. 回环检测（Loop Closing）。回环检测判断机器人是否曾经到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
5. 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。经典的视觉 SLAM 框架是过去十几年内，研究者们总结的成果。

这个框架本身，以及它所包含的算法已经基本定型，并且在许多视觉程序库和机器人程序库中已经提供。依靠这些算法，我们能够构建一个视觉 SLAM 系统，使之在正常的工作环境里实时进行定位与建图。因此，我们说， 如果把工作环境限定在静态、刚体，光照变化不明显、没有人为干扰的场景，那么，这个 SLAM 系统是相当成熟的了。

2.2.1 视觉里程计

视觉里程计关心相邻图像之间的相机运动，最简单的情况当然是两张图像之间的运动关系。

VO 能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。叫它为“里程计”是因为它和实际的里程计一样，只计算相邻时刻的运动，而和再往前的过去的信息没有关联。

假定我们已有了一个视觉里程计估计了两张图像间的相机运动。那么，只要把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一方面，我们根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。

然而，仅通过视觉里程计来估计轨迹，将不可避免地出现累计漂移（Accumulating Drift）。这是由于视觉里程计（在最简单的情况下）只估计两个图像间运动造成的。每次估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻，导致经过一段时间之后，估计的轨迹将不再准确。

误差积累导致漂移后的地图与定位（左）
后端优化、回环检测后的地图与定位（右）

所谓的漂移（Drift），它将导致我们无法建立一致的地图。你会发现原本直的走廊变成了斜的，而原本 90 度的直角变成了歪的——这实在是一件很难令人忍受的事情！为了解决漂移问题，我们还需要两种技术： 后端优化和回环检测。回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。

2.2.2 后端优化

 后端优化主要指处理 SLAM 过程中噪声的问题。

后端优化要考虑的问题，就是如何从这些带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori， MAP），这里的状态既包括机器人自身的轨迹，也包含地图。

在视觉 SLAM 中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。

2.2.3 回环检测

回环检测，又称闭环检测（Loop Closure Detection），主要解决位置估计随时间漂移的问题。

为了实现回环检测，我们需要让机器人具有识别曾到达过的场景的能力。我们可以判断图像间的相似性，来完成回环检测。如果回环检测成功，可以显著地减小累积误差。所以视觉回环检测，实质上是一种计算图像数据相似性的算法。在检测到回环之后，我们会把“A 与 B 是同一个点”这样的信息告诉后端优化算法。然后，后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。这样，如果我们有充分而且正确的回环检测，就可以消除累积误差，得到全局一致的轨迹和地图。

2.2.4 建图

建图（Mapping）是指构建地图的过程。地图是对环境的描述，但这个描述并不是固定的，需要视 SLAM 的应用而定。

多种地图

 一组空间点的集合也可以称为地图，一个漂亮的 3D 模型亦是地图，一个标记着城市、村庄、铁路、河道的图片亦是地图。地图
的形式随 SLAM 的应用场合而定。大体上讲，它们可以分为度量地图与拓扑地图两种。

度量地图（Metric Map）

度量地图强调精确地表示地图中物体的位置关系，通常我们用稀疏（Sparse）与稠密（Dense）对它们进行分类。

• 稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标（Landmark），那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略掉。
• 相对的，稠密地图着重于建模所有看到的东西。

对于定位来说，稀疏路标地图就足够了。而用于导航时，我们往往需要稠密的地图（否则撞上两个路标之间的墙怎么办？）。

稠密地图通常按照某种分辨率，由许多个小块组成。二维度量地图是许多个小格子（Grid），三维则是许多小方块（Voxel）。一般地，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。当我们查询某个空间位置时，地图能够给出该位置是否可以通过的信息。这样的地图可以用于各种导航算法，如 A*,D*‹等等，为机器人研究者们所重视。但是我们也看到，这种地图需要存储每一个格点的状态，耗费大量的存储空间，而且多数情况下地图的许多细节部分是无用的。另一方面，大规模度量地图有时会出现一致性问题。很小的一点转向误差，可能会导致两间屋子的墙出现重叠，使得地图失效。

拓扑地图（Topological Map）

相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。

拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性，例如 A， B 点是连通的，而不考虑如何从 A 点到达 B 点的过程。它放松了地图对精确位置的需要，去掉地图的细节问题，是一种更为紧凑的表达方式。然而，拓扑地图不擅长表达具有复杂结构的地图。如何对地图进行分割形成结点与边，又如何使用拓扑地图进行导航与路径规划，仍是有待研究的问题。

 2.3 SLAM 问题的数学表述

通过前面部分的介绍，对 SLAM 中各个模块的组成和主要功能有了直观上的理解。现在，要把它上升到理性层次——也就是用数学语言来描述 SLAM 过程。

 假设机器人（携带着某种传感器）在未知环境里运动， 表示某个时刻，表示对应时刻机器人位置，它们构成了机器人的轨迹。

地图方面，设地图是由许多个路标（Landmark）组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有 N 个，用 表示它们。

在这样设定中，“机器人携带着传感器在环境中运动”，由如下两件事情描述：

1. 运动：从 时刻到  时刻，机器人的位置 是如何变化的。
2. 观测：假设机器人在  时刻，于 处探测到了某一个路标 ，这件事情是如何用数学语言来描述的。

运动方程公式——表示上一刻的位置与这一刻位置的关系：

其中：
是运动传感器读数（也叫输入），是噪声。这个公式被称为运动方程。

观测方程公式——机器人在 位置上看到某个路标点 ，产生了一个观测数据 ，用一个抽象的函数 h 来描述这个关系：

其中：
是观测噪声。

这两个方程描述了最基本的 SLAM 问题：知道运动测量的读数 u，以及传感器的读数 z 时，求解定位问题（估计 x）和建图问题（估计 y）。把 SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。其中线性高斯系统（Linear Gaussian, LG 系统）是最简单的，它的无偏的最优估计可以由卡尔曼滤波器（Kalman Filter, KF）给出。而在复杂的非线性非高斯系统（Non-Linear Non-Gaussian， NLNG 系统）中，我们会使用以扩展卡尔曼滤波器（Extended Kalman Filter, EKF）和非线性优化两大类方法去求解它。

最早的实时视觉 SLAM 系统即是基于 EKF开发的。随后，为了克服 EKF 的缺点（例如线性化误差和噪声高斯分布假设），人们开始使用粒子滤波器（Particle Filter）等其他滤波器，乃至使用非线性优化的方法。时至今日，主流视觉 SLAM 使用以图优化（Graph Optimization）为代表的优化技术进行状态估计 。我们认为优化技术已经明显优于滤波器技术，只要计算资源允许，我们通常都偏向于使用优化方法（见第十、十一讲）。