视觉SLAM十四讲：从理论到实践

适读人群：本书适合对SLAM感兴趣的读者阅读，也适合有志于从事计算机视觉、机器人研究等领域的广大学生阅读，可作为SLAM技术的入门教材。
SLAM技术是全自动无人驾驶、无人机、机器人等人工智能产品的核心技术之一。
作者是SLAM领域非常杰出的青年专家。
书中不仅有深入浅出的讲解，同时注重理论和实践结合，大大降低了国内学生和相关从业者的进入门槛。

 

本书系统介绍了视觉SLAM（同时定位与地图构建）所需的基本知识与核心算法，既包括数学理论基础，如三维空间的刚体运动、非线性优化，又包括计算机视觉的算法实现，例如多视图几何、回环检测等。此外，还提供了大量的实例代码供读者学习研究，从而更深入地掌握这些内容。本书可以作为对SLAM 感兴趣的研究人员的入门自学材料，也可以作为SLAM 相关的高校本科生或研究生课程教材使用。

　　高翔，2008年就读清华大学自动化系，2012年免试进入清华大学自动化系攻读博士学位。研究课题为视觉SLAM，兴趣包括计算机视觉与机器学习。曾撰写过与SLAM相关的论文和技术博客。
张涛，清华大学自动化系教授、党委书记、副系主任。1995年9月至1999年9月在清华大学自动化系检测技术与自动化装置专业学习，获博士学位。1999年10月至2002年9月在日本国立佐贺大学大学院工学系研究科系统控制专业学习，获博士学位。研究课题包括机器人、航空航天、计算机视觉等。
　　刘毅，华中科技大学图像与人工智能研究所在读博士，本科毕业于武汉理工大学数学系。读博期间专注于图像处理、三维重建、视觉SLAM，以及传感器融合研究和应用，读博期间先后于深圳市大疆创新公司，英特尔中国研究院等单位实习。
颜沁睿，比利时荷语鲁汶大学人工智能硕士，电子工程学士（GroupT）。电子科技大学信息显示与光电技术学士。长期致力于研究人工智能技术在机器人领域的应用，包括计算机视觉、机器学习和SLAM。现担任地平线机器人公司智能驾驶部算法工程师。

