视觉惯性SLAM：理论与源码解析

本书系统介绍以相机和惯性测量单元为主传感器的视觉、视觉惯性SLAM算法。本书通过选取该领域有代表性的两个开源项目ORB-SLAM2/3，从原理阐述、公式推导、代码解析和工程经验等多个维度，对SLAM技术进行全面的解读。为了让读者在轻松的氛围中快速理解专业知识，本书以小白和师兄对话的形式娓娓道来，帮助读者在学习的过程中不断思考和提升。同时，本书秉承“一图胜千言”的理念，把大量复杂的原理或流程绘制成清晰、易懂的示意图，降低了初学者的学习门槛。本书理论和实践并重，引导读者循序渐进地掌握项目实践经验。

本书兼具技术的广度和深度，适合有一定SLAM基础的高等院校学生、科研机构研究人员和企业从业者阅读，尤其适合希望深入研究视觉（惯性）SLAM的算法工程师参考。

程小六，中国科学院博士。计算机视觉life平台创始人，研究兴趣为计算机视觉、机器人定位与建图。

√ SLAM基础理论+ ORB-SLAM2/3核心源码解析+对话体叙述+丰富图表+全书开源代码+详细中文注释+资深专家答疑解惑+全彩印刷极致设计！

√ 高翔、章国锋、邹丹平、秦通、吴克艰、史雪松等专家学者倾情推荐！

√ 带领读者从头到尾学习一个完整的SLAM 项目，从原理解析、代码解读到工程技巧，一步一个脚印地完成。

√ 本书从初学者的视角切入，部分章节以零基础的小白和经验丰富的师兄两人对话的形式阐述。一方面可以把初学者在学习过程中的很多基础问题展现出来，帮助读者在学习过程中不断思考和提升，提高工程实践经验；另一方面，对话这种口语化的表达方式能够让读者在轻松的氛围中快速理解理论知识。

√ 每个重要的知识点都尝试从3 个角度去分析——“What （是什么）”“Why（为什么）”“How（怎么做）”，让读者知其然也知其所以然。

√ 丰富的图示和类比。把大量复杂或难以理解的原理或流程绘制成具象化的图像，一图胜千言，极大地降低了学习门槛。

√ 开源代码配套详细的中文注释。

√ 全彩印刷，提供上乘阅读体验。

前言

人工智能技术按照信息来源主要分为计算机视觉（视觉）、自然语言处理（文本）和语音识别（语音）三大方向。其中，计算机视觉是需求最多、发展最快、应用最广泛的领域。计算机视觉算法通常分为基于学习的方法和基于几何的方法，前者主要指利用深度学习来实现图像识别、物体检测、物体分割、视频理解和图像生成等；后者主要指利用多视图立体几何来实现空间定位、三维重建和测距测绘等。

本书主要聚焦于基于几何方法的计算机视觉核心技术——同步定位与建图（SimultaneousLocalizationandMapping，SLAM）。SLAM技术最早应用于潜艇、太空车等军用领域，之后逐渐进入民用领域。近几年，学术界诞生了大量优秀的SLAM算法框架，并且随着三维传感器的飞速发展和嵌入式设备算力的快速提升，SLAM技术开始大规模商业化应用，包括但不限于自主移动机器人、自动驾驶车辆、增强现实、智能穿戴设备和智能无人机等。

那么什么是SLAM呢？它是指移动智能体从一个未知环境中的未知地点出发，在运动过程中通过自身传感器观测周围环境，根据环境定位自身的位置并进行增量式的地图构建，从而达到同时定位和构建地图的目的。对于初学者来说，很难从晦涩的定义中看懂SLAM技术到底在干什么，也无法理解机器人在应用SLAM技术的过程中有什么难点。其实，人类也能执行定位和建图的任务，为方便读者理解，这里不妨拿人类的探索过程和机器人的视觉SLAM过程来进行类比。

假设我们接到一个任务，需要在不借助专业设备的前提下到一个陌生的地方探索并简单绘制当地的地图。我们如何完成上述任务呢？

- 我们在陌生的起始点用双眼（机器人上安装的视觉传感器）观察四周的环境，并记录那些与众不同的标志物（特征点），从而确定自己的初始位置（地图初始化）。然后我们一边行走，一边观察环境，记录当前位置（定位）并绘制地图（建图）。

- 由于我们探索的是未知的环境，因此难免发生意外。比如我们不小心从山坡上摔下，爬起来后已经不知道自己具体的位置（跟踪丢失）。此时有两种方法，一种方法是重新爬回山坡上，观察周围环境并和已经绘制的地图进行对比，从而确定自己当前在已绘制地图中的位置（重定位）；另一种方法是从山坡下当前地点出发，重新开始绘制新的地图（重新初始化地图）。

