高翔，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