第1 讲预备知识 1
1.1 本书讲什么1
1.2 如何使用本书3
1.2.1 组织方式3
1.2.2 代码5
1.2.3 面向的读者6
1.3 风格约定6
1.4 致谢和声明7
第2 讲初识SLAM 9
2.1 引子：小萝卜的例子11
2.2 经典视觉SLAM 框架17
2.2.1 视觉里程计17
2.2.2 后端优化19
2.2.3 回环检测20
2.2.4 建图21
2.3 SLAM 问题的数学表述22
2.4 实践：编程基础 25
2.4.1 安装Linux 操作系统25
2.4.2 Hello SLAM27
2.4.3 使用cmake28
2.4.4 使用库30
2.4.5 使用IDE32
第3 讲三维空间刚体运动37
3.1 旋转矩阵39
3.1.1 点和向量，坐标系39
3.1.2 坐标系间的欧氏变换40
3.1.3 变换矩阵与齐次坐标42
3.2 实践：Eigen 44
3.3 旋转向量和欧拉角48
3.3.1 旋转向量48
3.3.2 欧拉角50
3.4 四元数51
3.4.1 四元数的定义51
3.4.2 四元数的运算53
3.4.3 用四元数表示旋转55
3.4.4 四元数到旋转矩阵的转换55
3.5 * 相似、仿射、射影变换56
3.6 实践：Eigen 几何模块57
3.7 可视化演示60
第4 讲李群与李代数62
4.1 李群与李代数基础 64
4.1.1 群64
4.1.2 李代数的引出65
4.1.3 李代数的定义 67
4.1.4 李代数so(3) 67
4.1.5 李代数se(3)68
4.2 指数与对数映射69
4.2.1 SO(3) 上的指数映射69
4.2.2 SE(3) 上的指数映射.70
4.3 李代数求导与扰动模型72
4.3.1 BCH 公式与近似形式72
4.3.2 SO(3) 李代数上的求导73
4.3.3 李代数求导74
4.3.4 扰动模型（左乘）75
4.3.5 SE(3) 上的李代数求导76
4.4 实践：Sophus76
4.5 * 相似变换群与李代数.79
4.6 小结81
第5 讲相机与图像82
5.1 相机模型 84
5.1.1 针孔相机模型84
5.1.2 畸变87
5.1.3 双目相机模型 90
5.1.4 RGB-D 相机模型92
5.2 图像93
5.3 实践：图像的存取与访问95
5.3.1 安装OpenCV95
5.3.2 操作OpenCV 图像96
5.4 实践：拼接点云99
第6 讲非线性优化104
6.1 状态估计问题106
6.1.1 后验与似然106
6.1.2 小二乘的引出 108
6.2 非线性小二乘109
6.2.1 一阶和二阶梯度法110
6.2.2 高斯牛顿法111
6.2.3 列文伯格—马夸尔特方法113
6.2.4 小结114
6.3 实践：Ceres115
6.3.1 Ceres 简介 116
6.3.2 安装Ceres116
6.3.3 使用Ceres 拟合曲线 117
6.4 实践：g2o121
6.4.1 图优化理论简介121
6.4.2 g2o 的编译与安装122
6.4.3 使用g2o 拟合曲线123
6.5 小结128
第7 讲视觉里程计1130
7.1 特征点法132
7.1.1 特征点132
7.1.2 ORB 特征134
7.1.3 特征匹配137
7.2 实践：特征提取和匹配138
7.3 2D?2D: 对极几何141
7.3.1 对极约束141
7.3.2 本质矩阵143
7.3.3 单应矩阵146
7.4 实践：对极约束求解相机运动148
7.5 三角测量153
7.6 实践：三角测量154
7.6.1 三角测量代码154
7.6.2 讨论156
7.7 3D?2D：PnP157
7.7.1 直接线性变换158
7.7.2 P3P159
7.7.3 Bundle Adjustment 161
7.8 实践：求解PnP165
7.8.1 使用EPnP 求解位姿165
7.8.2 使用BA 优化166
7.9 3D?3D：ICP172
7.9.1 SVD 方法173
7.9.2 非线性优化方法 175
7.10 实践：求解ICP176
7.10.1 SVD 方法176
7.10.2 非线性优化方法178
7.11 小结180
第8 讲视觉里程计2182
8.1 直接法的引出184
8.2 光流（Optical Flow）185
8.3 实践：LK 光流187
8.3.1 使用TUM 公开数据集187
8.3.2 使用LK 光流188
8.4 直接法（Direct Method）192
8.4.1 直接法的推导 192
8.4.2 直接法的讨论195
8.5 实践：RGB-D 的直接法196
8.5.1 稀疏直接法196
8.5.2 定义直接法的边197
8.5.3 使用直接法估计相机运动 199
8.5.4 半稠密直接法200
8.5.5 直接法的讨论 202
8.5.6 直接法优缺点总结 203
第9 讲实践：设计前端205
9.1 搭建VO 框架 206
9.1.1 确定程序框架207
9.1.2 确定基本数据结构208
9.1.3 Camera 类210
9.1.4 Frame 类212
9.1.5 MapPoint 类 213
9.1.6 Map 类 213
9.1.7 Config 类 214
9.2 基本的VO：特征提取和匹配216
9.2.1 两两帧的视觉里程计216
9.2.2 讨论224
9.3 改进：优化PnP 的结果 224
9.4 改进：局部地图 227
9.5 小结233
第10 讲后端1 235
10.1 概述237
10.1.1 状态估计的概率解释237
10.1.2 线性系统和KF239
10.1.3 非线性系统和EKF242
10.1.4 EKF 的讨论243
10.2 BA 与图优化245
10.2.1 投影模型和BA 代价函数 245
10.2.2 BA 的求解247
10.2.3 稀疏性和边缘化248
10.2.4 鲁棒核函数255
10.2.5 小结256
10.3 实践：g2o257
10.3.1 BA 数据集257
10.3.2 g2o 求解BA258
10.3.3 求解262
10.4 实践：Ceres 264
10.4.1 Ceres 求解BA 265
10.4.2 求解267
10.5 小结269
第11 讲后端2 270
11.1 位姿图（Pose Graph）271
11.1.1 Pose Graph 的意义271
11.1.2 Pose Graph 的优化272
11.2 实践：位姿图优化274
11.2.1 g2o 原生位姿图 274
11.2.2 李代数上的位姿图优化278
11.2.3 小结284
11.3 * 因子图优化初步285
11.3.1 贝叶斯网络285
11.3.2 因子图286
11.3.3 增量特性288
11.4 * 实践：gtsam 289
11.4.1 安装gtsam 4.0289
11.4.2 位姿图优化290
第12 讲回环检测297
12.1 回环检测概述299
12.1.1 回环检测的意义299
12.1.2 方法 300
12.1.3 准确率和召回率301
12.2 词袋模型303
12.3 字典 305
12.3.1 字典的结构305
12.3.2 实践：创建字典306
12.4 相似度计算309
12.4.1 理论部分309
12.4.2 实践：相似度的计算310
12.5 实验分析与评述314
12.5.1 增加字典规模314
12.5.2 相似性评分的处理316
12.5.3 关键帧的处理316
12.5.4 检测之后的验证317
12.5.5 与机器学习的关系317
第13 讲建图319
13.1 概述320
13.2 单目稠密重建322
13.2.1 立体视觉322
13.2.2 极线搜索与块匹配323
13.2.3 高斯分布的深度滤波器325
13.3 实践：单目稠密重建328
13.4 实验分析与讨论339
13.4.1 像素梯度的问题339
13.4.2 逆深度340
13.4.3 图像间的变换 341
13.4.4 并行化：效率的问题342
13.4.5 其他的改进343
13.5 RGB-D 稠密建图343
13.5.1 实践：点云地图344
13.5.2 八叉树地图347
13.5.3 实践：八叉树地图350
13.6 *TSDF 地图和Fusion 系列352
13.7 小结356
第14 讲SLAM：现在与未来357
14.1 当前的开源方案358
14.1.1 MonoSLAM358
14.1.2 PTAM359
14.1.3 ORB-SLAM361
14.1.4 LSD-SLAM363
14.1.5 SVO 364
14.1.6 RTAB-MAP366
14.1.7 其他367
14.2 未来的SLAM 话题367
14.2.1 视觉 惯性导航SLAM367
14.2.2 语义SLAM369
14.2.3 SLAM 的未来 370
附录A 高斯分布的性质371
A.1 高斯分布371
A.2 高斯分布的运算371
A.2.1 线性运算371
A.2.2 乘积372
A.2.3 复合运算372
A.3 复合的例子372
附录B ROS 入门374
B.1 ROS 是什么374
B.2 ROS 的特点375
B.3 如何快速上手ROS375
参考文献377