- 在探索的过程中还可能会遇到曾经去过的地方，这时我们需要非常谨慎地反复观察对比，确认这里是否真的是已绘制地图中我们曾经走过的某个地方（闭环检测）。由于在绘制地图的过程中有误差（累计漂移），这时相同的地点在地图上很可能无法形成一个闭环，而是一个缺口。一旦确认这是同一个地方，就需要整体调整已经绘制的地图，以便把缺口平滑地衔接起来（闭环矫正）。

随着探索的区域越来越大，我们的地图也越来越完善，如果存在多个地图，则可以根据地图的重叠区域将它们合并为统一的地图（地图融合）。最终我们得到了整个区域的地图，完成任务。

上述括号内加粗部分是SLAM中的常用术语，读者暂时不理解也不用担心，我们会在本书中逐步消化吸收这些术语背后的原理和代码实现。或许有些读者认为上述探索和绘图过程对一个普通人来说不难做到，机器人毫无悬念地应该做得更好。但事实上并非如此。人类在成长的过程中不断观察和学习，更擅长识别物体和场景，但在即时量化计算空间坐标位置方面比较吃力。而机器人等智能设备更擅长精密计算，但在准确识别物体和场景方面先天不足（图像深度学习技术正在改变这个现状）。举一个例子，相机在同一地点不同光照、不同角度、不同远近等情况下拍摄的图像差别非常大，但人类根据经验可以快速而准确地判断这是同一地点。而由于相机拍摄的数字化图像在计算单元中存储的仅仅是不同数字组成的矩阵，让机器人去理解这是同一个场景，进而在不同图像的像素之间建立对应关系是比较困难的，这正是SLAM技术研究的一个难点。

目前SLAM相关书籍还比较少，《视觉SLAM十四讲：从理论到实践》《机器人学中的状态估计》《计算机视觉中的多视图几何》是非常经典的图书，它们涵盖了该领域的核心知识点，公式推导严谨，有的还配有重要概念的代码实现。不过，笔者在和同行学习交流的过程中了解到，很多初学者在掌握了SLAM基础知识后，发现距离真正的项目实践还有较大差距，他们对如何开始自己的第一个SLAM项目实践仍然比较迷茫。本书则致力于解决这个问题，选取了最经典的视觉SLAM框架ORB-SLAM2和2022年综合效果最好的视觉惯性SLAM框架ORB-SLAM3，通过层层拆解、分析，引导读者循序渐进地掌握自己的第一个SLAM项目。

主要特点

- 带领读者从头到尾学习一个完整的SLAM项目，从原理解析、代码解读到工程技巧，一步一个脚印地完成。

- 本书从初学者的视角切入，部分章节以零基础的小白和经验丰富的师兄两人对话的形式阐述。采用对话形式，一方面可以把初学者在学习过程中的很多基础问题展现出来，帮助读者在学习过程中不断思考和提升，提高工程实践经验；另一方面，对话这种口语化的表达方式能够让读者在轻松的氛围中快速理解理论知识。

- 每个重要的知识点都尝试从3个角度去分析——“What（是什么）”“Why（为什么）”“How（怎么做）”，让读者知其然也知其所以然。

- 丰富的图示和类比。我们把大量复杂或难以理解的原理或流程绘制成具象化的图像，一图胜千言，极大地降低了学习门槛。

- 开源代码配套详细的中文注释。

虽然本书讲解的是针对ORB-SLAM系列的原理及代码解析，但其中涉及的知识点同样适用于其他同类算法，学习方法和思路也值得借鉴。

组织方式

本书的内容主要分为三部分。

1.第一部分：介绍SLAM的部分基础知识，它们将在第二、三部分的原理或工程实践中使用。

第1章为SLAM概览。你将了解SLAM的定义、应用场景和应用领域。

第2章为编程及编译工具。你将了解C++11新特性和CMake工具，方便读者看懂代码，提高工程实践能力。

第3章为SLAM中常用的数学基础知识。你将了解齐次坐标和三维空间中刚体旋转的表达方式。

第4章为相机成像模型。你将了解针孔相机模型的背景、推导和相机畸变模型。

第5章为对极几何。你将了解对极几何的基本概念，并从物理意义上理解推导的原理。

第6章为图优化库的使用。你将了解g2o库的编程框架，以及如何自己用g2o库构建图的顶点和边。

2.第二部分：介绍视觉SLAM框架ORB-SLAM2的原理和核心代码。

第7章为ORB特征提取。你将了解ORB特征点的构建及特征点均匀化策略。

