●目录 讲 预备知识 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 部分 数学基础 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 实际的坐标变换例子 ............................................................................... 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 0 讲 后端 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 1 讲 回环检测 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 2 讲 建图 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 3 讲 实践：设计 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 4 讲 SLAM：现在与未来 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363 14.1 当前的开源方案................................................................................................ 3 14.1.1 MonoSLAM ............................................................................................ 3 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