1 本书讲什么
这是一本介绍视觉SLAM 的书，也很可能是本以视觉SLAM 为主题的中文书。那么，SLAM 是什么？
SLAM 是Simultaneous Localization and Mapping 的缩写，中文译作“同时定位与地图构建”[1]。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动[2]。如果这里的传感器主要为相机，那就称为“视觉SLAM”。
本书的主题就是视觉SLAM。这里我们刻意把许多个定义放到一句话中，是希望读者有一个较明确的概念。首先，SLAM 的目的是解决“定位”与“地图构建”这两个问题。也就是说，一边要估计传感器自身的位置，一边要建立周围环境的模型。那么怎么解决呢？这需要用到传感器的信息。传感器能以一定形式观察外部的世界，不过不同传感器观察的方式是不同的。而之所以要花一本书的内容去讨论这个问题，是因为它很难——特别是我们希望实时地、在没有先验知识的情况下进行SLAM。当用相机作为传感器时，我们要做的就是根据一张张连续运动的图像（它们形成了一段视频），从中推断相机的运动，以及周围环境的情况。
这似乎是个很直观的问题。我们自己走进陌生的环境时不就是这么做的吗？
在计算机视觉（Computer Vision）创立之初，人们就想象着有朝一日计算机将和人一样，通过眼睛去观察世界，理解周遭的物体，探索未知的领域——这是一个美妙而又浪漫的梦想，吸引了无数的科研人员日夜为之奋斗[3]。我们曾经以为这件事情并不困难，然而进展却远不如预想的那么顺利。我们眼中的花草树木、虫鱼鸟兽，在计算机中却是那样的不同：它们只是一个个由数字排列而成的矩阵（Matrix）。让计算机理解图像的内容，就像让我们自己理解这些数字一样困难。我们既不了解自己如何理解图像，也不知道计算机该如何理解、探索这个世界。于是我们困惑了很久，直到几十年后的今天，才发现了一点点成功的迹象：通过人工智能（Artificial Intelligence）和机器学习（Machine Learning）技术，计算机渐渐能够辨别出物体、人脸、声音、文字——尽管它所用的方式（概率学建模）与我们是如此不同。另一方面，在SLAM 发展了将近30 年之后，我们的相机才渐渐开始能够认识到自身的位置，发觉自己在运动——虽然方式还是和我们人类有巨大的差异。不过，至少研究者们已经成功地搭建出种种实时SLAM系统，有的能够快速跟踪自身位置，有的甚至能够进行实时的三维重建。
这件事情确实很困难，但我们已经有了很大的进展。更令人兴奋的是，近年来随着科技的发展，涌现出了一大批与SLAM 相关的应用点。在许多地方，我们都希望知道自身的位置：室内的扫地机和移动机器人需要定位，野外的自动驾驶汽车需要定位，空中的无人机需要定位，虚拟现实和增强现实的设备也需要定位。SLAM 是那样重要。没有它，扫地机就无法在房间自主地移动，只能盲目地游荡；家用机器人就无法按照指令准确到达某个房间；虚拟现实也将永远固定在座椅之上——所有这些新奇的事物都无法出现在现实生活中，那将多么令人遗憾。
今天的研究者和应用开发人员，逐渐意识到了SLAM 技术的重要性。在国际上，SLAM 已经有近三十年的研究历史，也一直是机器人和计算机视觉的研究热点。21 世纪以来，以视觉传感器为中心的视觉SLAM 技术，在理论和实践上都经历了明显的转变与突破，正逐步从实验室研究迈向市场应用。同时，我们又遗憾地发现，至少在国内，与SLAM 相关的论文、书籍仍然非常匮乏，让许多对SLAM 技术感兴趣的初学者无从一窥门径。虽然SLAM 的理论框架基本趋于稳定，但其编程实现仍然较为复杂，有着较高的技术门槛。刚步入SLAM 领域的研究者，不得不花很长的时间，学习大量的知识，往往要走过许多弯路才得以接近SLAM 技术的核心。
本书全面系统地介绍了以视觉传感器为主体的视觉SLAM 技术，我们希望它能（部分地）填补这方面资料的空白。我们会详细地介绍SLAM 的理论背景、系统架构，以及各个模块的主流做法。同时，极其重视实践：本书介绍的所有重要算法，都将给出可以运行的实际代码，以求加深读者的理解。之所以这么做，主要是考虑到SLAM 毕竟是一项和实践紧密相关的技术。再漂亮的数学理论，如果不能转化为可以运行的代码，那就仍是可望而不可即的空中楼阁，没有实际意义。我们相信，实践出真知，实践出真爱。只有实际地演算过各种算法之后，你才能真正地认识SLAM，真正地喜欢上科研。
自1986 年提出以来[4]，SLAM 一直是机器人领域的热点问题。关于它的文献数以千计，想要对SLAM 发展史上的所有算法及变种做一个完整的说明，是十分困难而且没有必要的。本书中会介绍SLAM 所牵涉的背景知识，例如射影几何、计算机视觉、状态估计理论、李群李代数等，并在这些背景知识之上，给出SLAM 这棵大树的主干，而略去一部分形状奇特、纹理复杂的枝叶。我们认为这种做法是有效的。如果读者能够掌握主干的精髓，那么自然会有能力去探索那些边缘的、细节的、错综复杂的前沿知识。
所以，我们的目的是，让SLAM 的初学者通过阅读本书快速地成长为能够探索这个领域边缘的研究者。另一方面，即便你已经是SLAM 领域的研究人员，本书也可能有一些你还觉得陌生的地方，可以让你产生新的见解。
目前，与SLAM 相关的书籍主要有《概率机器人》（Probabilistic robotics）[5]、《计算机视觉中的多视图几何》（Multiple View Geometry in Computer Vision）[3]、《机器人学中的状态估计》（State Estimation for Robotics: A Matrix-Lie-Group Approach）[6] 等。它们内容丰富、论述全面、推导严谨，是SLAM 研究者中脍炙人口的经典教材。然而就目前来看，还存在两个重要的问题：其一，这些图书的目的在于介绍基础理论，SLAM 只是其应用之一。因此，它们并不能算是专门讲解SLAM 的书籍。其二，它们的内容偏重于数学理论，基本不涉及编程实现，导致读者经常出现“书能看懂却不会编程”的情况。而我们认为，只有读者亲自实现了算法，调试了各个参数，才能谈得上真正理解了问题本身。
我们会提及SLAM 的历史、理论、算法、现状，并把完整的SLAM 系统分成几个模块：视觉里程计、后端优化、建图，以及回环检测。我们将陪着读者一点点实现这些模块中的核心部分，探讨它们在什么情况下有效，什么情况下会出问题，并指导大家在自己的机器上运行这些代码。你会接触到一些必要的数学理论和许多编程知识，会用到Eigen、OpenCV、PCL、g2o、Ceres 等库，掌握它们在Linux 操作系统中的使用方法。
从写作风格上，我们不想把本书写成枯燥的理论书籍。技术类图书应该是严谨可靠的，但严谨不意味着刻板。一本优秀的技术书应该是生动有趣而易于理解的。如果你觉得“这个作者怎么这么不正经”，敬请原谅，因为我并不是一个非常严肃的人。无论如何，有一件事是可以肯定的：只要你对这门新技术感兴趣，在学习本书的过程中肯定会有所收获！您会掌握与SLAM 相关的理论知识，你的编程能力也将有明显的进步。
在很多时候，您会有一种“我在陪你一起做科研”的感觉，这正是我所希望的。但愿您能在此过程中发现研究的乐趣，喜欢这种“通过一番努力，看到事情顺利运行”的成就感。
好了，话不多说，祝你旅行愉快！
2 如何使用本书
组织方式。本书名为“视觉SLAM 十四讲”。顾名思义，我们会像在学校里讲课那样，以“讲”作为本书的基本单元。每一讲都对应一个固定的主题，其中会穿插“理论部分”和“实践部分”两种内容。通常是理论部分在前，实践部分在后。在理论部分中，我们将介绍理解算法所必需的数学知识，并且大多数时候以叙述的方式，而不是像数学教科书那样用“定义—定理—推论”的方式，因为我们觉得这样的方式阅读起来更容易一些，尽管有时候显得不那么严谨。实践部分主要是编程实现，讨论程序里各部分的含义及实验结果。看到标题中带有“实践”两个字的章节，你就应该（兴致勃勃地）打开电脑，和我们一起愉快地编写代码了。
值得一提的是，我们只会把与解决问题相关的数学知识放在书里，并尽量保持浅显。虽然我们是工科生，但也要承认，某些做法只要经验上够用，没必要非得在数学上追求完备。只要我们知道这些算法能够工作，并且数学家们告诉了我们在什么情况下可能不工作，那么我们就表示满意，而不追究那些看似完美但实际复杂冗长的证明（当然它们固有自己的价值）。由于SLAM 牵涉到了太多数学背景，为了防止使本书变成数学教科书，我们把一些细节上的推导和证明留作习题和补充阅读材料，方便感兴趣的读者进一步阅读参考文献，更深入地掌握相关细节。
每一讲正文之后，我们设计了一些习题。其中，带* 号的习题是具有一定难度的。我们强烈建议读者把习题都练习一遍，这对你掌握这些知识很有帮助。
全书内容主要分为两个部分。
1. 部分为数学基础篇，我们会以浅显易懂的方式，铺垫与视觉SLAM 相关的数学知识，包括：