第8章为ORB-SLAM2中的特征匹配。你将了解不同场景下使用的不同特征匹配方法，包括单目初始化中的特征匹配、通过词袋进行特征匹配、通过地图点投影进行特征匹配、通过Sim(3)变换进行相互投影匹配。

第9章为地图点、关键帧、图结构。你将了解这3个核心概念，它们贯穿在整个SLAM过程中。

第10章为ORB-SLAM2中的地图初始化。你将了解地图初始化的意义，以及单目模式和双目模式的不同地图初始化方法。

第11章为ORB-SLAM2中的跟踪线程。你将了解参考关键帧跟踪、恒速模型跟踪、重定位跟踪和局部地图跟踪。

第12章为ORB-SLAM2中的局部建图线程。你将了解如何处理新的关键帧、剔除不合格的地图点、生成新的地图点、检查并融合当前关键帧与相邻帧的地图点及关键帧的剔除。

第13章为ORB-SLAM2中的闭环线程。你将了解闭环检测的原因，如可寻找并验证闭环候选关键帧，计算Sim(3)变换，闭环矫正。

第14章为ORB-SLAM2中的优化方法。你将了解跟踪线程仅优化位姿、局部建图线程中局部地图优化、闭环线程中的Sim(3)位姿优化、闭环时本质图优化及全局优化。

3.第三部分：介绍ORB-SLAM2的升级版——视觉惯性系统ORB-SLAM3的主要新增内容和代码。

第15章为ORB-SLAM3中的IMU预积分。你将了解视觉惯性紧耦合的意义、IMU预积分原理及推导、IMU预积分的代码实现。

第16章为ORB-SLAM3中的多地图系统。你将了解多地图的基本概念、多地图系统的效果和作用、创建新地图的方法和时机，以及地图融合。

第17章为ORB-SLAM3中的跟踪线程。你将了解ORB-SLAM3中跟踪线程流程图及跟踪线程的新变化。

第18章为ORB-SLAM3中的局部建图线程。你将了解局部建图线程的作用、局部建图线程的流程及其中IMU的初始化。

第19章为ORB-SLAM3中的闭环及地图融合线程。你将了解共同区域检测、地图融合的具体流程和代码实现。

第20章为视觉SLAM的现在与未来。你将了解视觉SLAM的发展历程、视觉惯性SLAM框架对比及数据集，以及视觉SLAM的未来发展趋势。

配套代码

本书涉及的代码来自ORB-SLAM2和ORB-SLAM3作者开源的代码，我们对该代码中的重点和难点部分进行了中文详细注释，并托管在GitHub上。

https://github.com/electech6/ORB_SLAM2_detailed_comments

https://github.com/electech6/ORB_SLAM3_detailed_comments

GitHub上的注释内容和代码会持续更新。如果本书代码和GitHub上的有出入，则以GitHub上的最新代码及注释为准。

面向读者

本书适合有一定SLAM基础的高等院校学生、科研机构的研究人员和企业从业者阅读，尤其适合希望深入研究视觉（惯性）SLAM的算法工程师参考。建议在学习本书前，读者已经具备如下知识。

- 微积分、线性代数和概率论的基础知识。对于大多数理工科背景的读者来说，读懂本书所需的数学知识已经足够。

- C++编程基础。SLAM领域使用的编程语言主要是C++，所以强烈建议读者事先掌握C++的编程基础，推荐学习《C++Primer》等入门图书。本书第一部分也讲述了部分C++11标准以供参考，它们在代码中出现时能够读懂即可。

- Linux基础。SLAM的开发环境用Linux会非常方便，如果你不了解Linux，则推荐学习《鸟哥的Linux私房菜》等入门图书，只需掌握基础的Linux操作指令即可开始学习。

- SLAM基础知识。本书第一部分并未详细讲解SLAM的完整理论，只是对第二、三部分涉及的基础知识和工程实践技巧进行了介绍。推荐读者在阅读完《视觉SLAM十四讲：从理论到实践》之后再来看本书的内容，会比较容易理解。

风格说明

本书包含理论阐述、公式推导和代码注释，内容较多，为了较好的阅读体验，我们做如下约定。

- 数学公式中标量使用斜体（如??），向量和矩阵使用加粗斜体（如a，A）。

- 为了推导方便，会使用简单符号来代替上括号或下括号内的复杂内容。

- 代码中重要部分或疑难部分都用中文进行了注释。代码中部分不重要的内容会进行省略，并用“……”加以标明。读者可以去GitHub上下载完整的源代码和注释。

- 本书以“*”开头的内容为选学部分，对于初学者来说可能有一定难度，读者可以根据需要选择性地学习。

程小六

2023年1月