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

    高翔，清华大学自动化系博士，慕尼黑工业大学博士后。研究兴趣主要为计算机视觉、定位与建图、机器学习等，主要著、译作包括《视觉SLAM十四讲：从理论到实践》、《机器人学中的状态估计》，在RAS、Auto Robotics、IROS等期刊会议上发表论文，现从事自动驾驶车辆研发工作。

本书可以作为对 SLAM 感兴趣的研究人员的入门自学材料，也可以作为 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 致谢和声明. 8

第 1 部分 数学基础 10

第 2 讲 初识 SLAM.11

2.1 引子：小萝卜的例子 . 13

2.2 经典视觉 SLAM 框架  19

2.2.1 视觉里程计. 20

2.2.2 后端优化 21

2.2.3 回环检测 22

2.2.4 建图. 23

2.3 SLAM 问题的数学表述 24

2.4 实践：编程基础 27

2.4.1 安装 Linux 操作系统 27

2.4.2 Hello SLAM 29

2.4.3 使用 cmake . 30

2.4.4 使用库 . 32

2.4.5 使用 IDE  35

第 3 讲 三维空间刚体运动 40

3.1 旋转矩阵 42

3.1.1 点、向量和坐标系. 42

3.1.2 坐标系间的欧氏变换 . 43

3.1.3 变换矩阵与齐次坐标 . 46

3.2 实践：Eigen 47

3.3 旋转向量和欧拉角. 53

3.3.1 旋转向量 53

3.3.2 欧拉角 . 54

3.4 四元数 . 56

3.4.1 四元数的定义 . 56

3.4.2 四元数的运算 . 57

3.4.3 用四元数表示旋转. 59

3.4.4 四元数到其他旋转表示的转换 59

3.5 * 相似、仿射、射影变换 61

3.6 实践：Eigen 几何模块 . 62

3.6.1 Eigen 几何模块的数据演示. 62

3.6.2 实际的坐标变换例子 . 64

3.7 可视化演示. 66

3.7.1 显示运动轨迹 . 66

3.7.2 显示相机的位姿 69

第 4 讲 李群与李代数 71

4.1 李群与李代数基础. 73

4.1.1 群 73

4.1.2 李代数的引出 . 74

4.1.3 李代数的定义 . 76

4.1.4 李代数 so(3). 76

4.1.5 李代数 se(3) . 77

4.2 指数与对数映射 78

4.2.1 SO(3) 上的指数映射 78

4.2.2 SE(3) 上的指数映射  80

4.3 李代数求导与扰动模型 81

4.3.1 BCH 公式与近似形式  81

4.3.2 SO(3) 上的李代数求导. 83

4.3.3 李代数求导. 84

4.3.4 扰动模型（左乘）. 85

4.3.5 SE(3) 上的李代数求导. 85

4.4 实践：Sophus 86

4.4.1 Sophus 的基本使用方法 . 86

4.4.2 例子：评估轨迹的误差 89

4.5 * 相似变换群与李代数 . 92

4.6 小结. 93

第 5 讲 相机与图像. 95

5.1 相机模型 97

5.1.1 针孔相机模型 . 97

5.1.2 畸变模型 100

5.1.3 双目相机模型 . 103

5.1.4 RGB-D 相机模型. 104

5.2 图像. 106

5.3 实践：计算机中的图像 107

5.3.1 OpenCV 的基本使用方法 . 107

5.3.2 图像去畸变. 112

5.4 实践：3D 视觉 113

5.4.1 双目视觉 113

5.4.2 RGB-D 视觉 . 115

第 6 讲 非线性优化. 119

6.1 状态估计问题 . 121

6.1.1 批量状态估计与*大后验估计 121

6.1.2 *小二乘的引出 123

6.1.3 例子：批量状态估计 . 125

6.2 非线性*小二乘 126

6.2.1 一阶和二阶梯度法. 127

6.2.2 高斯牛顿法. 128

目录 vii

6.2.3 列文伯格—马夸尔特方法. 130

6.3 实践：曲线拟合问题 . 132

6.3.1 手写高斯牛顿法 132

6.3.2 使用 Ceres 进行曲线拟合 . 136

6.3.3 使用 g2o 进行曲线拟合 141

6.4 小结. 148

第 2 部分 实践应用 150

第 7 讲 视觉里程计 1. 151

7.1 特征点法 153

7.1.1 特征点 . 153

7.1.2 ORB 特征. 155

7.1.3 特征匹配 158

7.2 实践：特征提取和匹配 159

7.2.1 OpenCV 的 ORB 特征  159

7.2.2 手写 ORB 特征 . 162

7.2.3 计算相机运动 . 165

7.3 2D?2D：对极几何. 165

7.3.1 对极约束 165

7.3.2 本质矩阵 168

7.3.3 单应矩阵 170

7.4 实践：对极约束求解相机运动 172

7.5 三角测量 177

7.6 实践：三角测量 178

7.6.1 三角测量代码 . 178