隆重向读者推荐《视觉SLAM十四讲：从理论到实践》。一方面本书是业界少有的涵盖从基础理论到代码实例，系统性讲解SLAM的书；另一方面，本书的作者和地平线颇有渊源，高翔曾经是我们的算法实习生，颜沁睿是自动驾驶算法工程师，都是在SLAM领域非常杰出的青年专家，走在技术实践前沿。在移动互联网大潮之后，自动驾驶、无人机、服务机器人等人工智能硬件会成为下一个产业爆发点，其中关键的技术之一就是动态定位和环境建模的SLAM技术。本书是国内非常有价值的有关SLAM技术的书籍，适合有志于从事机器人技术的研究生和工程师，一定会让读者很有收获。
地平线机器人创始人，中国人工智能学会副秘书长，余凯

我在新加坡和加拿大给学生讲视觉SLAM时常常觉得缺乏一本适合初学者的教材。高翔博士的《视觉SLAM十四讲：从理论到实践》从基础的四元数、李代数讲起，涵盖了卡尔曼滤波、Bundle Adjustment、Pose-Graph等高级优化工具。书中更有zui近十多年成功系统的概述，从2003年的MonoSLAM直到2016年的ORB-SLAM。通篇既有清晰的理论叙述，又辅以大量示例程序，是一本非常好的视觉SLAM教材。
                                                                                                                                                加拿大西蒙弗雷泽大学终身教授，谭平

视觉SLAM随着近年增强现实、无人驾驶等应用的兴起而重新获得重大关注。视觉SLAM属于计算机视觉和机器人研究的交叉领域，因此涉及的基础知识广而分散。国内专门的研究机构相对较少，因此学生入门的门槛较高。幸运的是，本书不仅有深入浅出的讲解，同时注重理论和实战的结合，大大降低了国内学生和相关从业者的进入门槛。因此，本书非常值得初学者学习实践。
                                                                                                                                 网易感知与智能中心增强现实算法架构师，刘海伟

作者的这本书既是通俗有趣的高科技演义，又是足以指导研发实践的翔实教程，对国内SLAM 界而言可谓意义重大。我甚至发现有不少目前圈内的一流人才都是因为看了本书的早期章节才决定进入这个行业并快速成长起来的。本书里所涵盖的知识面、技术细节，甚至是某些宝贵的实践经验，对国内刚刚起步的虚拟现实和增强现实（VR/AR）、无人机、无人车、机器人等行业而言，都将产生深远影响！
                                                                                                                  uSens凌感科技联合创始人/首席运营官，时驰（Chris）博士