7.6.2 讨论. 179

7.7 3D?2D：PnP. 180

7.7.1 直接线性变换 . 180

7.7.2 P3P  182

7.7.3 *小化重投影误差求解 PnP 184

7.8 实践：求解 PnP  188

7.8.1 使用 EPnP 求解位姿  188

7.8.2 手写位姿估计 . 189

7.8.3 使用 g2o 进行 BA 优化  191

7.9 3D?3D：ICP. 196

7.9.1 SVD 方法 . 196

7.9.2 非线性优化方法 198

7.10 实践：求解 ICP. 199

7.10.1 实践：SVD 方法 . 199

7.10.2 实践：非线性优化方法 201

7.11 小结. 203

第 8 讲 视觉里程计 2. 205

8.1 直接法的引出 . 207

8.2 2D 光流 208

8.3 实践：LK 光流 . 210

8.3.1 使用 LK 光流 210

8.3.2 用高斯牛顿法实现光流 211

8.3.3 光流实践小结 . 218

8.4 直接法 . 218

8.4.1 直接法的推导 . 218

8.4.2 直接法的讨论 . 221

8.5 实践：直接法 . 221

8.5.1 单层直接法. 221

8.5.2 多层直接法. 226

8.5.3 结果讨论 227

8.5.4 直接法优缺点总结. 230

第 9 讲 后端 1 232

9.1 概述. 234

9.1.1 状态估计的概率解释 . 234

9.1.2 线性系统和 KF 236

9.1.3 非线性系统和 EKF  239

9.1.4 EKF 的讨论  241

9.2 BA 与图优化. 242

9.2.1 投影模型和 BA 代价函数. 242

9.2.2 BA 的求解 243

9.2.3 稀疏性和边缘化 245

9.2.4 鲁棒核函数. 251

9.3 实践：Ceres BA  253

9.3.1 BAL 数据集 253

9.3.2 Ceres BA 的书写 . 253

9.4 实践：g2o 求解 BA . 257

9.5 小结. 263

第 10 讲 后端 2 265

10.1 滑动窗口滤波和优化 . 266

10.1.1 实际环境下的 BA 结构  266

10.1.2 滑动窗口法. 267

10.2 位姿图 . 270

10.2.1 位姿图的意义 . 270

10.2.2 位姿图的优化 . 270

10.3 实践：位姿图优化. 272

10.3.1 g2o 原生位姿图. 272

10.3.2 李代数上的位姿图优化 277

10.3.3 小结. 282

第 11 讲 回环检测 283

11.1 概述. 285

11.1.1 回环检测的意义 285

11.1.2 回环检测的方法 286

11.1.3 准确率和召回率 287

11.2 词袋模型 289

11.3 字典. 291

11.3.1 字典的结构. 291

11.3.2 实践：创建字典 292

11.4 相似度计算. 295

11.4.1 理论部分 295

11.4.2 实践：相似度的计算 . 296

11.5 实验分析与评述 300

11.5.1 增加字典规模 . 300

11.5.2 相似性评分的处理. 302

11.5.3 关键帧的处理 . 302

11.5.4 检测之后的验证 303

11.5.5 与机器学习的关系. 303

第 12 讲 建图 305

12.1 概述. 306

12.2 单目稠密重建 . 308

12.2.1 立体视觉 308

12.2.2 极线搜索与块匹配. 309

12.2.3 高斯分布的深度滤波器 311

12.3 实践：单目稠密重建 . 314

12.3.1 实验分析与讨论 323

12.3.2 像素梯度的问题 324

12.3.3 逆深度 . 325

12.3.4 图像间的变换 . 326

12.3.5 并行化：效率的问题 . 327

12.3.6 其他的改进. 327

12.4 RGB-D 稠密建图. 328

12.4.1 实践：点云地图 328

12.4.2 从点云重建网格 333

12.4.3 八叉树地图. 336

12.4.4 实践：八叉树地图. 338

12.5 * TSDF 地图和 Fusion 系列. 340

12.6 小结. 343

第 13 讲 实践：设计 SLAM 系统.345

13.1 为什么要单独列工程章节. 346

13.2 工程框架 347

13.3 实现. 349

13.3.1 实现基本数据结构. 349

13.3.2 前端. 354

13.3.3 后端. 357

13.4 实验效果 361

第 14 讲 SLAM：现在与未来. 363

14.1 当前的开源方案 364

14.1.1 MonoSLAM  364

14.1.2 PTAM  365

14.1.3 ORB-SLAM  366

14.1.4 LSD-SLAM. 369

14.1.5 SVO. 370

14.1.6 RTAB-MAP. 371

14.1.7 其他. 372

14.2 未来的 SLAM 话题 372

14.2.1 视觉 + 惯性导航 SLAM 373

14.2.2 语义 SLAM. 374

14.2.3 SLAM 的未来 . 375

附录 A 高斯分布的性质. 377

附录 B 矩阵求导 380

附录 C ROS 入门.382

参考文献 385