还 督 己 天进步一 吗？

还 天 读的习惯吗？

还 为找不 己喜欢的书 恼吗？

愿意与我成为书友吗？

国 当下 行书

各图书 量 行 书

量工具书

我 受益终生的书

……

量电 版、纸质版书 及音频课程

还有贴心的“ 习 ”服务哦！
微 ：shuyou099
 内容简介
本书 介 了视觉SLAM（同时 位与 图构建）所需的 本知
识与核心 ，既包 理论 础， 三 空 的刚 运动、非
性优化，又包 计 机视觉的 现， 视图几 、 环检
等。此 ，我 还 了 量的 代码 读 习研究，从 更
这些 。
本书可以 为 SLAM感兴趣的研究人员的 材料，也可以
为SLAM相关的高校本科生 研究生课程 材 用。
 未 许可，不得以 方式 制 袭本书之 分 。
版权所有， 权必究。
图书在版编目（CIP）数据
视觉SLAM十 讲：从理论 /高 等著.—北京：电 工业出
版社，2017.3
ISBN 978-7-121-31104-8
I. ① 视 … 　 II. ① 高 … 　 III. ① 人 工 智 -视觉 -研究　
IV.①TP18
中国版本图书馆CIP 核字（2017）第053910号
： 柳
责 ：白　
印　　刷：北京 蜂印刷厂
装　　订：北京 蜂印刷厂
出版发行：电 工业出版社
北京市 区万 173 　　　 ：100036
开　　本：720×1000　1/16　　印张：25　　字 ：560千字
版　　次：2017年3月第1版
印　　次：2017年3月第1次印刷
　　价：75.00
凡所购买电 工业出版社图书有 问题，请向购买书店调 。
书店售 ，请与本社发行 ， 及 购 电 话 ： （ 010 ）
88254888，88258888。
质量 诉请发 件至zlts@phei.com.cn，盗版 权举报请发 件
至dbqq@phei.com.cn。
本书咨询 方式：（010）51260888-819faq@phei.com.cn。
 阅读须知
本书图片
为 方 ，书中介 图片相关 时直 用了 应的 彩名
称， 应 纸面上即 。此 ， 分图片可 需要 观 ，纸面
上无 呈现应有 果。为此，书中图片 博 视 方 站 下
。
读者服务
松 册成为博 视 社区用 （www.broadview.com.cn），您
即可享受以下服务。
•下载资源： 本书所 的示 代码及资 件 可 下 资
下 。
•提交勘误： 您 书中 的修 意见可 交勘误 交，
被 ， 获赠博 视 社区积分（ 您购买电 书时，积分可用
来 相应 额）。
•与作者交流 ： 页面下方读 评论 留下您的疑问 观 ，
与 和其 读 一同 习交 。
页面 口：http://www.broadview.com.cn/31104
 目　录
介

读须知

第1讲　预 知识
1.1　本书讲什么

1.2　 用本书

1.2.1　组织方式

1.2.2　代码

1.2.3　面向的读

1.3　风格约

1.4　致谢和 明

第2讲　 识SLAM
2.1　引 ： 萝卜的

2.2　 典视觉SLAM框架

2.2.1　视觉里程计

2.2.2　后端优化

2.2.3　 环检
 2.2.4　建图

2.3　SLAM问题的 表

2.4　 ： 程 础

2.4.1　安装Linux操

2.4.2　Hello SLAM

2.4.3　 用cmake

2.4.4　 用库

2.4.5　 用IDE

第3讲　三 空 刚 运动
3.1　旋转矩阵

3.1.1　 和向量，坐标

3.1.2　坐标 的 变

3.1.3　变 矩阵与齐次坐标

3.2　 ：Eigen

3.3　旋转向量和 角

3.3.1　旋转向量

3.3.2　 角

3.4　

3.4.1　 的 义

3.4.2　 的运
 3.4.3　用 表示旋转

3.4.4　 旋转矩阵的转

3.5　*相 、 、 影变

3.6　 ：Eigen几

3.7　可视化 示

第4讲　李 与李代
4.1　李 与李代 础

4.1.1　

4.1.2　李代 的引出

4.1.3　李代 的 义

4.1.4　李代 so(3)

4.1.5　李代 se(3)

4.2　 与 映

4.2.1　SO(3)上的 映

4.2.2　SE(3)上的 映

4.3　李代 导与 动

4.3.1　BCH公式与近 形式

4.3.2　SO(3)李代 上的 导

4.3.3　李代 导

4.3.4　 动 （左乘）
 4.3.5　SE(3)上的李代 导

4.4　 ：Sophus

4.5　*相 变 与李代

4.6　

第5讲　相机与图
5.1　相机

5.1.1　 相机

5.1.2　畸变

5.1.3　双目相机

5.1.4　RGB-D相机

5.2　图

5.3　 ：图 的存取与访问

5.3.1　安装OpenCV

5.3.2　操 OpenCV图

5.4　 ： 云

第6讲　非 性优化
6.1　 态估计问题

6.1.1　最 后验与最

6.1.2　最 二乘的引出
 6.2　非 性最 二乘

6.2.1　一阶和二阶梯度

6.2.2　高斯 顿

6.2.3　列 伯格—马夸 方

6.2.4　

6.3　 ：Ceres

6.3.1　Ceres 介

6.3.2　安装Ceres

6.3.3　 用Ceres 合曲

6.4　 ：g2o

6.4.1　图优化理论 介

6.4.2　g2o的 译与安装

6.4.3　 用g2o 合曲

6.5　

第7讲　视觉里程计1
7.1　 征

7.1.1　 征

7.1.2　ORB 征

7.1.3　 征匹

7.2　 ： 征 取和匹
7.3　2D-2D： 极几

7.3.1　 极约束

7.3.2　本质矩阵

7.3.3　单应矩阵

7.4　 ： 极约束 解相机运动

7.5　三角 量

7.6　 ：三角 量

7.6.1　三角 量代码

7.6.2　讨论

7.7　3D-2D：PnP

7.7.1　直 性变

7.7.2　P3P

7.7.3　Bundle Adjustment

7.8　 ： 解PnP

7.8.1　 用EPnP 解位

7.8.2　 用BA优化

7.9　3D-3D：ICP

7.9.1　SVD方

7.9.2　非 性优化方

7.10　 ： 解ICP
 7.10.1　SVD方

7.10.2　非 性优化方

7.11　

第8讲　视觉里程计2
8.1　直 的引出

8.2　光 （OpticalFlow）
8.3　 ：LK光

8.3.1　 用TUM公开 集

8.3.2　 用LK光

8.4　直 （DirectMethod）

8.4.1　直 的推导

8.4.2　直 的讨论

8.5　 ：RGB-D的直

8.5.1　稀疏直

8.5.2　 义直 的

8.5.3　 用直 估计相机运动

8.5.4　半稠 直

8.5.5　直 的讨论

8.5.6　直 优 总
第9讲　 ：设计前端
9.1　 建VO框架

9.1.1　确 程序框架

9.1.2　确 本 构

9.1.3　Camera

9.1.4　Frame

9.1.5　MapPoint

9.1.6　Map

9.1.7　Con fi g

9.2　 本的VO： 征 取和匹

9.2.1　两两帧的视觉里程计

9.2.2　讨论

9.3　 进：优化PnP的 果

9.4　 进： 图

9.5　

第10讲　后端1
10.1　

10.1.1　 态估计的 率解

10.1.2　 性 和KF
 10.1.3　非 性 和EKF

10.1.4　EKF的讨论

10.2　BA与图优化

10.2.1　 影 和BA代价

10.2.2　BA的 解

10.2.3　稀疏性和 化

10.2.4　鲁棒核

10.2.5　

10.3　 ：g2o

10.3.1　BA 集

10.3.2　g2o 解BA

10.3.3　 解

10.4　 ：Ceres

10.4.1　Ceres 解BA

10.4.2　 解

10.5　

第11讲　后端2
11.1　位 图（PoseGraph）

11.1.1　Pose Graph的意义

11.1.2　Pose Graph的优化
 11.2　 ：位 图优化

11.2.1　g2o原生位 图

11.2.2　李代 上的位 图优化

11.2.3　

11.3　* 图优化 步

11.3.1　贝叶斯

11.3.2　 图

11.3.3　 量 性

11.4　* ：gtsam

11.4.1　安装gtsam 4.0

11.4.2　位 图优化

第12讲　 环检
12.1　 环检

12.1.1　 环检 的意义

12.1.2　方

12.1.3　 确率和召 率

12.2　词袋

12.3　字典

12.3.1　字典的 构

12.3.2　 ：创建字典
 12.4　相 度计

12.4.1　理论 分

12.4.2　 ：相 度的计

12.5　 验分析与评

12.5.1　 加字典规

12.5.2　相 性评分的 理

12.5.3　关 帧的 理

12.5.4　检 之后的验证

12.5.5　与机器 习的关

第13讲　建图
13.1　

13.2　单目稠 重建

13.2.1　立 视觉

13.2.2　极 与 匹

13.2.3　高斯分布的 度 器

13.3　 ：单目稠 重建

13.4　 验分析与讨论

13.4.1　 梯度的问题

13.4.2　 度

13.4.3　图 的变
 13.4.4　并行化： 率的问题

13.4.5　其 的 进

13.5　RGB-D稠 建图

13.5.1　 ： 云 图

13.5.2　八叉树 图

13.5.3　 ：八叉树 图

13.6　*TSDF 图和Fusion 列

13.7　

第14讲　SLAM：现 与未来
14.1　当前的开 方案

14.1.1　MonoSLAM

14.1.2　PTAM

14.1.3　ORB-SLAM

14.1.4　LSD-SLAM

14.1.5　SVO

14.1.6　RTAB-MAP

14.1.7　其

14.2　未来的SLAM话题

14.2.1　视觉+惯性导 SLAM

14.2.2　语义SLAM
 14.2.3　SLAM的未来

附录A　高斯分布的

附录B　ROS

参 献
 第1讲　预备知识
1.1　本书讲什么
这是一本介 视觉SLAM的书，也很可 是第一本以视觉SLAM为主题
的中 书。
么，SLAM是什么？
SLAM 是 Simultaneous Localization and Mapping 的 ，中 译
同时定位与地图构建
“ ”[1] 。 是 传感器 的主 ，没
有环境先验信息 的情 下，于 运动过程中 环境建立 的 ，同时估计
运动
己的 [2]
。 果这里的 感器主要为相机， 称为“视觉
SLAM”。
本书的主题 是视觉SLAM。这里我 刻意 许 个 义 一句话
中，是希望读 有一个 明确的 念。首先，SLAM的目的是解 “
位”与“ 图构建”这两个问题。也 是说，一 要估计 感器 的
位 ，一 要建立周 环 的 。 么怎么解 呢？这需要用 感
器的 息。 感器 以一 形式观 的世界，不过不同 感器观
的方式是不同的。 之所以要 一本书的 去讨论这个问题，是 为
很难—— 是我 希望 、 实时地 没有先验知识
的情 下进行
SLAM。当用相机 为 感器时，我 要 的 是根 一张张连 运动的
图 （ 形成了一 视频），从中推断相机的运动，以及周 环 的
情 。
这 乎是个很直观的问题。我 己走进陌生的环 时不 是这么
的吗？
计 机视觉（ComputerVision）创立之 ，人 想象着有朝一
日计 机 和人一样， 过眼睛去观 世界，理解周 的 ， 未
知的领 ——这是一个 又 的梦想，吸引了无 的科研人员日
[3]
为之 。我 曾 以为这件事情并不 难， 进展却远不 预
想的 么顺 。我 眼中的 草树木、虫鱼鸟兽， 计 机中却是 样
的不同： 只是一个个由 字 列 成的矩阵（Matrix）。让计 机
理解图 的 ， 让我 己理解这些 字一样 难。我 既不了
解 己 理解图 ，也不知 计 机该 理解、 这个世界。于
是我 惑了很久，直 几十年后的今天，才发现了一 成功的
象 ： 过 人 工 智 （ Arti fi cial 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等库[1] ， Linux操 中的 用
方 。
 从 风格上，我 不想 本书 成枯 的理论书 。技术 图书
应该是严谨可靠的， 严谨不意味着刻板。一本优秀的技术书应该是生
动有趣 易于理解的。 果 觉得“这个 怎么这么不正 ”， 请
原谅， 为我并不是一个非常严 的人[2] 。无论 ，有一件事是可以
的：只要 这 新技术感兴趣， 习本书的过程中 会有所
获！您会 与SLAM相关的理论知识， 的 程 力也 有明显的进
步。 很 时 ，您会有一种“我 陪 一起 科研”的感觉，这正是
我所希望的。 愿您 此过程中发现研究的乐趣，喜欢这种“ 过一
番努力，看 事情顺 运行”的成 感。
了，话不 说，祝 旅行愉快！

1.2　如何使用本书
1.2.1　组织方式
本书名为“视觉SLAM十 讲”。顾名思义，我 会 校里讲课
样，以“讲” 为本书的 本单 。 一讲 应一个 的主题，
其中会穿 “理论 分”和“ 分”两种 。 常是理论 分
前， 分 后。 理论 分中，我 介 理解算法所必需的
知识，并且 时 以叙 的方式， 不是 科书 样用“
义— 理—推论”的方式， 为我 觉得这样的方式 读起来更 易一
些，尽 有时 显得不 么严谨。 分主要是 程 现，讨论程序
里各 分的含义及 验 果。看 标题中带有“ ”两个字的章 ，
应该（兴致勃勃 ） 开电 ，和我 一起愉快 代码了。
得一 的是，我 只会 与解 问题相关的 知识 书里，
并尽量 显。虽 我 是工科生， 也要承认，某些 只要 验
上 用， 必要非得 上 。只要我 知 这些 工
，并且 告诉了我 什么情 下可 不工 ， 么我 表
示 意， 不 究 些看 际 杂 的证明（当 有
己的价 ）。由于SLAM 涉 了太 景，为了 止 本书变成
科书，我 一些细 上的推导和证明留 习题和补 读材
料，方 感兴趣的读 进一步 读参 献，更 相关细 。
 一讲正 之后，我 设计了一些习题。其中，带*号的习题是具有
一 难度的。我 强 建议读 习题 练习一 ，这 这些知
识很有帮助[3] 。
书 主要分为两个 分。
1.第一 分为 数学基础 ，我 会以 显易懂的方式， 与视觉
SLAM相关的
知识，包 ：
•第1讲是前言，介 这本书的 本 息，习题 分主要包 一些
题。
•第2讲为SLAM ，介 一个SLAM 由哪些 组成，各
的具 工 是什么。 分介 程环 的 建过程以及IDE的
用。
•第3讲介 三 空 运动， 触 旋转矩阵、 、 角
的相关知识，并且 Eigen当中 用 。
•第4讲为李 和李代 。即 现 不懂李代 为 ，也 有关
。 李代 的 义和 用方式， 后 过Sophus操 。
•第5讲介 相机 以及图 计 机中的表 。 用
OpenCV来调取相机的 参 。
•第6讲介 非 性优化，包 态估计理论 础、最 二乘问题、
梯度下降方 。 会 成一个 用Ceres和g2o进行曲 合的 验。
这些 是我 要用 的所有 知识了，当 ，其中还隐含了 以
前 过的高等 和 性代 。我 证 看起来 不会很难。当
， 想进一步 ，我 会 一些参 资料 读， 些
材料可 会比正 里讲的知识难一些。
2.第二 分为SLAM 技术 。我 会 用第一 分所介 的理论，讲
视觉SLAM中各个 的工 原理。
•第7讲为 征 的视觉里程计。该讲 比 ，包 征 的
取与匹 、 极几 约束的计 、PnP和ICP等。 中， 用这
些方 去估计两个图 之 的运动。
 •第8讲为直 的视觉里程计。 习光 和直 的原理，
后 用g2o 现一个 单的RGB-D直 。
•第9讲为视觉里程计的 章， 建一个视觉里程计框架，
合运用先前 过的知识， 现 的 本功 。这个过程中， 会碰 一
些问题， 优化的必要性、关 帧的 等。
•第10讲为后端优化，主要为 Bundle Adjustment的 讨论，包
本的BA，以及 用稀疏性加速 解过程。 用Ceres和g2o分
书 一个BA程序。
•第11讲主要讲后端优化中的位 图。位 图是表 关 帧之 约束
的一种更 的形式。 用g2o和gtsam 一个位 球进行优化。
•第12讲为 环检 ，主要介 以词袋方 为主的 环检 。
用dbow3书 字典训练程序和 环检 程序。
•第13讲为 图构建。我 会讨论 用单目进行稠 度图的估
计（以及这是 么不可靠）， 后讨论RGB-D的稠 图构建过程。 会
书 极 与 匹 的程序， 后 RGB-D中 云 图和八叉树
图的构建问题。
•第14讲主要介 当前的开 SLAM项目以及未来的发展方向。相
读了前面的知识之后， 会更 易理解 的原理， 现 己的新想
。
最后， 果 看不懂上面 说什么， 么恭喜 ！这本书很
合 ！加 ！

1.2.2　代码
本书所有 代码 github上：
https://github.com/gaoxiang12/slambook
强 建议读 下 以 随时查看。代码是 章 分的，比
，第7讲的 会 ch7 件夹中。此 ， 于书中用 的一些
库，会以压 包的形式 3rdparty 件夹下。 于 OpenCV 种
中 库，我 会 第一次出现时介 其安装方 。 果 代码有
疑问，请单击GitHub上的Issues ， 交问题。 果确 是代码
出现问题，我 会及时进行修 ；即 是 的理解有 差，我 也会尽
可 。 果 不习惯 用Git， 么单击右 包含download字样的
代码下 至本 即可。

1.2.3　面向的读者
本书面向 SLAM感兴趣的 生和研究人员。 读本书需要一 的
础，我 设 具 以下知识：
• 高等数学、线性代数、概率论 。这些是 分读 应该 本
科阶 触过的 本 知识。 应当明白矩阵和向量是什么，
微分和积分是什么意思。 于SLAM中用 的专业知识，我 会额 加以
介 。
•C++语言基础。由于我 用C++ 为 码语言，所以建议读 至
悉这 语言的语 。比 ， 应该知 是什么， 用C++标
库， 板 用，等等。我 会 过 用技巧， 有些 方
确 无 。此 ，我 还 用了一些C++11标 的 ，不过，我
会 用 的 方加以解 。
基础
•Linux 。我 的开发环 是Linux 非Windows，并且只
不会
Linux下的 程序， 再 我们认为，
Windows下的开发方 介 。
掌握 是一个 研究人员所必需的，请初学者暂时不要问为什
Linux SLAM
么，把本书的知识学好之后相信你会和我们有同样的想法。 各种程序
库 Linux下的 非常 ， 也会 此过程中 会 Linux的
。 果读 此前从未 用过Linux， 么最 找一本Linux的 材稍加
习（ 本知识即可，一 是相关图书的前面几章 ）。我
不要 读 具 高超的Linux操 技 ， 希望读 至 知 “ 开终
端，进 代码目录”是 操 的。本讲的习题里有一些Linux知识
题， 果 楚 题的 案， 么 读本书代码不会有 问题。
SLAM感兴趣 不具 上 知识的读 ，可 读本书时会感
难。 果 不了解C++的 本知识，可以读一 C++Primer Plus
之
的图书 ； 果 相关的 知识，也可以先 读一些相关
材补 知识，不过我 认为， 本科 平的朋友，读懂本
书所需的 景 是具 了。代码方面， 最 时 亲
一 ，再调 里面的参 ，看看 果会发生怎样的 变。这会 习很
有帮助。
 本书可 为SLAM相关课程的 材，亦可 为课 材料 用。

1.3　风格约定
本书既有 理论介 ，也有 程 现， 此，为方 读， 不
同 用了不同 版方式加以区分。
1. 公式单 列出，重要的公式还 右 标了序号， ：

标量 用斜 字（ a ），向量和矩阵 用 斜 （ a,A ）。空

心 代表 集合， 集R、 集Z。李代 分 用哥 ，
se(3)。
2.程序代码以方框框出， 用 一些的字号，左 带有行号。 果
程序 ，方框会延 下一页：

3.当代码 量 有的 分与之前列出的重 ，不 合 列
书中时，我 会 仅给出重要片段 ，并以“片 ”二字 明。 此，我
强 建议读 GitHub上下 所有 代码， 成练习，以更 本
书知识。
4.由于 版原 ，书中展示的代码可 与GitHub中的代码有稍许不
同，请以GitHub上的代码为 。
5.我 用 的 个库， 第一次出现的时 会有比 详细的说明，
后 的 用中则不再赘 。所以，建议读 章 顺序 读本书
。
 6. 一讲的开 会列出本讲的 要， 末尾会有 和练习
题。引用的参 献 书末尾列出。
7.以星号开 的章 是 读 分，读 可以根 兴趣 读。 过
不会 理解后 章 产生影响。
8. 中重要的 以 黑体 标出，相 已 习惯了。
9.我 设计的 验 是 示性质的。看懂了 不代表 已
悉 个库的 用。所以我 建议 课 一 时 ， 本书 常用
的几个库进行 习。
10.本书的习题和 读 可 需要 己 额 材料，所以 需
要 会 用 引擎。

1.4　致谢和声明
本书 的 过程中，我 得 了许 人的帮助，包 不
于：
•中科院的贺一 博 为第5讲的相机 分 了材料。
•颜 睿 了第7讲的公式推导材料。
•华中科 的刘 博 为本书第6讲和第10讲 了材料。
•众 的 师、同 为本书 了修 意见： 、谢晓 、
欣、李帅杰、刘 强、袁梦、孙志明、 昊升、王京、朱晏 、丁
东、 帝楷、衡昱帆、高扬、李 朋、吴博、 雪 、张 、 帆、卢
、杨楠，等等。 此向 表示感谢。
此 ，感谢我的导师张 一直以来 我的 和帮助。感谢电
工业出版社 柳 的 。 有 的帮助，本书不可 以现
的面貌来 读 面前。本书的成书与出版是所有人 同努力的 晶，尽
我 列 列表中， 是 的出版离不开 的工 。
本书 过程中参 了 量 献和论 。其中 分 理论知识
是前人研究的成果，并非我的原创。一 分 验设计亦来 各开 代
码的 示程序，不过 分是我 己 的。此 ，也有一些图片
公开发表的期 会议论 ， 中 已 明。未 说明的图 ， 为原
创， 来 ，恕不一一列举。 有问题，请与我 ，我 会
第一时 加以修正。
本书涉及知识 众 ， 所难 。 有疑问，欢 过电
件与我 。
我的 是：gaoxiang12@mails.tsinghua.edu.cn。
感谢我的 人刘丽莲 期的理解和 。这本书是献给 的。
习题（基本知识自测题）
1.有 性方程 Ax =b ， 已知 A,b ，需要 解 x ，该 解？这
A 和b 有哪些要 ？ 示：从A 的 度和秩角度来分析。
2.高斯分布是什么？ 的一 形式是什么样 ？ 的高 形式是什
么样 ？
3. 知 C++中的 类 吗？ 知 STL吗？ 用过 吗？
4. 以前怎样书 C++程序？（ 可以说只 Visual C++6.0下
过C++工程，只要 有 C++和C语言的 验 行。）
5. 知 C++11标 吗？其中哪些新 性 听说过 用过？有 有其
的标 ？
6. 知 Linux吗？ 有 有至 用过一种（不 安卓），比
Ubuntu？
7.Linux的目录 构是什么样的？ 知 哪些 本命令，比 ls,cat
等？
8. Ubuntu中安装软件（不 开软件中心的情 下）？这些软
件被安装 什么 方？ 果只知 的软件名称（比 想要装一个名
称中含有eigen的库），应该 安装 ？
9.* 一个 时 习一下Vim， 为 迟早会用 。 可以 终端中
vimtutor 读一 所有 。我 不需要 非常 练 操 ，只
要 习本书的过程中 用 代码即可。 不要在它的插件上浪
费时间，不要想着把 Vim IDE 用成 ，我们只用它做文本编辑的工作。
[1] 果 有听说过 ， 么应该感 兴 ，这说明 会从本书中 获很 知
识。
[2] 会 常 中发现一些神 的东西。

[3] 也可 成为今后相关行业的面试题， 许还 帮 找工 时留个 印象。

 第2讲　初识SLAM
主要目标
1.理解一个视觉SLAM框架由哪几个 组成，各 的 务是什
么。
2. 建 程环 ，为开发和 验 。
3.理解 Linux下 译并运行一个程序， 果程序出了问题，又
该 进行调试。
4. cmake的 本 用方 。
本讲 介 一个视觉SLAM 的 构， 为后 的 。
分介 环 建、程序 本 知 识 ， 最 后 成 一 个 “Hello
SLAM”程序。
2.1　引子：小萝卜的例子
设我 组装了一台叫 “ 萝卜”的机器人， 的样 图2-1
所示。

图2-1　 萝卜设计图。左 ：正视图；右 ： 视图。设 有相机、轮 、笔记

本， 是装饰品。

虽 有 “安卓”， 并不是靠安卓 来计 的。我 一

台笔记本 进了 的后 （方 我 随时 出来调试程序）。
什么呢？
我 希望 萝卜具有 自主运动能力
。虽 世界上也有 桌面
件一样的机器人， 和人说话 音乐，不过一台平板电 可
以 这些事情。 为机器人，我 希望 萝卜 房 里 由移
动。不 我 哪里 呼一 ， 会 走过来。
要移动首先得有轮 和电机，所以我 萝卜的下方安装了轮
（足式机器人步态很 杂，我 暂时不 虑）。有了轮 ，机器人
行动了， 不加控制的话， 萝卜不知 行动的目标， 只
乱走，更 的情 下会 上 造成 。为了 这种情 的发
生，我 的 袋上安装了一个相机。安装相机的主要动机，是 虑
这样一个机器人 和人类非常相似
——从画面上一眼 看出。有眼
睛、 和 的人 ， 意环 里 松 行走、 ，我
（天真 ）觉得机器人也 成这件事。为了 萝卜 一
个房 ， 至 需要知 两件事：
1.我 什么 方？—— 位。
2.周 环 是什么样？——建图。
“ 位”和“建图”，可以看成感知的“ 之分”。 为一
个“ 兼修”的 萝卜，一方面要明白 的 状态
（即位 ），另一
方面也要了解 的 环境
（即 图）。当 ，解 这两个问题的方 非
常 。比方说，我 可以 房 板上 设导引 ， 上贴识 二
码， 桌 上 无 电 位设 。 果 ，还可以 萝卜
袋上安装 位设 （ 机 一样）。有了这些东西之后， 位问
题是否已 解 了呢？我 不 这些 感器（见图2-2）分为两 。
一 感器是 携带于机器人本体上
的， 机器人的轮式 码器、
相机、 光 感器，等等。另一 是 安装于环境中
的， 前面讲的导
、二 码标志，等等。安装于环 中的 感设 ， 常 直 量
机器人的位 息， 单有 解 位问题。 ，由于 必须
环 中设 ， 一 程度上 制了机器人的 用 。比方说，有些
方 有GPS 号，有些 方无 设导 ，这时该怎么 位呢？

图2-2　一些 感器。（a） 用二 码进行 位的 强现 软件；（b）GPS 位

装 ；（c） 设导 的 ；（d） 光雷 ；（e）IMU单 ；（f）双目相机。
 我 看 ，这 约束感器 了 环 。只有 这些约束 足时，
于 的 位方案才 工 。反之，当约束无 足时，我
进行 位了。所以说，虽 这 感器 单可靠， 无 一个
普 的、 用的解 方案。相 ， 些 带于机器人本 上的 感
器，比 光 感器、相机、轮式 码器、惯性 量单 （Inertial
Measurement Unit，IMU）等， 的 常 是一些 的 理量
不是直 的位 。 ，轮式 码器会 轮 转动的角度，IMU
量运动的角速度和加速度，相机和 光 感器则读取 环 的某种观
。我 只 过一些 的 ，从这些 推 己的位 。
虽 这听上去是一种 术， 更明显的 是， 有 环 出
要 ，从 得这种 位方案可 用于未知环 。
顾前面讨论过的SLAM 义，我 SLAM中非常强调未知环 。
理论上，我 制 萝卜的 用环 [1] ，这意味着我 设
GPS这些 感器 顺 工 。 此， 用 带式的 感器来 成
SLAM是我 重 关心的问题。 ，当谈论视觉SLAM时，我 主要是
相机
用 解 位和建图问题。
视觉SLAM是本书的主题，所以我 其关心 萝卜的眼睛 些
什么事。SLAM中 用的相机与我 平时见 的单反 并不是同一个
东西。 往往更加 单，不 带昂贵的 ， 是以一 速率 周
的环 ，形成一个连 的视频 。普 的 以 秒 30张图片的
速度 集图 ，高速相机则更快一些。 工 方式的不同，相机可以
分为单目相机（Monocular）、双目相机（Stereo）和 度相机（RGB-
D）三 ， 图2-3所示。直观看来，单目相机只有一个 ，双目
有两个， RGB-D原理 杂，除了 集 彩 图片之 ，还 读出
个 与相机之 的 离。 度相机 常 带 个 ，工 原理
和普 相机不尽相同， 第5讲会详细介 其工 原理，此 读 只需有
一个直观 念即可。此 ，SLAM中还有 景相机[7] 、Event相机[8] 等
新兴的种 。虽 看 SLAM中的应用，不过 目前为
止还 有成为主 。从样 上看， 萝卜 用的 乎是双目相机[2] 。
 图2-3　形形 的相机：单目、双目和 度相机。

我 来分 看一看各种相机用来 SLAM时有什么 。
单目相机
只 用一个 进 行 SLAM 的 称 为 单 目 SLAM （ Monocular
SLAM）。这种 感器 构 单，成本 低，所以单目SLAM非常受
研究 关 。 见过单目相机的 ： 片。是的， 为一张
片， 有什么 呢？
片本质上是 时的 景（Scene） 相机的成 平面上留下的一
个投影 它以二维的形式反映了三维的世界。
。 显 ，这个过程丢 了
景的一个 度，也 是所谓的 度（ 离）。 单目相机中，我
无 过单张图片来计 景中 与我 之 的 距离
（远近）。之后
我 会看 ，这个 离 是SLAM中非常关 的 息。由于我 人 见过
量的图 ，形成了一种天生的直觉， 分 景 有一个直观的 距
离感（空间感） ， 可以帮助我 断图 中 的远近关 。比
说，我 认出图 中的 ，并且知 其 致的 ；比 ，近
的 会 住远 的 ， 太 、月亮等天 一 很远的 方；
再 ， 受光 后会留下影 ，等等。这些 息 可以帮助我 断
的远近， 也存 一些情 会 这种 离感 ，这时我 无
断 的远近及其真 了。图2-4所示 是这样一个 。 这张
图 中，我 无 仅 过 来 断后面 些 人是真 的人，还是
。除非我 转 视角，观 景的三 构。 言之， 单张图
里， 无 确 一个 的真 。 可 是一个很 很远的
 ，也可 是一个很近 很 的 。由于近 远 的原 ， 可
图 中变成同样 的样 。

图2-4　单目视觉中的 ：不知 度时， 上的人是真人还是 ？

由于单目相机 的图 只是三 空 的二 影，所以， 果真

想恢 三 构，必须 变相机的视角。 单目SLAM中也是同样的原
理。我 必须移动相机，才 估计 的 运动
（Motion），同时估计 景
中 的远近和 ，不 称之为 结构
（Structure）。 么，怎么估
计这些运动和 构呢？从生 验中我 知 ， 果相机往右移动，
么图 里的东西 会往左 移动——这 给我 推 运动带来了 息。
另一方面，我 还知 ： 近处的物体移动快，远处的物体则运动缓慢。
于是，当相机移动时，这些 图 上的运动 形成了 视差。 过视
差，我 量 断哪些 离得远，哪些 离得近。
，即 我 知 了 远近， 仍 只是一个相 的 。比
我 看电影时，虽 知 电影 景中哪些 比另一些 ，
无 确 电影里 些 的“真 度”： 些 是真 的高
厦，还是 桌上的 ？ 厦的是真 怪兽，还是穿着 服
装的 员？直观 说， 果 相机的运动和 景 同时 两 ，单
目相机所看 的 是一样的。同样 ， 这个 乘以 意 ，我
看 一样的景象。这说明，单目SLAM估计的 和 图 与真 的
和 图相差一个 ，也 是所谓的 尺度
（Scale）[3] 。由于单目
SLAM无 仅 图 确 这个真 度，所以又称为 尺度不确定性
。
 平移之后才 计 度，以及无 确 真 度，这两件事情给单
目SLAM的应用造成了很 的麻 。其根本原 是 过单张图 无 确
度。所以，为了得 这个 度，人 开 用双目和 度相机。
双目相机和深度相机
用双目相机和 度相机的目的， 于 过某种 量 与我
之 的 离， 服单目相机无 知 离的 。一旦知 了 离，
景的三 构 可以 过单个图 恢 出来，也 消除了 度不确
性。尽 是为了 量 离， 双目相机与 度相机 量 度的原理是
不一样的。双目相机由两个单目相机组成， 这两个相机之 的 离
〔称为基线 （Baseline）〕是已知的。我 过这个 来估计 个
的空 位 ——这和人眼非常相 。我 人 可以 过左右眼图 的
差异 断 的远近， 计 机上也是同样的 理（见图2-5）。 果
双目相机进行 展，也可以 建 目相机，不过本质上并 有什么不
同。

图2-5　双目相机的 ：左眼图 ，右眼图 。 过左右眼的差异， 断

景中 与相机之 的 离。

计 机上的双目相机需要 量的计 才 （不太可靠 ）估计 一

个 的 度，相比于人 真是非常笨 。双目相机 量 的 度
与 相关。 离越 ， 量 的 越远，所以无人 上
的双目 常会是个很 的 伙。双目相机的 离估计是比 左右眼的
图 获得的，并不 赖其 感设 ，所以 既可以应用 ，亦可
应用于 。双目 目相机的 是 与标 为 杂，其 度
量程和 度受双目的 与分 率所 ， 且视差的计 非常消 计
资 ，需要 用GPU和FPGA设 加速后，才 时 出 张图 的 离
息。 此 现有的条件下，计 量是双目的主要问题之一。
度相机（又称RGB-D相机， 本书中主要 用RGB-D这个名称）是
2010年左右开 兴起的一种相机， 最 的 是可以 过 构光
 Time-of-Flight（ToF）原理， 光 感器 样， 过主动向 发
光并 的光， 出 与相机之 的 离。这 分并不 双目
相机 样 过软件计 来解 ， 是 过 理的 量 ，所以相比于
双目相机可 省 量的计 （见图2-6）。目前常用的RGB-D相机包
Kinect/Kinect V2、Xtion Pro Live、RealSense等。不过，现
RGB-D相机还存 量 窄、 、视野 、易受日光干 、无
量透 材质等诸 问题， SLAM方面，主要用于 ， 则 难应
用。

图2-6　RGB-D ： 度相机可以直 量 的图 和 离，从 恢 三

构。

我 讨论了几种常见的相机，相 过以上的说明， 已
有了直观的了解。现 ，想象相机 景中运动的过程，我 得 一
列连 变化的图 [4] 。视觉SLAM的目标，是 过这样的一些图 ，进
行 位和 图构建。这件事情并 有我 想象的 么 单。 不是某种
，只要我 ， 可以往 不断 出 位和 图 息了。
SLAM需要一个 善的 框架， 过研究 期的努力工 ，现有
这个框架已 了。

2.2　经典视觉SLAM框架
 下面来看 典的视觉SLAM框架， 图2-7所示，了解一下视觉SLAM究
竟由哪几个 组成。

图2-7　 视觉SLAM 程图。

个视觉SLAM 程包 以下步骤。
1. 感器 息读取。 视觉SLAM中主要为相机图 息的读取和预
理。 果是 机器人中，还可 有码盘、惯性 感器等 息的读取和
同步。
2. 视觉里程计 （Visual Odometry，VO）。视觉里程计的 务是估
相 图 相机的运动，以及 图的样 。VO又称为前端（Front
End）。
3. 后端优化
（Optimization）。后端 受不同时刻视觉里程计 量
的相机位 ，以及 环检 的 息， 进行优化，得 一致的
和 图。由于 VO之后，又称为后端（Back End）。
4.回环检测 （Loop Closing）。 环检 断机器人是否 过先
前的位 。 果检 环， 会 息 给后端进行 理。
5.建图 （Mapping）。 根 估计的 ，建立与 务要 应的
图。
典的视觉SLAM框架是过去十几年的研究成果。这个框架本 及其
所包含的 已 本 ，并且已 许 视觉程序库和机器人程序
库中 。 靠这些 ，我 构建一个视觉SLAM ， 之 正
如果把工作环境限定
常的工 环 里 时 位与建图。 此，我 说，
在静态、刚体，光照变化不明显、没有人为干扰的场景 ， 么，这个
SLAM 是相当成 的了[9] 。
 读 可 还 有理解上面几个 的 念，下面 来详细介 各个
具 的 务。 是， 确理解其工 原理需要一些 知识，我
本书的第二 分进行。这里读 只需 各 有一个直观的、
性的理解即可。

2.2.1　视觉里程计
视觉里程计关心的是 相邻图像
之 的相机运动，最 单的情 当
是两张图 之 的运动关 。 ，当看 图2-8时，我 会 反应
出右图应该是左图向左旋转一 角度的 果（ 视频情 下感觉会更加
）。我 不 思 一下： 己是怎么知 “向左旋转”这件事情的
呢？人 早已习惯于用眼睛 世界，估计 己的位 ， 又往往难以
用理性的语言 我 的直觉。看 图2-8时，我 会 认为，这个
景中离我 近的是吧台，远 是 和黑板。当相机向左转动时，吧
台离我 近的 分出现 视野中， 右 远 的柜 则移出了视野。
过这些 息，我 断相机应该是向左旋转了。

图2-8　相机 的图片与人眼反应的运动方向。

是， 果进一步问： 否确 旋转了 度，平移了 厘 ？

我 很难给出一个确切的 案了。 为我 的直觉 这些具 的 字
并不 感。 是， 计 机中，又必须 确 量这 运动 息。所以
我 要问： 计算机是如何通过图像确定相机的运动的呢？
前面也 过， 计 机视觉领 ，人 直觉上看来十分 的事
情， 计 机视觉中却非常 难。图 计 机里只是一个 矩阵。
这个矩阵里表 着什么东西，计 机 无 念（这也正是现 机器 习
要解 的问题）。 视觉SLAM中，我 只 看 一个个 ，知
是某些空 相机的成 平面上 影的 果。所以，为了 量 估
计相机运动，必须先 了解相机与空间点的几何关系 。
 要讲 这个几 关 以及VO的 现方 ，需要 一些 景知识。
这里我 先让读 VO有个直观的 念。现 只需知 ，VO 过
相 帧 的图 估计相机运动，并恢 景的空 构。称 为“里程
计”是 为 和 际的里程计一样，只计 相 时刻的运动， 和再往
前的过去的 息 有关 。 这一 上，VO 一种只有短时 记忆的
种。
现 ， 我 已有了一个视觉里程计，估计了两张图 的相机
运动。 么，只要 相 时刻的运动“串”起来， 构成了机器人的运
动 ，从 解 了 位问题。另一方面，我 根 个时刻的相机位
，计 出各 应的空 的位 ， 得 了 图。这么说来，有
了VO，是不是 解 了SLAM问题呢？
视觉里程计确 是SLAM的关 ，我 也会 量的 幅来介 。
，仅 过 视 觉里程 计来估计 ， 不可 出现 累积漂移
（Accumulating Drift）。这是由于视觉里程计（ 最 单的情 下）
只估计两个图 的运动造成的。我 知 ， 次估计 带有一 的误
差， 由于里程计的工 方式，先前时刻的误差 会 下一时刻，
导致 过一 时 之后，估计的 不再 确（见图2-9）。比方说，
机器人先向左转90 ° ，再向右转90 ° 。由于误差，我 第一个90 °
估计成了89 ° 。 我 会 发现，向右转之后机器人的估计位
并 有 原 。更 的是，即 之后的估计再 确，与真 相
-
比， 会带上这 1 ° 的误差。

图2-9　 积误差与 环检 的校正 果[10] 。

 这也 是所谓的 漂移（Drift）。 导致我 无 建立一致的
图。 会发现原本直的走廊变成了斜的， 原本90 ° 的直角变成了 的
——这 是一件很难令人忍受的事情！为了解 移问题，我 还需
要两种技术： 后端优化
[5]
和 回环检测 。 环检 负责 “机器人
原 位 ”的事情检 出来， 后端优化则根 该 息，校正 个
的形 。

2.2.2　后端优化
笼 说，后端优化主要 噪声
理SLAM过程中 的问题。虽 我
很希望所有的 是 确的， 现 中，再 确的 感器也带有一
的 。 的 感器 量误差 ，昂贵的可 会 一些，有的
感器还会受磁 、 度的影响。所以，除了解 “ 从图 估计出相
机运动”之 ，我 还要关心这个估计带有 的 ，这些 是
从上一时刻 下一时刻的， 我 又 当前的估计有 的
。后端优化要 虑的问题， 是 从这些带有 的 中估计
个 的 态，以及这个 态估计的不确 性有 ——这称为最 后
验 率估计（Maximum-a-Posteriori，MAP）。这里的 态既包 机器人
的 ，也包含 图。
相 ，视觉里程计 分有时被称为“前端”。 SLAM框架中，前
端给后端 待优化的 ，以及这些 的 。 后端负责
的优化过程， 往往面 的只有 ，不必关心这些 底来 什么
感器。在视觉 中，前端和计算机视觉研究领域更为相关，比如
SLAM
图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。
从历史意义上来说，现 我 称为后端优化的 分，很 一 时
直 被称为“SLAM研究”。早期的SLAM问题是一个 态估计问题——正
是后端优化要解 的东西。 最早 出SLAM的一 列论 中，当时的人
称 为“空 态不确 性的估计”（Spatial Uncertainty）[4,11] 。
虽 有一些晦 ， 也确 反映出了SLAM问题的本质： 对运动主体自身
和周围环境空间不确定性的估计 。为了解 SLAM问题，我 需要 态估
计理论， 位和建图的不确 性表 出来， 后 用 器 非 性
优化，估计 态的 和不确 性（方差）。 态估计与非 性优化的
具 第6讲、第10讲和第11讲介 。让我 暂时 过 的原理说
明， 往下介 。
2.2.3　回环检测
环检 ，又称闭环检 （Loop Closure Detection），主要解
位 估计 随时间漂移的问题。怎么解 呢？ 设 际情 下机器人 过
一 时 的运动后 了原 ， 是由于 移， 的位 估计 却 有
原 。怎么办呢？我 想， 果有某种 ，让机器人知 “
了原 ”这件事， “原 ”识 出来，我 再 位 估计
“ ”过去， 可以消除 移了。这 是所谓的 环检 。
环检 与“ 位”和“建图”二 有 切的关 。事 上，我
认为， 图存 的主要意义是让机器人知晓 己 过的 方。为了
现 环检 ，我 需要让机器人具有 识别到过的场景
的 力。 的 现
有很 。 前面说的 样，我 可以 机器人下方设 一个标
志 （ 一张二 码图片）。 只要看 了这个标志， 知 己
了原 。 是，该标志 质上是一种环 中的 感器， 应用环
了 制（万一不 贴二 码怎么办？）。我 更希望机器人 用 带
的 感器——也 是图 本 ，来 成这一 务。 ，可以 断 图像
间的相似性 来 成 环检 。这一 和人是相 的。当我 看 两张相
的图片时， 易 认 来 同一个 方。 果 环检 成功，可以
显著 积误差。所以，视觉 环检 质上是一种计 图
相 性的 。由于图 的 息非常丰 ， 得正确检 环的难度降
低了不 。
检 环之后，我 会 “A与B是同一个 ”这样的 息告诉
后端优化 。 后，后端根 这些新的 息， 和 图调 符
合 环检 果的样 。这样， 果我 有 分 且正确的 环检 ，
可以消除 积误差，得 一致的 和 图。

2.2.4　建图
建图（Mapping）是 构建 图的过程。 图（见图2-10）是 环
的 ， 这个 并不是 的，需要视SLAM的应用 。
 图2-10　形形 的 图[12] 。

于 用扫 机器人来说，这种主要 低矮平面里运动的机器人，
只需要一个二 的 图，标记哪里可以 过，哪里存 障碍 ，
一 导 了。 于一个相机， 有6 由度的运动，我 至
需要一张三 的 图。有些时 ，我 想要一个 亮的重建 果，不仅
是一组空 ，还需要带纹理的三角面片。另一些时 ，我 又不关心
图的样 ，只需要知 “A B 可 过， B C 不行”这样的
事情。甚至，有时不需要 图， 图可以由其 人 ， ，行
驶的 往往可以得 已绘制 的当 图。
于 图，我 有太 的想 和需 。 此，相比于前面 的视
觉里程计、 环检 和后端优化，建图并 有一个 的形式和 。
一组空 的集合也可以称为 图，一个 亮的3D 亦是 图，一个
标记着 市、村庄、 、 的图片还是 图。 图的形式随SLAM的
应用 合 。 上讲，可以分为 与 度量地图 拓扑地图
两种。
度量地图（ Metric Map）
 度量 图强调 确 表示 图中 的位 关 ， 常用稀疏
（Sparse）与稠 （Dense） 其分 。稀疏 图进行了一 程度的
象，并不需要表 所有的 。 ，我 一 分具有代表意义的
东西，称之为 标（Landmark）， 么一张稀疏 图 是由 标组成的
图， 不是 标的 分 可以忽略 。相 ，稠 图着重于建
所有看 的东西。 于 位来说，稀疏 标 图 足 了。 用于导
时，则往往需要稠 的 图（否则 上两个 标之 的 怎么办？）。
稠 图 常 某种分 率，由许 个 组成。 于二 度量 图
是 许 个 格 （ Grid ） ， 于三 度量 图则是许 方
（Voxel）。一 ，一个 含有占 、空 、未知三种 态，以表
该格 是否有 。当查询某个空 位 时， 图 给出该位 是否
可以 过的 息。这样的 图可以用于各种导 ， A*、D*[6] 等，
为机器人研究 所重视。 是我 也看 ，这种 图需要存 一个格
的 态，会 费 量的存 空 ， 且 情 下 图的许 细
分是无用的。另一方面， 规 度量 图有时会出现一致性问题。很
的一 转向误差，可 会导致两 屋 的 出现重叠， 图 。
拓扑地图（ Topological Map）
相比于度量 图的 确性， 图则更强调 图 之 的关
。 图是一个图（Graph），由 和 组成，只 虑 的连
性， A、B 是连 的， 不 虑 从A B 。 松了
图 确位 的需要，去 了 图的细 问题，是一种更为 的表
方式。 ， 图不 表 具有 杂 构的 图。 图进
行分割形成 与 ，又 用 图进行导 与 径规 ，仍是
有待研究的问题。

2.3　SLAM问题的数学表述
过前面的介 ，读 应该 SLAM中各个 的组成和主要功 有
了直观的了解。 仅仅靠直观印象并不 帮助我 出可以运行的程
序。我 要 上升 理性层次，也 是用 语言来 SLAM过程。
我 会用 一些变量和公式， 请读 心，我 会尽量让 足
楚。
 设 萝卜正 带着某种 感器 未知环 里运动，怎么用 语
言 这件事呢？首先，由于相机 常是 某些时刻 集 的，所以
我 也只关心这些时刻的位 和 图。这 一 连 时 的运动变成
了离 时刻 =1t ,···,K
当中发生的事情。 这些时刻，用 表示 x
萝卜 的位 。于是各时刻的位 记为 1 K ， 构成 x ,···,x
了 萝卜的 。 图方面，我 设 图是由许 个 路标
（Landmark）组成的， 个时刻， 感器会 量 一 分 标 ，得
的观 。不 设 标 一 有 个，用 1 N 表示 N y ,···,y
。 这样的设 中，“ 萝卜 带着 感器 环 中运动”，由
下两件事情 ：
1.什么是运动 ？我 要 虑从k- 1时刻 k 时刻， 萝卜的位 x
是 变化的。
2.什么是观测 ？ 设 萝卜 k 时刻于xk 了某一个 标yj
，我 要 虑这件事情是 用 语言来 的。
先来看运动。 常，机器人会 带一个 量 运动的 感器，比
说码盘 惯性 感器。这个 感器可以 量有关运动的读 ， 不一
直 是位 之差，还可 是加速度、角速度等 息。 ，无论是
什么 感器，我 用一个 用的、 象的 ：

这里， uk 是运动 感器的读 （有时也叫 输入 ），wk 为 。

意 ，我 用一个一 来 f
这个过程， 不具 明 的 用 f
方式。这 得 个 可以 代 意的运动 感器，成为一个 用的方
程， 不必 于某个 的 感器上。我 称为 。 运动方程
与运动方程相 应，还有一个 。观 方程 观测方程 的是，当
萝卜 x
k 位 上看 某个 标 j ，产生了一个观 y z k,j 。同样，
用一个 象的 h来 这个关 ：

这里，vk,j 是这次观 里的 。由于观 所用的 感器形式更

，这里的观 z 以及观 方程h 也有许 不同的形式。
 读 许会说，我 用的 f,h ， 乎并 有具 说明运动和
x y z
观 是怎么 事？同时，这里的 , , 又是什么东西呢？事 上，根
萝卜的真 运动和 感器的种 ，存 着 干种 方式 参数化
（Parameterization）。什么叫参 化呢？举 来说， 设 萝卜 平
面中运动， 么， 的位 [7] 由两个位 和一个转角来 ，即 k = x
同时，运动 感器 量 萝卜 意两个时 隔位 和
转角的变化量 于是，此时运动方程 可以具 化为这是
单的 性关 。不过，并不是所有的 感器 直 量出位移和角
度变化，所以也存 着其 形式更加 杂的运动方程， 时我 可 需
要进行动力 分析。关于观 方程，比方说 萝卜 带着一个二 光
感器。我 知 光 感器观 一个2D 标 时， 两个量：
r
标 与 萝卜本 之 的 离 和夹角ϕ 。记 标 为 =[ x y ]T y p ,p
（为 ，省略了下标），观 为 z =[r,ϕ ]
T
， 么观 方程
具 化为：

虑视觉SLAM时， 感器是相机， 么观 方程 是“ 标
后，得 图 中的 ”的过程。这个过程 涉 相机 的 ，
第5讲中详细介 ，这里暂且略过。
可见， 不同的 感器，这两个方程有不同的参 化形式。 果
我 用性， 取成 用的 象形式， 么SLAM过程可总 为
两个 本方程：
 这两个方程 了最 本的SLAM问题：当知 运动 量的读 ， u
以及 感器的读 z 时， x
解 位问题（估计 ）和建图问题（估
y
计 ）？这时，我 SLAM问题建 成了一个 状态估计问题
：
过带有 的 量 ，估计 的、隐藏着的 态变量？
态估计问题的 解，与两个方程的具 形式，以及 服从哪种
分布有关。 运动和观 方程是否为 性， 是否服从高斯分布进
行分 ，分为 线性 非线性 高斯 非高斯
/ 和 / 。其中 性高斯
（Linear Gaussian，LG ）是最 单的， 的无 的最优估计可以由
卡 曼 器（Kalman Filter，KF）给出。 杂的非 性非高斯
（Non-Linear Non-Gaussian，NLNG ）中，我 会 用以 展卡
曼 器（Extended Kalman Filter，EKF）和非 性优化两 方 去
解。直至21世 早期，以EKF为主的 器方 SLAM中占 了主导
位。我 会 工 性化，并以预 —更新两 步骤进行
解（见第10讲）。最早的 时视觉SLAM 即是 于EKF[2] 开发的。随
后，为了 服EKF的 （ 性化误差和 高斯分布 设），人
开 用 器（Particle Filter）等其 器，乃至 用非
性 优 化 的 方 。 时 至 今 日 ， 主 视 觉 SLAM 用 以 图 优 化 （ Graph
Optimization）为代表的优化技术进行 态估计[13] 。我 认为优化技术
已 明显优于 器技术，只要计 资 许， 常 向于 用优化
方 （见第10讲和第11讲）。
相 读 已 SLAM的 有了 致的了解， 我 仍需
一些问题。首先，要说明机器人 位置 x 是什么
。我 方才说 是 位置
有些 的。也许读 理解， 平面中运动的 萝卜可以用两个坐
标加一个转角的形式 位 参 化。 ，虽 我的 画风格有些二次
， 萝卜 更 时 是一个三 空 里的机器人。我 知 三 空
的运动由3个 构成，所以 萝卜的运动要由3个 上的平移，以及 着3
个 的旋转来 ，一 有6个 由度。 是否意味着随 用一个R6 中的
向量 了呢？我 发现事情并 有 么 单。 6 由度的 位
姿 [8]
， 表 ， 优化 ， 需要一 幅来介 ，这 是第3
讲和第4讲的主要 。随后，我 要说明 视觉SLAM中， 观测方程
参 化。 句话说，空 中的 标 是 影 一张 片上的。这
需要解 相机的成 ，我 第5讲介 。最后，当知 了这些
息， 怎么求解上述方程 ？这需要非 性优化的知识，是第6讲的 。
 这些 组成了本书 知识的 分。 进行 之后，我
细讨论视觉里程计、后端优化等更详细的知识了。可以看 ，
本讲介 的 构成了本书的一个 要。 果读 还 有很 理解上
面的 念，不 过 再 读一 。下面 要开 介 程序啦！

2.4　实践：编程基础
2.4.1　安装Linux操作系统
终于开 令人兴 的 环 啦！ 是否 了呢？为了 成本
书的 环 ，我 需要 一台电 。 可以 用笔记本 台式机，
当 最 是 个人的电 ， 为我 需要 上面安装操 进行
验。
我 的程序 以Linux上的C++程序为主。 验过程中，我 会
用 量程序库。 分程序库只 Linux 了 的 ，
Windows上的 则相 （相当）麻 。 此，我 不得不 已 具
关于Linux的 本知识了（参见上一讲的练习题），包 用 本的命
令，了解软件 安装。这样我 才无须讲解这些 。当 ， 不必
了解 Linux下开发C++程序，这正是下面会详细讲解的。
我 先来 建本书所需的 验环 。 为一本面向 的书，我
用Ubuntu 为开发环 。 Linux的各 发行版中，Ubuntu及其衍生
版本一直享有 用 友 的 誉。Ubuntu是一个开 操 ， 的
和软件可以 方 站（http://cn.ubuntu.com） 费下 ，并且
了详细的安装方式说明。同时， 华、中科 等国 各 高校也
了Ubuntu软件 ， 软件的安装十分 。 于 ，建议 用和
我 一样的环 ：Ubuntu 14.04。 果 想试试其 口味， 么Ubuntu
16.04、Ubuntu Kylin、Debian 7/8和Linux Mint 17/18也是不 的
。我 证书中所有代码 Ubuntu 14.04下 过了 的 试，
果 其 发行版，则无 确 是否会 问题。 可 需要 费
一些时 解 问题（不过 也可以 当 己的机会）。
来说，Ubuntu 各种库的 为 善，软件也非常丰 。尽 我
不 制 具 用哪种Linux发行版，不过 讲解中， 我们会以 Ubuntu
为例
14.04 ，且主要 用Ubuntu下的命令（ apt-get）， 不谈 其
 Linux下怎么操 了。一 情 下，程序 Linux 移植不会非常
琐。 果 想 Windows OS X下 用本书中的程序，则需要有一 的
移植 验。
现 ，请 己的PC上安装 Ubuntu 14.04。关于Ubuntu的安
装，可以 上 量 程，只要 即可，此 略过。最 单的方
式是 用虚 机（见图2-11）， 需要占用 量 存（我 的 验是4GB
以上）和CPU才 畅； 也可以安装双 ，这样会快一些， 需
要一个空白的U盘来 为启动盘。另 ，虚 机软件 硬件的 往
往不 ， 果希望 用 际的 感器（双目、Kinect等），则建议
用双 来安装Linux。

图2-11　一个运行 虚 机中的Ubuntu 14.04。

关于安装的 示：
•安装操 时请不要 “安装中下 更新”，并且断开 连
，这样可以 高安装速度。至于更新可以 安装 毕后再装。
果 有SSD硬盘，这个过程 用时15分 。
•安装 成后，请务必 软件 设 离 近的服务器上，以获得
更快的下 速度。 我 用 华的软件 常 以10MB/s的速度安
装软件[9] 。
现 ， 设 已 成功安装 Ubuntu，无论是 用虚 机还是双
的方式。 果 还不 悉Ubuntu，可以试试 的各种软件， 验一下
 的界面和交互方式[10] 。不过我必须 提醒
， 是新 朋友：不要
Ubuntu的用 界面上 费太 时 ！Linux有许 可 费时 的
方， 可 会找 某些 众的软件、一些 戏，甚至会为找一张 纸
费不 时 。 是请记住， 是用Linux来工 的。 是 本书中，
是用Linux来 习SLAM的，所以要尽量 时 习SLAM上。
了，我 一个目录， 本书中SLAM程序的代码。 ，可
以 代码 目录（/home）的“slambook”下。以后我 这个目
录称为“ 代码根目录 ”。同时，可以另 一个目录， 本书的Git
代码 制下来，方 验时随时 。本书的代码是 章 分的。
比 ，本讲的代码 slambook/ch2下，下一讲则 slambook/ch3下。
所以，现 请读 进 slambook/ch2下（ 应该会新建 件夹并进 该
件夹了吧）。

2.4.2　Hello SLAM
我 从最 本的程序开 。与许 计 机 书 一样，我 来书
一个HelloSLAM程序。不过 这件事之前，我 先来 程序是什么。
Linux中，程序是一个具有 行权 的 件。 可以是一个 本，
也可以是一个二进制 件，不过我 不 的后 名（不 Windows
样需要 成.exe 件）。我 常用的cd、ls等命令， 是位于/bin目
录下的可 行 件。 于其 方的可 行程序，只要 有可 行权
， 么当我 终端中 程序名时， 会运行。 C++ 程时，我
下面这样用 译器 一个 本 件 译成可 行程序。
 这是一个非常 单的程序。 应该 不费力 看懂 ，所以这里
不 加解 —— 果 际情 不是这样，请 先补习一下C++的 本知
识。这个程序只是 一个字符串 出 屏幕上 已。 可以用 本
器gedit（ Vim， 果 上一讲 习了Vim） 这些代码，并 存
上面列出的 径下。现 ，我 用 译器g++（g++是一个C++ 译器）
译成一个可 行 件。 ：

果顺 ，这条命令应该 有 出。 果机器上出现“command
not found”的 误 息，说明 可 还 有安装g++，请用 下命令进
行安装：

果出现 的 误，请再检查一 刚才的程序是否 正确。

刚才这条 译命令 helloSLAM.cpp这个 本 件 译成了一个可
行程序。我 检查当前目录，会发现 了一个a.out 件， 且 具有
行权 （终端里颜 不同）。我 ./a.out即可运行此程序：

我 所想，这个程序 出“Hello SLAM!”，告诉我 正确运

行。
请 顾一下我 之前 的事情。 这个 中，我 用 器
了helloSLAM.cpp的 代码， 后调用g++ 译器 进行 译，得 了
可 行 件。g++默认 件 译成a.out这个名字的程序（虽 有些
古怪， 是可以 受的）。 果我 愿意，也可以 这个 出的 件
名（留 习题）。这是一个极其 单的 ，我 用了 量的默认参
，几乎省略了所有中 步骤，为的是给读 一个 的印象（虽
可 有 会 ）。下面我 要用cmake来 译这个程序。
2.4.3　使用cmake
理论上说， 意一个C++程序 可以用g++来 译。 当程序规 越
来越 时，一个工程可 有许 个 件夹和 件，这时 的 译命
令 越来越 。 常一个 C++项目可 含有十几个 ，各 还存
着 杂的 赖关 。其中一 分要 译成可 行 件，另一 分 译成
库 件。 果仅靠g++命令，我 需要 量的 译 令， 个 译过
程会变得异常 琐。 此， 于C++项目， 用一些工程 理工具会更加
高 。 历史上工程师 曾 用make fi le进行 动 译， 下面要谈
的cmake比 更加方 。并且，我 会看 后面 的 库 用
cmake来 理 代码。
一个cmake工程中，我 会用cmake命令生成一个make fi le
件， 后，用make命令根 这个make fi le 件的 译 个工程。
读 可 还不知 make fi le是什么东西，不过 关 ，我 会 过
来 习。仍 以上面的helloSLAM.cpp为 ，这次我 不是直 用
g++， 是用cmake来制 一个工程， 后再 译 。 slambook/ch2/中
新建一个CMakeLists.txt 件， 下：
slambook/ch2/CMakeLists.txt

CMakeLists.txt 件用于告诉cmake我 要 这个目录下的 件 什

么事情。CMake-Lists.txt 件的 需要 守cmake的语 。这个示
中，我 示了最 本的工程： 一个工程名和一个可 行程序。根
，读 应该理解 句话 了些什么。
现 ， 当前目录下（slambook/ch2/），调用cmake 该工程进行
分析：
 cmake会 出一些 译 息， 后 当前目录下生成一些中 件，
其中最重要的 是MakeFile[11] 。由于MakeFile是 动生成的，我 不必
修 。现 ，用make命令 工程进行 译：

译过程中会 出一个 译进度。 果顺 过，我 可以得

CMakeLists.txt中 明的 个可 行程序helloSLAM。 行 ：

为我 并 有修 代码，所以得 的 果和之前是一样的。请
读 想想这种 和之前直 用g++ 译的区 。这次我 用cmake–
make的 ，cmake过程 理了工程 件之 的关 ， make过程 际调
用了g++来 译程序。虽 这个过程中 了调用cmake和make的步骤，
我 项目的 译 理工 ，从 一串g++命令，变成了 护 干个比
直观的CMakeLists.txt 件，这 明显降低 护 个工程的难度。比
， 果 想 新 一 个 可 行 件 ， 只 需 CMakeLists.txt 中 加 一
行“add_executable”命令即可， 后 的步骤是不变的。cmake会帮我
解 代码的 赖关 ， 无须 一 串g++命令。
现 这个过程中唯一让我 不 的是，cmake生成的中 件还留
我 的代码 件当中。当想要发布代码时，我 并不希望 这些中
件一同发布出去。这时我 还需要 一个个 除，十分不 。一种
更 的 是让这些中 件 一个中 目录中， 译成功后，
这个中 目录 除即可。所以，更常见的 译cmake工程的 下：
 我 新建了一个中 件夹“build”， 后进 build 件夹，
过cmake..命令 上一层 件夹，也 是代码所 的 件夹进行 译。这
样，cmake产生的中 件 会生成 build 件夹中，与 代码分开。
当发布 代码时，只要 build 件夹 即可。请读 行 这种方
式 ch2中的代码进行 译， 后调用生成的可 行程序（请记得 上一
步产生的中 件 ）。

2.4.4　使用库
一个C++工程中，并不是所有代码 会 译成可 行 件。只有带
有main 的 件才会生成可 行程序。 另一些代码，我 只想
包成一个东西， 其 程序调用。这个东西叫 。 库
一个库往往是许 、程序的集合，我 会 之后的练习中 触
许 库。 ，OpenCV库 了许 计 机视觉相关的 ， Eigen
库 了矩阵代 的计 。 此，我 要 习 用cmake生成库，并且
用库中的 。现 书 下libHelloSLAM.cpp 件。
slambook/ch2/libHelloSLAM.cpp

这个库 了一个printHello ，调用此 出一条 息。

是 有 main ，这意味着这个库中 有可 行 件。我
CMakeLists.txt里加上 下 ：
 这 条 命 令 告 诉 cmake ， 我 想 这个 件 译成一个叫
“hello”的库。 后，和上面一样， 用cmake 译 个工程：

这时， build 件夹中 会生成一个libhello.a 件，这 是我

得 的库。
静态库 共享库
Linux中，库 件分成 和 两种[12] 。静态库以.a
为后 名， 享库以.so 尾。所有库 是一些 包后的集合，差
于静态库每次被调用都会生成一个副本，而共享库则只有一个副本
，更省空 。 果想生成 享库 不是静态库，只需 用以下语句即
可。

此时得 的 件 是libhello_shared.so了。
库 件是一个压 包，里面有 译 的二进制 。不过， 果仅
有.a .so库 件， 么我 并不知 里面的 底是什么，调用的形
式又是什么样。为了让 人（ 己） 用这个库，我 需要 一
个头文件 ，说明这些库里 有些什么。 此， 于库的 用 ， 只要拿
到了头文件和库文件，就可以调用这个库了 。下面 libhello的
件。
slambook/ch2/libHelloSLAM.h
 这样，根 这个 件和我 刚才 译得 的库 件， 可以 用
printHello 了。下面 一个可 行程序来调用这个 单的 ：
slambook/ch2/useHello.cpp

后， CMakeLists.txt中 加一个可 行程序的生成命令，

刚才 用的库上：

过这两行语句，useHello程序 顺 用hello_shared库中的
代码了。这个 示了 生成并调用一个库。请 意， 于 人
的库，我 也可用同样的方式 进行调用， 合 己的程序
中。
除了已 示的功 之 ，cmake还有许 语 和 项，这里不一一
列举。习题中包含了一些cmake的 读材料，感兴趣的读 可 行 读。
现 ， 单 顾一下我 之前 了哪些事：
1.首先，程序代码由 件和 件组成。
2.带有main 的 件 译成可 行程序，其 的 译成库
件。
3. 果可 行程序想调用库 件中的 ， 需要参 该库 的
件，以明白调用的格式。同时，要 可 行程序 库 件上。
这几个步骤应该是 单 楚的， 际操 中 可 会 上一些问
题。比 说， 果代码里引用了库的 ， 忘了 程序 库上，
会发生什么呢？请试试 CMake-Lists.txt中的 分去 ，看看会发
生什么情 。 看懂cmake报告的 误消息吗？
2.4.5　使用IDE
最后，我 来谈谈 用集成开发环 （ Integrated
Development Environment，IDE）。前面的 程 可以用一个 单的
本 器来 成。 ， 可 需要 各个 件 来 去，查询某
个 的 明和 现。当 件很 时，这仍 很 琐。IDE为开发
了 转、补 、断 调试等很 方 的功 ，所以，我 建议读
一个IDE进行开发。
Linux下的IDE有很 种。虽 与Windows下的Visual Studio还有一
些差 ，不过 C++开发的也有 几种， ：Eclipse、QtCreator、
Code::Blocks、Clion，等等。同样，我 不强制读 用某种 的
IDE， 仅给出我 的建议。我 用的是Kdevelop（见图2-12）。 是
一个 费软件， Ubuntu的 中 了，这意味着 可以用apt-get来安
装 。Kdevelop的优 列举 下：
1. cmake工程。
2. C++ （包 11标 ）。有高亮、 转、补 等功 。
动 版代码。
3. 方 看 各个 件和目录树。
4.有一 译、断 调试等功 。
5.无须 费。
 图2-12　Kdevelop界面。

本上，我 一个IDE的功 要 具 ，所以读 不 试一

下。有时 会碰 一些问题， 某些 板 的解析响应比 慢
等，确 还不 善。不过相比于其 IDE， 是不 的。
Kdevelop 原 生 cmake 工 程 。 具 是， 终端建立
CMakeLists.txt后，用Kdevelop中的“工程 → 开 /导 工程 ” 开
CMakeLists.txt。软件会询问 几个问题，并且默认建立一个build 件
夹，帮 调用刚才的cmake和make命令。只要 下快 F8，这些 可以
动 成。图2-12的下面 分 显示了 译 息。
我 应IDE的 务交给读 己来 成， 并不 书中进行
详细说明。 果 是从Windows转过来的，会觉得 的界面与Visual
C++ Visual Studio 相 。请用Kdevelop 开刚才的工程 后进行
译，看看 出什么 息。相 会觉得比 开终端更方 一些。
不过，本 重 想讲的是 IDE中进行调试。 Windows下 程
的同 半会有 Visual Studio下断 调试的 历。不过 Linux中，
默认的调试工具gdb只 了 本界面， 新 来讲不太方 。有些IDE
了断 调试功 （底层仍旧是gdb），Kdevelop 是其中之一。要
用Kdevelop的断 调试功 ， 需要 成以下几件事：
 1. CMakeLists.txt中 工程调为Debug 译 式。
2.告诉Kdevelop 想运行哪个程序。 果有参 ，也要 的参
和工 目录。
3.进 断 调试界面， 可以单步运行，看 中 变量的 了。
第一步， CMakeLists.txt中加 下面的命令来设 译 式：

cmake 带一些 译相关的 变量， 可以 译过程进行更

细的控制。 于 译 ， 常 有 调 试 用 的 Debug 式 与 发 布 用 的
Release 式。 Debug 式中，程序运行 慢， 可以进行断 调试；
Release 式则速度 快， 有调试 息。我 程序设 成Debug
式， 断 了。 下来，告诉Kdevelop 想启动哪个程序。
第二步， 开“运行 →
启 动 器 ” ， 后 单 击 左 的 “Add
→
New 应用程序”。 这一步中，我 的 务是告诉Kdevelop想要启动
哪一个程序。 图2-13所示，既可以直 一个cmake的工程目标（也
是我 用add_executable 令构建的可 行程序），也可以直 向
一个二进制 件。建议 用第二种方式，根 我 的 验，这样更 出
现问题。
 图2-13　启动器设 界面。

第二栏里，可以设 程序的运行参 和工 目录。有时程序是有

运行参 的， 会 为main 的参 被 。 果 有则可以留
空， 于工 目录亦是 此。 这两项后，可以单击“OK”
存 果。
刚才这几步我 了一个应用程序的启动项。 于 一个启动
项 ， 我 可 以 单 击 “Execute” 直 启动这个程序，也可单
击“Debug” 进行断 调试。读 可以试着单击“Execute”
，查看 出的 果。现 ，为了调试这个程序，单击printHello 行
的左 ， 加一个断 。 后，单击“Debug” ，程序会 留 断
等待， 图2-14所示。

图2-14　调试界面。

调试时，Kdevelop会切 调试 式，界面会发生一 变化。 断

，可以用单步运行（F10 ）、单步 进（F11 ）、单步 出（F12
）功 控制程序的运行。同时，可以 开左 的界面，查看 变量
的 。 “ 止” ， 束调试。调试 束后，Kdevelop会
正常的开发界面。
现 应该 悉了 个断 调试的 程。今后， 果 程序运行阶
发生了 误，导致程序崩 ， 可以用断 调试确 出 的位 ，
后加以修正[13] 。
 习题
1. 读 献[1]和[14]， 看懂其中的 吗？
2.* 读SLAM的 献， [9,15,16,17,18]等。这些 献关于
SLAM的看 与本书有 异同？
3.g++命令有哪些参 ？怎么 参 可以更 生成的程序 件名？
4. 用build 件夹来 译 的cmake工程， 后 Kdevelop中试
试。
5.刻意 代码中 加一些语 误，看看 译会生成什么样的
息。 看懂g++的 误 息吗？
6. 果忘了 库 可 行程序上， 译会报 吗？报什么样的
？
7.* 读《cmake 》，了解cmake的其 语 。
8.* 善hello SLAM 程序， 成一个 程序库，安装 本 硬
盘中。 后，新建一个工程， 用 fi nd_package找这个库并调用。
9.* 寻 找 其 cmake 材 料 ， 了 解 cmake ，
https://github.com/TheErk/CMake-tutorial。
10.找 Kdevelop的 方 站，看看 还有哪些 性。 用上了
吗？
11. 果 上一讲 习了Vim，请试试Kdevelop的Vim 功 。

[1] 不过现 中我 会有一个 的 ， 和 的区分。

[2] 画成单目会比 吓人。

[3] 上的原 会 视觉里程计一讲中解 。

[4] 可以用 机录个 视频试试。

[5] 更 时 称为后端（Back End）。由于主要 用的是优化方 ， 称为后端优化。

[6] https://en.wikipedia.org/wiki/A*_search_algorithm

[7] 本书中，我 以“位 ”这个词表示“位 ”加上“ 态”。

[8] 我 以后称 为位 （Pose），以与位 进行区 。我 说的位 ，包含了旋转

（Rotation）和平移（Translation）。
[9] 感谢TUNA同 的 护！

[10] 人第一次看 Ubuntu 觉得很 亮。

[11] MakeFile是一个 动化 译的 本，读 现 可以 理解成一 动生成的

译 令， 无须理会其 。

[12] 半猜 了， 并不叫 动态库。

[13] 不是直 给我 发 件询问怎么 理 的问题。

 第3讲　三维空间刚体运动
主要目标
1.理解三 空 的刚 运动 方式：旋转矩阵、变 矩阵、
和 角。
2. Eigen库的矩阵、几 用方 。
上一讲中，我 讲解了视觉SLAM的框架与 。本讲 介 视觉
SLAM的 本问题之一： 一个刚体在三维空间中的运动是如何描述的 。我
当 知 这由一次旋转加一次平移组成。平移确 有太 问题，
旋转的 理是件麻 事。我 介 旋转矩阵、 、 角的意
义，以及 是 运 和转 的。 分，我 介 性代
库Eigen。 了C++中的矩阵运 ，并且 的Geometry 还 了
等刚 运动的 。Eigen的优化非常 善， 是 的 用方 有
一些 的 方，我 会 程序中介 。
3.1　旋转矩阵
3.1.1　点和向量，坐标系
我 日常生 的空 是三 的， 此我 生来 习惯于三 空 的
运动。三 空 由3个 组成，所以一个空 的位 可以由3个坐标
。不过，我 现 要 虑 刚体
， 不光有位 ，还有 的 态。相
机也可以看成三 空 的刚 ，于是位 是 相机 空 中的哪个
方， 态则是 相机的朝向。 合起来，我 可以说，“相机正 于
, ,
空 (0 0 0) ，朝向正前方”这样的话。 是这种 语言很
琐，我 更喜欢用 语言来 。
我 从最 本的 讲起： 和 点 向量
。 的几 意义很 易理
解。向量是什么呢？ 是 性空 中的一个 ，可以 想象成从原
向某 的一个 。需要 读 的是，请不要 向量与 的 坐标
两个 念 。一个向量是空 当中的一样东西，比 说 。这里 并 a a
不是和 干个 相关 的。只有当我 这个三 空 中的某个 坐
标系 时，才可以谈论该向量 此坐标 下的坐标，也 是找 干个
应这个向量。 ，三 空 中的某个向量的坐标可以用R3 当中的3
个 来 。某个 的坐标也可以用R3 来 。怎么 呢？ 果我
确 了一个坐标 ，也 是一个 性空 的 ( 1 3 )， e ,e ,e
么
a 坐标 了：
2

可以谈论向量 这组 下的

所以坐标的具 取 ，一是和向量本 有关，二是和坐标 的 取

有关。坐标 常由3个正交的坐标 组成（尽 也可以有非正交的，
际中很 见）。 ，给 和 时， x y z
可以 过右 （ 左
） 则由 x×y义出来。根 义方式的不同，坐标 又分为左
和右 。左 的第3个 与右 方向相反。 验来讲，人 更习
惯 用右 ，尽 也有一 分程序库仍 用左 。
 根 本的 性代 知识，我 可以谈论向量与向量，以及向量与
之 的运 ， 乘、加 、 、 积、 积等。 乘和 则运
是相当 本的 ，这里不再赘 。 积 读 来说可 有些陌
生，这里给出 的运 方式。 于 a,b∈
R3 ， 积可以 成：

积可以 向量 的 影关 。 积则是这个样 ：

积的方向 直于这两个向量， 为 sin |a||b|

，是两 〈a,b〉
个向量张成的 形的有向面积。 于 积，我 引 了 ∧ 符号， a
成一个矩阵。事 上是一个反 称矩阵（Skew-symmetric）， 可以
∧ 记成一个反 称符号。这样 积 a×b
成了矩阵与向量的乘
a b
∧ ， 变成了 性运 。这个符号 后 常用 ，请记住
。 积只 三 向量存 义，我 还 用 积表示向量的 。 旋转
为什么 积可以表示旋转呢？
虑两个不平行的向量 a,b
，我 要 从 之 是 旋转 a b
的， 图3-1所示。我 可以用一个向量来 三 空 中两个向量的旋
转关 。 右 则下，我 用右 的4个 从 转向 ， 朝向 a b
是旋转向量的方向，事 上也是 a×b
的方向。 的 则由 和 的 a b
夹角 。 过这种方式，我 构造了从 a b
的一个旋转向量。这个
向量同样位于三 空 中， 此坐标 下，可以用3个 来 。
 图3-1　左右 的区 与向量 的旋转。a b 的旋转可以由向量w 来 。

3.1.2　坐标系间的欧氏变换
与向量 的旋转 ，同样可以 两个坐标 之 的旋转关 ，
再加上平移， 称为坐标 之 的 变换 关 。 机器人的运动过程中，
常见的 是设 一个惯性坐标 （ 叫世界坐标 ），可以认为
是 不动的， x ,y ,z
图3-2中的 W W W 义的坐标 。同时，相机
机器人则是一个移动坐标 ， xC ,yC ,zC 义的坐标 。我 可
会问：相机视野中某个向量p ， 的坐标为pc ， 世界坐标 下看，
的坐标pw 。这两个坐标之 是 转 的呢？这时， 需要先得 该
机器人坐标 的坐标 ，再根 机器人位 转 世界坐标
中，这个转 关 由一个矩阵T 来 ， 图3-2所示。
 p
图3-2　坐标变 。 于同一个向量 ， 世界坐标 下的坐标 pw和 相机坐
标 下的坐标 p c 是不同的。这个变 关 由坐标 的变 矩阵T 来 。

相机运动是一个刚 运动， 证了同一个向量 各个坐标 下的

度和夹角 不会发生变化。这种变 称为 。想象 机欧氏变换
空中， 落 碎之前，只可 有空 位 和 态的不同，
己的 度、各个面的角度等性质不会有 变化。这样一个 变 由
一个旋转和一个平移两 分组成。首先来 虑旋转。我 设某个单位正
交 过一次旋转变成了 么， 于同一个向量 a
（ 意该向量并 有随着坐标 的旋转 发生运动）， 两个坐标
下的坐标为 和 根 坐标的 义，有：

为了 两个坐标之 的关 ，我 上 等式的左右两 同时左

乘 么左 的 变成了单位矩阵，所以：
 我 中 的矩阵 出来， 义成一个矩阵 。这个矩阵由两组 R
之 的 积组成，刻画了旋转前后同一个向量的坐标变 关 。只要旋
转是一样的， 么这个矩阵也是一样的。可以说，矩阵 了旋转本 R
。 此又称为 。 旋转矩阵
旋转矩阵有一些 的性质。事 上， 是一个行列式为1的正交矩
阵[1] 。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以
旋转矩阵的集合 义 下：

SO( )是 n 正交 （Special Orthogonal Group）的意思。我

解 “ ”的 留 下一讲。这个集合由 n
空 的旋转矩阵组
成， ，SO(3) 是三 空 的旋转了。 过旋转矩阵，我 可以直
谈论两个坐标 之 的旋转变 ， 不用再从 开 谈起。 句话
说， 旋转矩阵可以描述相机的旋转 。
由于旋转矩阵为正交矩阵， 的 （即转 ） 了一个相反的旋
转。 上面的 义方式，有：

显 R T
刻画了一个相反的旋转。
变 中，除了旋转之 还有平移。 虑世界坐标 中的向量 a
， 过一次旋转（用 R
）和一次平移 后，得 了 ′ ， 么 旋转 t a
和平移合 一起，有：

t
其中， 称为平移向量。相比于旋转，平移 分只需 这个平移量
加 旋转之后的坐标上，显得非常 。 过上式，我 用一个旋转矩
R
阵 和一个平移向量 了一个 t
空 的坐标变 关 。
3.1.3　变换矩阵与齐次坐标
式（3.8） 表 了 空 的旋转与平移，不过还存 一个
问题：这里的变 关 不是一个 性关 。 设我 进行了两次变 ： R
1,t
1 和 2 R ,t 2 ， 足：

是从a c 的变 为

这样的形式 变 次之后会过于 杂。 此，我 要引 齐次坐

标和变 矩阵重 式(3.8)：

这是一个 技巧：我 一个三 向量的末尾 加1， 其变成了

向量，称为 齐次坐标 。 于这个 向量，我 可以 旋转和平移
一个矩阵里面， 得 个关 变成 性关 。该式中，矩阵 称为 T
变换矩阵（ Transform Matrix） a
。我 暂时用 表示 的齐次坐标。
稍微说一下齐次坐标。 是 影几 里的 念。 过 加最后一
，我 用4个 了一个三 向量，这显 了一个 由度，
许我 变 成 性的形式。 齐次坐标中，某个 的 个分量同 x
乘一个非零常 后， k 仍然表示同一个点
。 此，一个 的具 坐标
, , , , , ,
不是唯一的。 [1 1 1 1]T 和[2 2 2 2]T 是同一个 。 当最后
一项不为零时，我 总可以 所有坐标除以最后一项，强制最后一项为
1，从 得 一个 唯一的坐标表示（也 是转 成非齐次坐标）：

这时，忽略 最后一项，这个 的坐标和 空 是一样的。

靠齐次坐标和变 矩阵，两次变 的 加 可以有很 的形式：
 是区分齐次和非齐次坐标的符号令我 感 厌 。 不引起 义
的情 下，以后我 直 成 = 的样b Ta ，默认其中是齐次坐
标了。
T
关于变 矩阵 ， 具有比 的 构：左上角为旋转矩阵，右
为平移向量，左下角为0向量，右下角为1。这种矩阵又称为
（Special Euclidean Group）：

与SO(3)一样， 解该矩阵的 表示一个反向的变 ：

最后，为了 符号的 ， 不引起 义的情 下，我 以后不

区 齐次坐标与普 坐标的符号， 默认使用的是符合运算法则的那一种
。 ，当我 Ta
时， 用的是齐次坐标（不 计 ）。 Ra
时， 用的是非齐次坐标。 果 一个等式中， 设齐次坐标 普
坐标的转 ，是已 了的—— 为齐次坐标和非齐次坐标之 的
转 事 上非常 易。
顾一下：首先，介 了向量及其坐标表示，并介 了向量 的运
； 后，坐标 之 的运动由 变 ， 由平移和旋转组成。
旋转可以由旋转矩阵SO(3) ， 平移直 由一个R3 向量 。最后，
果 平移和旋转 一个矩阵中， 形成了变 矩阵SE(3)。

3.2　实践：Eigen
本讲的 分有两 。第一 分中， 讲解 用Eigen来表示
矩阵、向量，随后引申至旋转矩阵与变 矩阵的计 。本 的代码
slambook/ch3/useEigen中。
 Eigen[2] 是一个C++开 性代 库。 了快速的有关矩阵的
性代 运 ，还包 解方程等功 。许 上层的软件库也 用Eigen进行
矩阵运 ，包 g2o、Sophus等。 应本讲的理论 分，我 来 习一下
Eigen的 程。
的PC上可 还 有安装Eigen。请 以下命令进行安装：

分常用的库 已 Ubuntu软件 中 。以后， 想要安装某

个库，不 先 一下Ubuntu的软件 中是否已 。 过apt命令，我
方 安装Eigen。 顾上一讲的知识，我 知 一个库由 件
和 库 件 组 成 。 Eigen 件 的 默 认 位
“/usr/include/eigen3/” 中 。 果 不 确 ， 可 以 以下命令查
找：

相比于其 库，Eigen的 之 于， 是一个 用 件 建起

来的库（这非常神 ！）。这意味着 只 找 的 件， 有.so
.a 样的二进制 件。 用时，只需引 Eigen的 件即可，不需
要 库 件（ 为 有库 件）。下面 一 代码来 际练习一下
Eigen的 用：
slambook/ch3/useEigen/eigenMatrix.cpp
 这个 程 示了Eigen矩阵的 本操 与运 。要 译 ，需要
CMakeLists.txt里 Eigen的 件目录：

重 一 ， 为 Eigen 库 只 有 件，所以不需要再用
target_link_libraries语句 程序 库上。不过， 于其 分
库， 时 需要用 命令。这里的 并不见得是最 的， 为
其 人可 Eigen安装 了不同位 ， 么 必须 动修 这里的
件目录。 之后的工 中，我 会 用 fi nd_package命令去 库，
不过 本讲中暂时 这个样 。 译 这个程序后，运行 ，可以看
各矩阵的 出 果。
 由于 代码中给出了详细的 ， 此 不一一解 行语句了。
本书中，我 仅给出几 重要 方的说明（后面的 分亦
这个风格）。
1.读 最 亲 一 上面的代码（不包 ）。至 要 译
运行一 上面的程序。
2.Kdevelop可 不会 示C++成员运 ，这是 得不 善导致
的。请 着上面的 即可，不必理会 是否 示 误。
3.Eigen 的矩阵和MATLAB很相 ，几乎所有的 当 矩阵来
理。 是，为了 现更 的 率， Eigen中需要 矩阵的 和
。 于 译时期 知 的矩阵， 理起来会比动态变化 的
矩阵更快一些。 此， 旋转矩阵、变 矩阵这样的 ， 可
译时期确 的 和 。
4.Eigen 的矩阵 现比 杂，这里不 介 ，我 希望
用 fl oat、double等 样 用Eigen的矩阵。这应该是符
合其设计 衷的。
5.Eigen矩阵不 动 升，这和C++的 建 有
差异。 C++程序中，我 可以 一个 fl oat 和double 相加、
相乘， 编译器会自动把数据类型转换为最合适的那种 。 Eigen中，
出于性 的 虑，必须 显式地 矩阵 进行转 。 果忘了这样
， Eigen 会 （ 不 太 友 ） 示 一 个 “YOU MIXED DIFFERENT
NUMERIC TYPES...”的 译 误。 可以 试找一下这条 息出现
误 示的哪个 分。 果 误 息太 最 存 一个 件里再找。
 6.同理， 计 过程中也需要 证矩阵 的正确性，否则会出
现“YOU MIXED MATRICES OF DIFFERENT SIZES” 误。请不要 怨这种
误 示方式， 于C++ 板 程， 示出可以 读的 息已 是
很幸运的了。以后， 发现Eigen出 ， 可以直 寻找 的 分，推
出了什么问题。
7. 我 的 程只介 了 本的矩阵运 。 可以 读
http://eigen.tuxfamily.org/dox-devel/modules.html 习 更 的
Eigen知识。这里只 示了最 单的 分， 看懂 示程序不等于 已
练操 Eigen。
最后一 代码中比 了 与 QR分解的运行 率， 可以看看
己机器上的时 差异，两种方 是否有明显的差异？

3.3　旋转向量和欧拉角
3.3.1　旋转向量
我 重新 理论 分。有了旋转矩阵来 旋转，有了变 矩阵
一个6 由度的三 刚 运动，是不是已 足 了呢？矩阵表示方式
至 有以下几个 ：
1.SO(3)的旋转矩阵有9个量， 一次旋转只有3个 由度。 此这种
表 方式是 的。同理，变 矩阵用16个量表 了6 由度的变 。
么，是否有更 的表示呢？
2.旋转矩阵 带有约束： 必须是个正交矩阵，且行列式为1。变
矩阵也是 此。当想要估计 优化一个旋转矩阵/变 矩阵时，这些约
束会 得 解变得更 难。
此，我 希望有一种方式 旋转和平移。 ，用
一个三 向量表 旋转，用六 向量表 变 ，可行吗？事 上， 前
面介 积的 分，我 过 这件事。我 介 了 用
积表 两个向量的旋转关 。 于坐标 的旋转，我 知 ， 意旋转
可以用 一个旋转轴和一个旋转角 来刻画。于是，我 可以 用一个向
量，其方向与旋转 一致， 度等于旋转角。这种向量称为 旋转向量
（ 角，Axis-Angle）。这种表示 只需一个三 向量即可 旋
转。同样， 于变 矩阵，我 用一个旋转向量和一个平移向量即可
表 一次变 。这时的 正 是六 。
事 上，旋转向量 是下一讲 介 的李代 。所以本讲中读
只需知 旋转可以这样表示即可。剩下的问题是，旋转向量和旋转矩阵
之 是 转 的呢？ 设有一个旋转 为 ，角度为 n θ
的旋转，显
， 应的旋转向量为 θn
。从旋转向量 旋转矩阵的转 过程由 罗
德里格斯公式 （Rodrigues’s Formula）表明，由于推导过程比
杂，这里不 ，只给出转 的 果[3] ：

符号 ∧ 是向量 反 称的转 符，见式（3.3）。反之，我 也可

以计 从一个旋转矩阵 旋转向量的转 。 于转角 ，有： θ

此：

关于转 n ，由于旋转 上的向量 旋转后不发生 变，说明

此，转 n
是矩阵 R
征 1 应的 征向量。 解此方程，再归
一化， 得 了旋转 。读 也可以从“旋转 过旋转之后不变”的
几 角度看待这个方程。顺 一下，这里的两个转 公式 下一讲仍
出现， 会发现 正是SO(3)上李 与李代 的 应关 。

3.3.2　欧拉角
下面来说说 角。
 无论是旋转矩阵、旋转向量， 虽 旋转， 我 人
是非常不直观的。当我 看 一个旋转矩阵 旋转向量时，很难想象出
这个旋转究竟是什么样的。当 变 时，我 也不知 是向哪个
方向 转动。 角则 了一种非常直观的方式来 旋转——
用了3 个分离的转角
， 一个旋转分解成3次 不同 的旋转。当 ，
由于分解方式有许 种，所以 角也存 着不同的 义方 。比
说，先 旋转，再 X ，最后 Y Z
， 得 了一个 的旋 XY Z
转。同理，可以 义 、 ZY Z ZY X
等旋转方式。 果讨论得更细一些，
还需要区分 次是 固定轴
旋转的，还是 旋转之后的轴
旋转的，这
也会给出不一样的 义方式。
许 空、 中听说过“俯 角”“ 角”这些词。
角当中比 常用的一种， 是用“ - 俯 - 转 ” （ yaw-pitch-
roll）3个角度来 一个旋转的。由于 等价于 的旋转， 此 ZY X
以 ZY X
为 。 设一个刚 的前方（朝向我 的方向）为 ，右 X
为Y ，上方为 Z
， 图3-3所示。 么， ZY X
转角相当于 意旋
转分解成以下3个 上的转角：
1. Z 旋转，得
的 角yaw；
2. 旋转之后 的Y 旋转，得 俯 角pitch；
3. 旋转之后 的X 旋转，得 转角roll。

此时，可以 用[r,p,y ] 这样一个三 的向量

T
意旋转。这个
向量十分直观，我 可以从这个向量想象出旋转的过程。其 的 角
亦是 过这种方式， 旋转分解 3个 上，得 一个三 的向量，只不
过 用的 及顺序不一样。这里介 的rpy角是比 常用的一种，只有很
的 角种 会有rpy这样 人口的名字。不同的 角是 旋转
的顺序来称呼的。 ，rpy角的旋转顺序是 。同样，也有 ZY X XY
Z,ZY Z
这样的 角—— 是 有专 的名字了。 得一 的
是， 分领 用 角时 有各 的坐标方向和顺序上的习惯，
不一 和我 这里说的相同。

角的一个重 是会碰 著名的万向 问题（Gimbal Lock[4]

）： 俯 角为 ±
90 ° 时，第一次旋转与第三次旋转 用同一个
， 得 丢 了一个 由度（由3次旋转变成了2次旋转）。这被称
为 异性问题， 其 形式的 角中也同样存 。理论上可以证明，
只要想用3个 来表 三 旋转时， 会不可 碰 异性问题。
由于这种原理， 角不 于 和 代，往往只用于人机交互中。我
也很 SLAM程序中直 用 角表 态，同样不会 优
化中 用 角表 旋转（ 为 具有 异性）。不过， 想验证
己的 是否有 ，转 成 角 快速分 果是否正确。

图3-3　 角的旋转示意图。上方为ZYX角 义。下方为pitch=90 ° 时，第三次

旋转与第一次 转角相同， 得 丢 了一个 由度。 果 还 有理解万向
，可以看看相关视频，理解起来会更方 。

3.4　四元数
3.4.1　四元数的定义
旋转矩阵用9个量 3 由度的旋转，具有 性； 角和旋转
向量是 的， 具有 异性。事 上，我 找不 不带奇异性的三维
向量描述方式 [19]
。这有 于用两个坐标表示 球表面（ 度和
度）， 必 存 异性（ 度为 ±
90 ° 时 度无意义）。三 旋
转是一个三 形，想要无 异性 表 ，用3个量是不 的。
忆以前 习过的 。我 用 集C表示 平面上的向量，
的乘 则表示 平面上的旋转： ，乘上 i相当于 时 一
个 向量旋转90 ° 。 ， 表 三 空 旋转时，也有一种 于
的代 ： 四元数
（Quaternion）。 是Hamilton找 的一种
展的 。 既是紧凑的，也没有奇异性 。 果说 ， 不 直
观，其运 稍 杂些。
一个 q 有一个 和三个虚 。本书 前面（也
有 方 后面）， 下面这样：

其中i,j,k 为 的三个虚 。这三个虚 足以下关 式：

由于 的这种 表示形式，有时人 也用一个标量和一个向量来

表 ：

这里， 称为 s 的 ， v
称为 的虚 。 果一个
的虚 为0，称之为 实四元数 。反之， 的 为0，则称之为 虚四元
数 。
这和 非常相 。 虑 三 空 需要3个 ， 也有3个虚
， 么，一个虚 不 应 一个空 呢？事 上我 是
这样 的。同理，我 知 一个 为1的 可以表示 平面上的 旋
转（ 有 度的 ）， 么，三 空 中的旋转是否 用单位
表 呢？ 案也是 的。
我 用 单位四元数
表示三 空 中 意一个旋转，不过这种表
方式和 有着微 的不同。 中，乘以 意味着旋转90 ° 。这是 i
否意味着 中，乘 i
是 i
旋转90 ° ？ 么， = 是否意 ij -k
味着，先 i
转90 ° ，再 j
转90 ° ， 等于 转 90 ° ？读 可 k -
以找一 机比 一下—— 后 会发现情 并不是这样。正确的情形
应该是，乘以 i
应着旋转180 ° ，这样才 证 = 的性质。 ij -k i
2
-= 1，意味着 i
旋转360 ° 后得 一个相反的东西。这个东西要
旋转两周才会和 原先的样 相等。
这 乎有些玄 了， 的解 需要引 太 额 的东西，我 还
是 静一下 眼前。至 ，我 知 单位 表 三 空 的
旋转。这种表 方式和旋转矩阵、旋转向量有什么关 呢？我 不 先
来看旋转向量。 设某个旋转是 单位向量 =[ x y z ]T 进行了角 n n ,n ,n
度为 θ 的旋转， 么这个旋转的 形式为

反之，亦可从单位 中计 出 应旋转 与夹角：

这个式 给了我 一种微 的“转了一半”的感觉。同样， 式

（3.19）的θ加上2 π
，我 得 一个相同的旋转， 此时 应的
-q
变成了 。 此， 中， 任意的旋转都可以由两个互为相反数
的四元数表示 。同理，取 为0，则得 一个 有 θ
旋转的
：

3.4.2　四元数的运算
和 常 一样，可以进行一 列的运 。常见的有 则运
、 乘、 、 轭等。下面分 介 。
现有两个 qa ,qb ， 的向量表示为[ sa ,va ], [sb ,vb ]，
原 表示为
 么，其运 可表示 下。
1.加 和
qa ,qb 的加 运 为

2.乘
乘 是 qa 的 一项与 qb 的 项相乘，最后相加，虚 要 式
（3.18）进行。 理可得：

虽 稍为 杂， 形式上是 齐有序的。 果 成向量形式并 用

积运 ，该表 会更加 ：

该乘 义下，两个 的 乘积仍是 的，这与 也是一

致的。 ， 意 ，由于最后一项 积的存 ， 乘 常是不
可交 的，除非 a 和 b v v
R3 中 ，此时 积项为零。

3. 轭
的 轭是 虚 取成相反 ：

轭与其本 相乘，会得 一个 ，其 为 的
平方：
 4.
的 义为

可以验证，两个 乘积的 即为 的乘积。这 证了单位

相乘后仍是单位 。

5.
一个 的 为

此 义， 和 己的 的乘积为 1：

果 为单位 q ，其 和 轭 是同一个量。同时，乘积的
有和矩阵相 的性质：

6. 乘与 乘
和向量相 ， 可以与 相乘：

乘是 两个 个位 上的 分 相乘：

3.4.3　用四元数表示旋转
我 可以用 表 一个 的旋转。 设一个空 三 p=
[x,y,z ∈ ] R3 ，以及一个由 角 n,θ 的旋转。三 p 过旋转
之后变为 p′ 。 果 用矩阵 ， 么有 p′ =Rp 。 果用
旋转， 的关 又 来表 呢？
首先， 三 空 用一个虚 来 ：

这相当于 的3个虚 与空 中的3个 相 应。 后，参

式(3.19)，用 q 表示这个旋转：

么，旋转后的 p′ 即可表示为这样的乘积：

可以验证（留 习题），计 果的 为0， 为 虚 。其

虚 的3个分量表示旋转后3D 的坐标。

3.4.4　四元数到旋转矩阵的转换
意单位 了一个旋转，该旋转亦可用旋转矩阵 旋转向
量 。从旋转向量 的转 方式已 式（3.20）中给出。
此，现 看来 转 为矩阵的最直观方 ，是先 转 q
为 角 θ n
和 ， 后再根 德里格斯公式转 为矩阵。不过 样要计
一个arccos ，代价 。 际上这个计 是可以 过一 的技巧
过的。这里省略推导过程，直 给出 旋转矩阵的转 方式。
设 q =q +q i +q j +q k ，
0 1 2 3 应的旋转矩阵 为 R

反之，由旋转矩阵 的转 下。 设矩阵为 R ={mij

},i,j∈ [1, 2, 3]，其 应的 q 由下式给出：
 得一 的是，由于 和 q -q
表示同一个旋转，事 上一个 应的 R
表示并不是唯一的。同时，除了上面给出的转 方式之 ，还存
其 几种计 方 ， 本书 省略了。 际 程中，当 0 近0时， q
其 3个分量会非常 ，导致解不稳 ，此时我 再 虑 用其 的方式
进行转 。
最后，无论是 、旋转矩阵还是 角， 可以用来 同
一个旋转。我 应该 际中 最为方 的形式， 不必 于某种
的形式。 随后的 和习题中，我 会 示各种表 方式之 的
转 ，以加 读 的印象。

3.5　*相似、仿射、射影变换
3D空 中的变 ，除了 变 之 ，还存 其 几种，只不过
变 是最 单的。 一 分和 量几 有关， 为 之后的讲解中
可 会 ，所以先 列出来。 变 了向量的 度和夹角，相
当于我 一个刚 原 不动 进行了移动 旋转，不 变 的样
。 其 几种变 则会 变 的 形。 有 的矩阵表示。
1.相 变
相 变 比 变 了一个 由度， 许 进行 匀 ，
其矩阵表示为

意 旋转 分 了一个 ，表示我 s向量旋转之

后，可以 x,y,z
三个坐标上进行 匀 。由于含有 ，相 变
不再 图形的面积不变。 可以想象一个 为1的立方 过相 变
后，变成 为10的样 （ 仍 是立方 ）。
2. 变
 变 的矩阵形式 下：

与 变 不同的是， 变 只要 是一个可 矩阵， A 不必

是正交矩阵。 变 也叫正交 影。 过 变 之后，立方 不
再是方的了， 是各个面仍 是平行 形。
3. 影变
影变 是最一 的变 ， 的矩阵形式为

A
的左上角为可 矩阵 ，右上角为平移 ，左下角为 T
。由 t a
于 用了齐次坐标，当 v/
=0时，我 可以 个矩阵除以 得 一个右 v
下角为1的矩阵；否则得 右下角为0的矩阵。 此，2D的 影变 一
有8个 由度，3D则 有15个 由度。 影变 是现 讲过的变 中，形
式最为一 的。从真 世界 相机 片的变 可以看成一个 影变 。
读 可以想象一个原本方形的 板砖， 片当中是什么样 ：首先，
不再是方形的。由于近 远 的关 ， 甚至不是平行 形， 是
一个不规则的 形。
表3-1总 了目前讲 的几种变 的性质。 意 “不变性质”中，
从上 下是有包含关 的。 ， 变 除了 积之 ，也具有
平行、相交等性质。

表3-1常见变 性质比
 我 之后会说 ，从真 世界 相机 片的变 是一个 影变 。
果相机的 为无穷远， 么这个变 为 变 。不过， 详细讲
相机 之前，我 只要 有个 致的印象即可。

3.6　实践：Eigen几何模块
现 ，我 来 际 练一下前面讲 的各种旋转表 方式。我
Eigen中 用 、 角和旋转矩阵， 示 之 的变 方式。
我 还会给出一个可视化程序，帮助读 理解这几个变 的关 。
slambook/ch3/useGeometry/useGeometry.cpp
 Eigen中 各种形式的表 方式总 下。请 意 种 有单
度和双 度两种 ， 且和之前一样，不 由 译器 动转 。
下面以双 度为 ， 可以 最后的d 成f，即得 单 度的
构。
•旋转矩阵（3× 3）：Eigen::Matrix3d。
•旋转向量（3× 1）：Eigen::AngleAxisd。
• 角（3× 1）：Eigen::Vector3d。
• （4× 1）：Eigen::Quaterniond。
• 变 矩阵（4× 4）：Eigen::Isometry3d。
• 变 （4× 4）：Eigen::A ffi ne3d。
• 影变 （4× 4）：Eigen::Projective3d。
 我 译此程序的问题交给读 。 这个程序中， 示了
用Eigen中的旋转矩阵、旋转向量（AngleAxis）、 角和
。我 用这几种旋转方式去旋转一个向量 ，发现 果是一样的（不 v
一样 真是见鬼了）。同时，也 示了 程序中转 这几种表 方
式 。 想 进 一 步 了 解 Eigen 的 几 的 读 可 以 参
（ http://eigen.tuxfamily.org/dox/group__TutorialGeometry.html ）
。

3.7　可视化演示
最 后 ， 我 为 读 了 一 个 程 序 ， 位 于
slambook/ch3/visualizeGeometry中。 以可视化的形式 示了各种表
方式的异同（见图3-4）。读 可以用鼠标操 一下，看看 是
变化的。

图3-4　旋转矩阵、 角、 的可视化程序。

这个 程序中，我 坐标原 了一个彩 立方 。用鼠标

可以平移/旋转相机。 可以 时 看 相机 态的变化。我 显示了变
矩阵 R,t
、 角和 的3种 态， 可以 际 验一下这几个量
是 变化的。 根 我 的 验，除了 角之 ， 应该看不出
直观的含义。
关于该程序的 代码，这里 不 解 了， 果 感兴趣，可以
行查看。该程序的 译说明请参 其中的Readme.txt。
得一 的是， 际中，我 会至 义两个坐标 ： 世界坐标系
和 相机坐标系 。 该 义下，设某个 世界坐标 中的坐标为 pw ，
相机坐标 下为 pc ， 么：

这里 Tcw 表示世界坐标 相机坐标 的变 。 可以用反过来

的 Twc ：

原则上， T 和 T 可以用来表示相机的位 ，事 上 也只
T T
wc cw

差一个 已。 当中 用 更加常见， 更为直观。 果

pc 取成零向量，也
cw wc

上面两式的 是相机坐标 中的原 ， 么，此时的

pw 是相机原 世界坐标 下的坐标：

我 发现这正是 T 的平移 分。 此，可以从 T 中直 看 相

机在何处 ，这也是我 说T
wc wc

更为直观的原 。 此， 可视化程序
里，我 显示了T 不是T
wc

wc cw 。

习题
1.验证旋转矩阵是正交矩阵。
2.*寻找 德里格斯公式的推导过程并加以理解。
3.验证 旋转某个 后， 果是一个虚 （ 为零），
所以仍 应 一个三 空 （式3.34）。
4.画表总 旋转矩阵、 角、 角、 的转 关 。
 5. 设有一个 的Eigen矩阵，想 的左上角3 × 3的 取出来，
后赋 I
为 3 × 3 。请 程 现。

6.*一 性方程Ax =b 有哪几种 ？ Eigen中 现吗？

7.设有 萝卜一号和 萝卜二号位于世界坐标 中。 萝卜一号的
位 为q =[0. 35, 0. 2, 0. 3, 0. 1],t =[0. 3, 0. 1, 0. 1] T

（q 的第一项为 。请 q 归一化后再进行计 ）。这里的 q 和 t 表

1 2

的是T ，也 是世界坐标 相机坐标 的变 关 。 萝卜二号

的位 为 q =[- 0. 5, 0. 4,- 0. 1, 0. 2],t =[- 0. 1, 0. 5, 0.
cw

2
T
3] 。现 ， 萝卜一号看 某个 的坐标 下坐标为p =[0. 5,
0, 0. 2] ， 该向量
T
萝卜二号坐标 下的坐标。请 程 现。

[1] 正交矩阵即 为 转 的矩阵。

[2] 方主页：http://eigen.tuxfamily.org/index.php?title=Main_Page。

[3] 感兴趣的读 请参见

https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula。

[4] https://en.wikipedia.org/wiki/Gimbal_lock。
 第4讲　李群与李代数
主要目标
1.理解李 与李代 的 念， ,
SO(3) SE(3)与 应李代 的表
示方式。
2.理解BCH近 的意义。
3. 会 李代 上的 动 。
4. 用Sophus 李代 进行运 。
上一讲，我 介 了三 世界中刚 运动的 方式，包 旋转矩
阵、旋转向量、 角、 等 干种方式。我 重 介 了旋转的
表示， 是 SLAM中，除了表示之 ，我 还要 进行估计和优
化。 为 SLAM中位 是未知的， 我 需要解 什么样的相机位姿最
符合当前观测数据 这样的问题。一种典 的方式是 构建成一个优化
问题， 解最优的R,t ， 得误差最 化。
前所言，旋转矩阵 是带有约束的（正交且行列式为1）。
为优化变量时，会引 额 的约束， 优化变得 难。 过李 —李
代 的转 关 ，我 希望 位 估计变成无约束的优化问题， 化
解方式。 虑 读 可 还 有李 李代 的 本知识，我 从最
本的知识开 讲起。
4.1　李群与李代数基础
上一讲，我 介 了旋转矩阵和变 矩阵的 义。当时，我 说三
旋转矩阵构成了 特殊正交群 SO(3)， 变 矩阵构成了 特殊欧氏群
SE(3)：

不过，当时我 并未详细解 群 的含义。细心的读 应该会 意

，旋转矩阵也 ，变 矩阵也 ， 它们对加法是不封闭的 。 句话
说， 于 意两个旋转矩阵 1 R ,R 2 ， 矩阵加 的 义，和不再是
一个旋转矩阵：

于变 矩阵亦是 此。我 发现，这两种矩阵并 有 义的

加 ，相 ， 只有一种 的运 ：乘 。SO(3)和SE(3)关于乘
是 闭的:

我 知 ，乘 应着旋转 变 的 合，两个旋转矩阵相乘表示
了两次旋转。 于这种只有一个运 的集合，我 称之为 。 群
4.1.1　群
（Group）是 一种集合 加上一种运算 的代 构。我 集合记
A，运 记 ，· 么 可以记 G =(A,· )。 要 这个运 足
以下几个条件：
1. 闭性：∀a ,a ∈A,a ·a ∈A .
1 2 1 2
 2. 合律： ∀ a ,a ,a ∈A, (a ·a )·a =a · (a
·a
1 2 3 1 2 3 1 2

3 ).

3.幺 ：∃a ∈A,s.t.∀a∈A,a ·a =a·a =a .

0 0 0

4. ：∀a∈A,∃a- ∈A,s.t.a·a- =a .
1 1
0

读 可以记 “ 幺 ”[1] 。我 可以验证，旋转矩阵集合和矩

阵乘 构成 ，同样变 矩阵和矩阵乘 也构成 （ 此才 称 为
旋转矩阵 和变 矩阵 ）。其 常见的 包 的加 (Z +)，去 ,
0后的有理 的乘 （幺 为1）(Q 0 )，等等。矩阵中常见的 \ ,·
有：
•一 性 n n×n 的可 矩阵，
GL( ) 矩阵乘 成 。
• 正交 SO(n )也 是所谓的旋转矩阵 ，其中SO(2)和SO(3)最
为常见。
• SE( n )也 是前面 的 n 变 ， SE(2)和
SE(3)。
构 证了 上的运 具有 的性质， 论则是研究 的
各种 构和性质的理论， 这里不 加介 。感兴趣的读 可以参
意一本近世代 材。
李群 是
具有连 （光 ）性质的 。 Z 样离 的
有连 性质，所以不是李 。 SO( )和SE( ) 空 上是连 n n
的。我 直观 想象一个刚 连 空 中运动，所以
是李 。由于SO(3)和SE(3) 于相机 态估计 其重要，所以我 主
要讨论这两个李 。 果读 李 的理论性质感兴趣，请参 献
[20]。
下面，我 先从 单的SO(3)开 讨论，我 发现 个李 有
应的李代 。我 首先引出SO(3)上面的李代 so(3)。

4.1.2　李代数的引出
虑 意旋转矩阵 R ，我 知 足：
 R
现 ，我 说， 是某个相机的旋转， 会随时 连 变化，即
为时 的 R t
： ( )。由于 仍是旋转矩阵，有

等式两 时 导，得 ：

理得：

R t R t
可以看出˙ ( ) ( )T 是一个 矩阵。 忆一下，我 反对称
式（3.3）介 叉积时，引 了 ∧ 符号， 一个向量变成了反 称矩
阵。同理， 于 意反 称矩阵，我 亦 找 一个与之 应的向量。
这个运 用符号 ∨ 表示：

于是，由于˙ R (t )R (t ) 是一个反
T
称矩阵，我 可以找 一个
三 向量ϕ (t )∈ R 与之 应。于是有：
3

等式两 右乘R (t )，由于R 为正交阵，有：

 可以看 ， 旋转矩阵 一次导 ，只需左乘一个ϕ∧ ( )矩阵即 t
t
可。为方 讨论，我 设 0 =0，并设此时旋转矩阵为 (0)= 。 导 R I
义，可以 R (t ) 0附近进行一阶 勒展开：

我 看 ϕ 反映了 的导 R 性质， 称 SO(3)原 附近的正切空

(Tangent Space)上。同时 t0 附近，设ϕ t
为常 ϕ ( 0 )=ϕ 0
。 么根 式（4.8），有：

上式是一个关于 R 的微分方程， 且知 R (0)=I ，解之，

得：

读 可以验证上式 微分方程和 成立。不过，由于 了一

的 设，所以 只 t
=0附近有 。我 看 ，旋转矩阵 与另一个 R
反 称矩阵ϕ 0 过 关 发生了 。也 是说，当我 知 某个时
R
刻的 时，存 一个向量ϕ ， 足这个矩阵 关 。 是矩阵的
是什么呢？这里我 有两个问题需要 ：
1. 果上式成立， 么给 某时刻的 ，我 得一个ϕ ， R
了 R 的导 关 。与 R
应的ϕ 有什么含义呢？后面会看
，ϕ 正是 应 SO(3)上的李代 so(3)；

2.其次，矩阵 exp(ϕ∧ ) 计 ？事 上，这正是李 与李代

的 / 映 。
下面一一加以介 。

4.1.3　李代数的定义
个李 有与之 应的李代 。李代 了李 的 性质。
用的李代 的 义 下：
 李代 由一个集合V，一个 F和一个二 运 [ ]组成。 , 果
足以下几条性质，则称(V F , , , [ ])为一个李代 ，记 g。
1. 闭性∀X,Y∈ V, [X,Y ]∈ V.
2.双 性∀X,Y,Z∈ V,a,b∈ F, 有：

3. ∀X∈ V, [X,X ]=0.

反性[2]
4. 雅 可 比 等 价 ∀X,Y,Z∈ V, [X, Y,Z
[ Z, [X,Y
]]+[ ]]+[Y, [Z,X
]]=0.
其中二 运 被称为 李括号
。从表面上来看，李代 所需要的性质
还是 的。相比于 中的 为 单的二 运 ，李 号表 了两个
的差异。 不要 合律， 要 和 己 李 号之后为零的性
质。 为 ，三 向量R3 上 义的叉积 是一种李 号， 此g=(R3 ×
, ,×
R )构成了一个李代 。读 可以 试 叉积的性质代 上面
条性质中。

4.1.4　李代数so(3)
之前 的ϕ ，事 上是一种李代 。SO(3) 应的李代 是 义
R3 上的向量，我 记 ϕ 。根 前面的推导， 个ϕ 可以生成一个反
称矩阵：

此 义下，两个向量ϕ 1 ,ϕ 2 的李 号为

读 可以验证该 义下的李 号 足上面的几条性质。由于ϕ 与反

称矩阵关 很 ， 不引起 义的情 下， 说so(3)的 是三
向量 三 反 称矩阵，不加区 ：

至此，我 已 楚了so(3)的 。 是一个由三 向量组成的集

合， 个向量 应 一个反 称矩阵，可以表 旋转矩阵的导 。 与
SO(3)的关 由 映 给 ：

映 会 稍后介 。由于已 介 了so(3)，我 顺带先来看

SE(3)上 应的李代 。

4.1.5　李代数se(3)
于SE(3)， 也有 应的李代 se(3)。为 省 幅，这里 不介
引出se(3)了。与so(3)相 ，se(3)位于R6 空 中：

我 个se(3) 记 ξ
， 是一个六 向量。前三 为平移
（ 含义与变 矩阵中的平移不同，分析见后），记 ；后三 为旋 ρ
转，记 ϕ ， 质上是so(3) [3]
。同时，我 展了 ∧ 符号的含
义。 se(3)中，同样 用 ∧ 符号， 一个六 向量转 成 矩阵，
这里不再表示反 称：

我 仍 用 ∧ 和 ∨ 符号来 代“从向量 矩阵”和“从矩阵 向

量”的关 ，以 和so(3)上的一致性。读 可以 单 se(3)理解
成“由一个平移加上一个so(3) 构成的向量”（尽 这里的 ρ 还不
直 是平移）。同样，李代 se(3)亦有 于so(3)的李 号：

读 可以验证 是否 足李代 的 义（留 习题）。至此我 已

见过两种重要的李代 so(3)和se(3)了。

4.2　指数与对数映射
4.2.1　SO(3)上的指数映射
现 来 虑第二个问题：exp(ϕ∧ )是 计 的？ 是一个矩阵的
， 李 和李代 中，称为 映 （Exponential Map）。同样，
我 会先讨论so(3)的 映 ，再讨论se(3)的情形。
意矩阵的 映 可以 成一个 勒展开， 是只有 的情
下才会有 果，其 果仍是一个矩阵。

同样 ， so(3)中 意 ϕ ，我 亦可 此方式 义 的 映
：

我 来 细推导一下这个 义。由于ϕ 是三 向量，我 可以 义

的 和 的方向，分 记 θ a
和 ，于是有ϕ = 。这里 是一 θa a
a
个 度为1的方向向量。首先， 于 ∧ ，有以下两条性质：

以及
 读 可以 行验证上 性质。 了 理 a∧ 高阶项的方 。
用这两个性质，我 可以 映 成：

最后得 一个 曾相识的式 ：

想前一讲 ， 和 德里格斯公式，即式（3.14） 出一 。
这表明，so(3) 际上 是由所谓的 旋转向量 组成的空 ， 映
即 德里格斯公式。 过 ，我 so(3)中 意一个向量 应 了一
个位于SO(3)中的旋转矩阵。反之， 果 义 映 ，也 SO(3)中
的 应 so(3)中：

不过我 常不 勒展开去计 映 。 第3讲中，我 已

介 过 根 旋转矩阵计 应的李代 ，即 用式（3.16），
用 的性质分 解转角和转 ， 用 种方式更加省事一些。
现 ，我 介 了 映 的计 方 。读 可 会问， 映
性质 呢？是否 于 意的 R
找 一个唯一的ϕ ？很 憾，
映 只是一个 。这意味着 个SO(3)中的 ， 可以找 一个
so(3) 与之 应； 是可 存 个so(3)中的 ， 应 同一个
SO(3)。至 于旋转角 θ，我 知 转360 ° 和 有转是一样的
—— 具有周期性。 是， 果我 旋转角度 之 ， 么±π
李 和李代 是一一 应的。
SO(3)与so(3)的 论 乎 我 的意料之中。 和我 前面讲的旋
转向量与旋转矩阵很相 ， 映 即是 德里格斯公式。旋转矩阵
的导 可以由旋转向量 ， 导着 旋转矩阵中进行微积分运
。

4.2.2　SE(3)上的指数映射
下面介 se(3)上的 映 。为了 省 幅，我 不再 so(3)
样详细推导 映 。se(3)上的 映 形式 下：

果 有 心，可以 着so(3)上的 推导， exp进行 勒展开

推导此式。从 果上看， ξ 的 映 左上角的 是我 R 知的SO(3)
中的 ，与se(3)当中的旋转 分ϕ 应。 右上角的 则可 理为 J
θa
（设ϕ = ）：

该式与 德里格斯公式有些相 ， 不 一样。我 看 ，平移

分 过 映 之后，发生了一次以 J 为 矩阵的 性变 。请读
J
重视这里的 ， 为后面还要用 。
同样 ，虽 我 也可以 比推得 映 ，不过根 变 矩阵 T
so(3)上的 应向量也有更省事的方式：从左上角的 计 旋转向量，R
 右上角的 t 足：

J
由于 可以由ϕ 得 ，所以这里的 ρ
亦可由此 性方程解得。现
，我 已 弄 了李 、李代 的 义与相互的转 关 ，总 图
4-1所示。 果读 有哪里不明白，可以 去几页看看公式推导。

, , ,
图4-1　SO(3) SE(3) so(3) se(3)的 应关 。

4.3　李代数求导与扰动模型
4.3.1　BCH公式与近似形式
用李代 的一 动机是进行优化， 优化过程中导 是非常必
要的 息（我 会 第6讲详细介 ）。下面来 虑一个问题。虽 我
已 楚了SO(3)和SE(3)上的李 与李代 关 ， 是，当 SO(3)中
成两个矩阵乘 时，李代 中so(3)上发生了什么 变呢？反过来说，当
so(3)上 两个李代 的加 时，SO(3)上是否 应着两个矩阵的乘积？
果成立，相当于：

果ϕ 1 ,ϕ 2 为标量， 显 该式成立； 此 我 计 的是 矩阵
的 ， 非标量的 。 言之，我 研究下式是否成立：

很 憾，该式 矩阵时并不成立。
两个李代 映 乘积的 形式，由Baker-Campbell-Hausdor
ff公式（BCH公式） [4] 给出。由于其 形式 杂，我 只给出其展
开式的前几项：

其中[]为李 号。BCH公式告诉我 ，当 理两个矩阵 之积时，

会产生一些由李 号组成的 项。 ， 虑SO(3)上的李代
当ϕ 1 ϕ 2 为 量时， 量二次以上的项 可以被忽
略 。此时，BCH 有 性近 表 ：

以第一个近 为 。该式告诉我 ，当 一个旋转矩阵 R （李代

R
2

为ϕ 2 ）左乘一个微 旋转矩阵 1 （李代 为ϕ 1 ）时，可以近 看

， 原有的李代 ϕ 2 上加上了一项 Jl (ϕ 2 ) - 1
ϕ 1 。同理，第二
个近 了右乘一个微 位移的情 。于是，李代 BCH近 下，分
成了左乘近 和右乘近 两种， 用时我 须 意 用的是左乘
还是右乘 。
 本书以左乘为 。左乘BCH近 雅可比Jl 事 上 是式（4.26）的
：

的 为

右乘雅可比仅需要 变量取负号即可：

这样，我 可以谈论李 乘 与李代 加 的关 了。为了方

读 理解，我 重新叙 一下BCH近 的意义。
R
某个旋转 ， 应的李代 为ϕ 。我 给 左乘一个微 旋
R
转，记 ∆ ， 应的李代 为∆ϕ 。 么， 李 上，得 的 果 是
R·R
∆ ， 李代 上，根 BCH近 ，为 合并起来，可以
单 成：

反之， 果我 李代 上进行加 ，让一个ϕ 加上∆ϕ ， 么可以

近 为李 上带左右雅可比的乘 ：

这 为之后李代 上 微积分 了理论 础。同样 ， 于

SE(3)，亦有 的BCH近 公式：
 这里 Jl 形式比 杂， 是一个6 × 6的矩阵，读 可以参 献
[6]中式(7.82)和(7.83)的 。由于我 计 中不用 该雅可比，
这里略去 的 际形式。

4.3.2　SO(3)李代数上的求导
下面来讨论一个带有李代 的 ， 关于该李代 导的问
题。该问题有很强的 际 景。 SLAM中，我 要估计一个相机的位
和 态，该位 是由SO(3)上的旋转矩阵 SE(3)上的变 矩阵 的。
不 设某个时刻 萝卜的位 为 。 观 T
了一个世界坐标位于 的 p
，产生了一个观 z
。 么，由坐标变 关 知：

，由于观 w
的存 ， 往往不可 z 确 足 z =Tp 的
关 。所以，我 常会计 理想的观 与 际 的误差：

N
设一 有 个这样的 标 和观 ，于是 有 N 个上式。 么，
萝卜的位 估计，相当于是寻找一个最优的 T ， 得 误差最
化：

解此问题，需要计 目标 J 关于变 矩阵T 的导 。我

具 的 留 后面再讲。这里重 要说的是，我们经常会构建与位姿
有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值
。 ，SO(3), SE(3)上并 有 义的加 ， 只是 。 果我
T 当成一个普 矩阵来 理优化， 必须 加以约束。 从李
代 角度来说，由于李代 由向量组成，具有 的加 运 。 此，
用李代 解 导问题的思 分为两种：
1.用李代 表示 态， 后根 李代 加 来 李代 导。
2. 李 左乘 右乘 微 动， 后 该 动 导，称为左 动
和右 动 。
 第一种方式 应 李代 的 导 ， 第二种则 应 动
。下面来讨论这两种思 的异同。

4.3.3　李代数求导
首先， 虑SO(3)上的情 。 设我 一个空 进行了旋转， p
得 了 Rp
。现 ，要计 旋转之后 的坐标相 于旋转的导 ，我 不
严谨 记为[5] ：

由于SO(3) 有加 ，所以该导 无 导 的 义进行计 。设

R 应的李代 为ϕ ，我 转 计 ：

导 的 义，有：

第2行的近 为BCH 性近 ，第3行为 勒展开 去高阶项后的近

，第4行至第5行 反 称符号看 叉积，交 之后变号。于是，我
推导出了旋转后的 相 于李代 的导 ：
 不过，由于这里仍 含有形式比 杂的 Jl ，我 不太希望计
。 下面要讲的 动 则 了更 单的导 计 方式。

4.3.4　扰动模型（左乘）
另一种 导方式，是 R
进行一次 动∆ 。这个 动可以乘 左 R
也可以乘 右 ，最后 果会有一 微 的差异，我 以左 动为
。设左 动∆ R应的李代 为 。 后， φ
导，即： φ

该式的 导比上面更为 单：

可见，相比于直 李代 导，省去了一个雅可比 Jl 的计 。这
得 动 更为 用。请读 务必理解这里的 导运 ，这 位 估
计当中具有重要的意义。

4.3.5　SE(3)上的李代数求导
最后，我 给出SE(3)上的 动 ， 直 李代 上的 导 不再
介 了。 设某空 p 过一次变 （ 应李代 为 T
），得 ξ Tp
[6]
T
。现 ，给 左乘一个 动∆ =exp( T δξ
∧ )，我 设 动项的李代
δξ δρ,δ
为 =[ ϕ ]T ， 么：
 我 最后的 果 义成一个 符 ⊙ [7]
， 一个齐次坐标的空
变 ×
成一个4 6的矩阵。
至此，我 已 介 了李 李代 上的微分运 。之后的章 中，
我 应用这些知识去解 际问题。关于李 李代 的某些重要
性质，我 为习题留给读 。

4.4　实践：Sophus
我 已 介 了李代 的 知识，现 是 过 练巩 一下
所 知识的机会了。我 来讨论 程序中操 李代 。 第3讲中，
我 看 Eigen 了几 ， 有 李代 的 。一个 的
李代 库是Strasdat 护的Sophus库[8] 。Sophus库 本章主要讨论的
,
SO(3)和SE(3)，此 还含有二 运动SO(2) SE(2)以及相 变 Sim(3)
的 。 是直 Eigen 础上开发的，我 不需要安装额 的 赖
库。读 可以直 从GitHub上获取Sophus[9] ， ， 本书的代码目录
slambook/3rdparty下也 了Sophus 代码。由于历史原 ，Sophus早
期版本只 了双 度的李 /李代 。后 版本 成了 板 。
板 的Sophus中可以 用不同 度的李 /李代 ， 同时 加了 用难
度。本书 用 非模板类 的Sophus库。 果读 用GitHub上的
Sophus，请确 用的是 非模板类 的版本。 可以 以下命令获得非
板 的Sophus：

本书的3rdparty中 的Sophus也是非 板版本。Sophus本 亦是

一个cmake工程。想必 已 了解 译cmake工程了，这里不再赘
。Sophus库只需 译即可，无须安装。
下面来 示一下Sophus库中的SO(3)和SE(3)运 ：
slambook/ch4/useSophus/useSophus.cpp
 该 示程序分为两 分。前半 分介 SO(3)上的操 ，后半 分则
为SE(3)。我 示了 ,
构造SO(3) SE(3) 象， 进行 、
映 ，以及当知 更新量后， 李 进行更新。 果读 切
理解了本讲 ， 么这个程序 来说应该 有什么难度。为了
译 ，请 CMakeLists.txt里 加以下几行：
slambook/ch4/useSophus/CMakeLists.txt

fi nd_package命令是cmake 的寻找某个库的 件与库 件的

令。 果cmake 找 ， 会 件和库 件所 的目录的变
量 。 Sophus 这 个 中 ， 是 Sophus_INCLUDE_DIRS 和
Sophus_LIBRARIES这两个变量。根 ，我 Sophus库引
己的cmake工程了。请读 行查看此程序的 出 息， 与我 之前的
推导是一致的。

4.5　*相似变换群与李代数
最后，我 要 一下 单目视觉中 用的相 变 Sim(3)，以及
应的李代 sim(3)。 果 只 双目SLAM RGB-D SLAM感兴趣，可以
过本 。
我 已 介 过单目的 度不确 性。 果 单目SLAM中 用SE(3)
表示位 ， 么由于 度不确 性与 度 移， 个SLAM过程中的 度
会发生变化，这 SE(3)中未 现出来。 此， 单目情 下我 一
会显式 度 表 出来。用 语言来说， 于位于空 的 p
， 相机坐标 下要 过一个 ， 非 相似变换
变 ：
 相 变 中，我 s
度 表 了出来。 同时 用 的3个坐 p
标之上， p
进行了一次 。与SO(3)、SE(3)相 ，相 变 亦 矩
阵乘 构成 ，称为相 变 Sim(3)：

同样 ，Sim(3)也有 应的李代 、 映 、 映 等。李代

sim(3) 是一个7 向量 ζ 。 的前6 与se(3)相同，最后 了一
σ
项 。

比se(3) 了一项 σ 。关 Sim(3)和sim(3)的仍是 映 和

映 。 映 为

其中Js 形式为

过 映 ，我 找 李代 与李 的关 。 于李代
ζ ， 与李 的 应关 为
 旋转 分和SO(3)是一致的。平移 分， se(3)中需要乘一个雅可
J
比 ， 相 变 的雅可比更 杂一些。 于 度 ，可以看 李
s
中的 即为李代 中 的 。σ
Sim(3)的BCH近 与SE(3)是 的。我 可以讨论一个 过相 p
变 Sp S
后，相 于 的导 。同样 ，存 微分 和 动 两种
方式， 动 为 单。我 省略推导过程，直 给出 动 的
果。设给予 Sp
左 一个 动exp( ∧ )，并 ζ
于 动的导 。 Sp
为 Sp
是4 的齐次坐标， ζ
是7 向量，该导 应该是4 7的雅可 ×
比。为了方 起见，记 Sp
的前3 组成向量 ， 么： q

关于Sim(3)，我 介 这里。更详细的关于Sim(3)的资料，建
议读 参见 献[21]。

4.6　小结
本 讲 引 了 李 SO(3) 和 SE(3) ， 以 及 应 的 李 代 so(3) 和
se(3)。我 介 了位 上面的表 和转 ， 后 过BCH的 性
近 ，我 可以 位 进行 导和 动了。这给之后讲解位 的优化
下了理论 础， 为我 需要 常 某一个位 的估计 进行调 ，
应的误差 。只有 弄 楚 位 进行调 和更新之后，
我 才 下一步的 。
可 本讲的 比 理论化，毕竟 不 计 机视觉 样 常有
看的图片可以展示。相比于讲解李 李代 的 科书，由于我
只关心 用的 ，所以讲的 非常 ，速度相 快了一些。请读
务必理解本章 ， 是解 后 许 问题的 础， 是位 估计
分。
习题
 1.验证SO(3)、SE(3)和Sim(3)关于乘 成 。

2.验证(R3 , R,× )构成李代 。

3.验证so(3)和se(3) 足李代 要 的性质。
4.验证性质（4.20）和（4.21）。
5.证明：

6.证明：

该式称为SO(3)上的 伴随 性质。同样 ， SE(3)上亦有 随性质：

其中：

7. 左 动的推导，推导SO(3)和SE(3) 右 动下的导 。
8. cmake的 fi nd_package 令是 运 的。 有哪些可 的
参 ？为了让cmake找 某个库，需要哪些先 条件？

[1] 谐音“ 咬 ”。

[2] 反性是 己与 己的运 为零。

[3] 请 意有些 方 旋转 前面，平移 后面，也是可行的。

[4] 参见https://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula。

[5] 请 意这里并不 矩阵微分来 义导 ，这只是一个记号。同时， 后 极

运 中，严谨的 需要 分 进行转 ，才 得 现 的 果。这里为方 书 省
去。

p
[6] 请 意为了 乘 成立， 必须 用齐次坐标。
[7] 我会读 “咚”， 一个石 井里的 音。

[8] 最早 出李代 的是Sophus Lie，这个库 以 的名字命名了。

[9] https://github.com/strasdat/Sophus。
 第5讲　相机与图像
主要目标
1.理解 相机的 、 参与径向畸变参 。
2.理解一个空 是 影 相机成 平面的。
3. OpenCV的图 存 与表 方式。
4. 会 本的 标 方 。
前面两讲中，我 介 了“机器人 表示 位 ”的问题，
分 解 了 SLAM 典 中变量的含义和运动方程 分。本讲 讨
论“机器人 观 世界”，也 是观 方程 分。 以相机为
主的视觉SLAM中，观 主要是 的过程。 相机成像
我 现 生 中 看 量的 片。 计 机中，一张 片由很
个 组成， 个 记录了 彩 亮度的 息。三 世界中的一个
反 发出的光 ，穿过相机光心后， 影 相机的成 平面上。
相机的感光器件 光 后，产生 量 ， 得 了 ，形成了我
见 的 片。这个过程 否用 原理来 呢？本讲 首先讨论相
机 ，说明 影关 具 ，相机的 参是什么。同时， 单
介 双目成 与RGB-D相机的原理。 后，介 二 片 的 本操
。最后，根 参 的含义， 示一个 云 的 验。
5.1　相机模型
相机 三 世界中的坐标 （单位为 ）映 二 图 平面（单
位为 ）的过程 用一个几 进行 。这个 有很 种，
其中最 单的称为 针孔模型。 是很常用 且有 的 ，
了一束光 过 之后， 面 影成 的关 。 本书中我
用一个 单的 相机 来 这种映 关 进行建 。同时，由于
相机 上的透 的存 ， 得光 影 成 平面的过程中会产生 畸
变 。 此，我 用 和畸变两个 来 个 影过程。
本 先给出相机的 ，再 透 的畸变 进行讲解。这
两个 的三 影 相机 成 平面，构成相机的内
参数 。
5.1.1　针孔相机模型
中 理课 上，我 可 见过一个蜡 影 验： 一个暗
的前方 着一 的蜡 ，蜡 的光透过暗 上的一个 影
暗 的后方平面上，并 这个平面上形成一个 立的蜡 图 。 这个
过程中， 三 世界中的蜡 影 一个二 成 平面。
同理，我 可以用这个 单的 来解 相机的成 过程， 图5-1所
示。
 图5-1　 相机

现 来 这个 单的 进行几 建 。设 为相机坐 O-x-y-z

标 ，习惯上我 让 z
向相机前方， 向右， 向下。 为 x 机y O
的光心 ，也是 中的 。现 世界的空 ， 过 P O
影之后，落 理成 平面 ′ O -x -y
′ ′ 上，成 为 ′ 。设 的坐标 P P
为[X,Y,Z ] ，P′ 为[X′ ,Y′ ,Z′ ] ，并且设 理成
T T
平面 的
离为f （ ）。 么，根 三角形相 关 ，有：

其中负号表示成的 是 立的。为了 化 ，我 可以 成 平
面 称 相机前方，和三 空 一起 机坐标 的同一 ，
图5-2的中图所示。这样 可以 公式中的负号去 ， 式 更加 ：

图5-2　真 成 平面， 称成 平面，归一化成 平面的图示。

理得：

读 可 要问，为什么我 可以看 随意 成 平面 前方
呢？这只是我 理真 世界与相机 影的 ，并且， 相
机 出的图 并不是 ——相机 的软件会帮 转这张图 ，所
以 看 的一 是正着的 ，也 是 称的成 平面上的 。所以，尽
从 理原理来说， 成 应该是 ， 由于我 图 了预
理，所以理解成 称平面上的 并不会带来什么坏 。于是， 不引
起 义的情 下，我 也不加 制 称后一种情 为 。
式（5.3） 了 P
和 的 之 的空 关 。不过， 相机中，
我 最终获得的是一个个的 ，这需要 成 平面上 进行 样和
量化。为了 感器 感受 的光 转 成图 的过程，我 设
理成 平面上 着一个 平面 。我 平面得 了o-u-v
P
′ 的 像素坐标 u,v
：[ ]T 。

像素坐标系 常的 义方式是：原 o′ 位于图 的左上角，u

[1]

向右与 x 平行， v 向下与 y 平行。 坐标 与成 平面之

，相差了一个缩放 和一个原点的平移 。我 设 坐标 u 上
了α ， v 上 了β 。同时，原 平移了[cx ,cy ] 。 T

么，P′ 的坐标与 坐标[u,v ] 的关 为 T

代 式（5.3）并 αf 合并成fx ， βf 合并成fy ，得：

其中， f 的单位为 ， α,β 的单位为 / ，所以 fx ,fy 的单位

为 。 该式 成矩阵形式会更加 ，不过左 需要用 齐次坐
标：

我 的习惯 Z 左 ：
 该式中，我 中 的量组成的矩阵称为相机的 内参数矩阵
K
（CameraIntrinsics） 。 常认为，相机的 参 出厂之后是
的，不会 用过程中发生变化。有的相机生产厂商会告诉 相机的
参， 有时需要 己确 相机的 参，也 是所谓的 。 于标 标定
业已成 ，且 上 找 量的标 ，这里 不介 了。
有 参， 也有相 的 参。 虑 式（5.6）中我 用的是
P 相机坐标 下的坐标。由于相机 运动，所以 的相机坐标应该是 P
P
的世界坐标（记为 w ），根 相机的当前位 变 相机坐标 下的
果。相机的位 由 的旋转矩阵 R 和平移向量t 来 。 么有：

意后一个式 隐含了一次齐次坐标 非齐次坐标的转 （ 看

出来吗？）。 了 的世界坐标 P坐标的 影关 。其中，相
机的位 R,t
又称为相机的 外参数
（Camera Extrinsics）。相比于不
变的 参， 参会随着相机运动发生 变，同时也是SLAM中待估计的目
标，代表着机器人的 。
上式两 是齐次坐标。 为齐次坐标乘上非零常 后表 同样的
含义，所以可以 单 去 ： Z

这样等号意义 变了，成为 齐次坐标下相等的 念，相差了一

个非零常 。为了 麻 ，我 还是从 意义上来 义书 等号。
这里还是 一下隐含着的齐次 非齐次的变 。可以看 ，右 的
TPw 表示 一个世界坐标 下的齐次坐标变 相机坐标 下。为了
与K 相乘，需要取 的前三 组成向量—— 为TPw 最后一 为1。此
时， 于这个三 向量，我 还可以 齐次坐标的方式， 最后一
进行归一化处理 ，得 P 相机归一化平面 上的 影：

Pc 可以看成一个二 的齐次坐标，称为归一化坐标 。 位于
这时
相机前方z =1 的平面上。该平面称为 归一化平面 。由于 Pc 过 参
之后 得 了 坐标，所以我 可以 坐标[u,v ] 看成 归一化
T

平面上的 进行量化 量的 果。
至此， 相机的成 我 讲 楚了。

5.1.2　畸变
为了获得 的成 果，我 相机的前方加了透 。透 的加
成 过程中光 的 会产生新的影响：一是透 的形 光
的影响，二是 机械组装过程中，透 和成 平面不可 平
行，这也会 得光 穿过透 影 成 面时的位 发生变化。
由透
形 引起的畸变称为 。 径向畸变
中，一条直
影 平面上还是一条直 。可是， 际 的 片中， 机的
透 往往 得真 环 中的一条直 图片中变成了曲 [2] 。越靠近图
的 ，这种现象越明显。由于 际加工制 的透 往往是中心 称
的，这 得不规则的畸变 常径向 称。 主要分为两 ： 桶形畸
变 枕形畸变
和 ， 图5-3所示。
 图5-3　径向畸变的两种 。

桶形畸变是由于图 率随着与光 之 的 离 加 ，
枕形畸变则恰 相反。 这两种畸变中，穿过图 中心和光 有交 的
直 还 形 不变。
除了透 的形 会引 径向畸变 ， 相机的组装过程中由于不
透 和成 面严格平行也会引 切向畸变 ， 图5-4所示。

图5-4　切向畸变来 示意图。

为更 理解径向畸变和切向畸变，我 用更严格的 形式 两
进行 。我 知 ，平面上的 意一 p
可以用笛卡 坐标表示为
x,y
[ ]T ，也可以 成极坐标的形式[ r,θ
]T ，其中 表示 与 r p
坐标 原 之 的 离， θ 表示与 平 的夹角。径向畸变可看成坐标
着 度方向发生了变化 δr
，也 是其 离原 的 度发生了变
化。切向畸变可以看成坐标 着切 方向发生了变化，也 是 平夹
角发生了变化δθ 。
 于 径向畸变
，无论是桶形畸变还是枕形畸变，由于 是随着
与中心之 的 离 加 加， 此可以用一个 项式 来 畸变
前后的坐标变化：这 畸变可以用与 中心的 离有关的二次及高次
项式 进行 正。其中[ x,y
]T 是未 正的 的坐标，[ corrected x ,y
corrected ]T 是 正后的 的坐标。 意， 是归一化平面 上的 ，
不是 平面上的 。

式(5.11) 的 正 中， 于畸变 的图 中心区 ，畸

变 正主要是 1 起 用；k 于畸变 的 区 ，主要是 2 起 k
用。普 用这两个 很 正径向畸变。 畸变很 的
，比 鱼眼 ，可以加 k 3 畸变项 畸变进行 正。

另一方面， 于切向畸变 ，可以 用另 的两个参 p ,p

1 2 来进
行 正：

此 ， 合 式 (5.11) 和 式 (5.12) ， 于 相 机 坐 标 中 的 一 P
(X,Y,Z )，我 过5个畸变 找 这个 平面上的正确位
：
1. 三 空 影 归一化图 平面。设 的归一化坐标为[ x,y
]T 。
2. 归一化平面上的 进行径向畸变和切向畸变 正。

3. 正后的 过 参 矩阵 影 平面，得 该 图
上的正确位 。
 上面的 正畸变的过程中，我 用了5个畸变项。 际应用中，
可以 正 ，比 只 k ,p ,p
1 1 2 这3项等。

这一 中，我 相机的成 过程 用 进行了建 ，也

透 引起的径向畸变和切向畸变进行了 。 际的图 中，
出了很 其 的 ，比 相机的 和透视 等，同时
也存 很 其 的畸变。 虑 视觉SLAM中一 用普 的
，有 及径向畸变和切向畸变 已 足 ， 此，我 不再
其 进行 。
得一 的是，存 两种去畸变 理（Undistort， 称畸变校正）
。我 可以 先 张图 进行去畸变，得 去畸变后的图 ，
后讨论此图 上的 的空 位 。 ，也可以先 虑图 中的某个
， 后 去畸变方程，讨论其去畸变后的空 位 。二 是可行
的，不过前 视觉SLAM中 乎更加常见一些。所以，当一个图 去畸
变之后，我 可以直 用 建立 影关 ， 不用 虑畸变
了。 此， 后 的讨论中，我 可以直 设图 已 进行了去畸变
理。
最后，我 一下单目相机的成 过程：
1.首先，世界坐标 下有一个 P ，世界坐标为Pw 。
的

2.由于相机 运动， 的运动由 R,t 变 矩阵 T∈ SE(3) 。 P

的相机坐标为
3.这时的 仍有 X,Y,Z
三个量， 影 归一化平面 Z =1上，
得 P
的归一化相机坐标： c =[ / P X Z,Y /Z, 1] T [3]
。

4.最后， P 的归一化坐标 过 参后， 应 的 坐标： Puv

=KPc 。
上所 ，我 一 谈 了 种坐标：世界坐标、相机坐标、归一
化相机坐标和 坐标。请读 厘 的关 ， 反映了 个成 的
过程。
5.1.3　双目相机模型
相机 了单个相机的成 。 ，仅根 一个
，我 是无 确 这个空 的具 位 的。这是 为，从相机光心
归一化平面连 上的所有 ， 可以 影至该 P
上。只有当 的
度确 时（比 过双目 RGB-D相机），我 才 确切 知 的空
位 。 图5-5所示。

图5-5　 可 存 的位 。

量 离（ 度）的方式有很 种， 人眼 可以根 左右
眼看 的景 差异（ 称视差）来 断 与我 之 的 离。双目相
机的原理亦是 此： 过同步 集左右相机的图 ，计 图 视差，
来估计 一个 的 度。下面 单讲讲双目相机的成 原理（ 图5-6
所示）。
双目相机一 由左眼相机和右眼相机两个 平 的相机组成。当
也可以 成上下两个目[4] ，不过我 见 的主 双目 是 成左右形
式的。 左右双目相机中，我 可以 两个相机 看 相机。
是 平 的，意味着两个相机的光 中心 位于 x
上。两 之 的
离称为双目相机的 基线 （Baseline，记b），是双目相机的重要参
。
 图5-6　双目相机的成 。O L ,O R 为左右光 中心，方框为成 平面， 为 f
。 u L 和u R 为成 平面的坐标。请 意， 图中坐标 义， u R 应该是负
，所以图中标出的 离为 -u R 。
现 ， 虑一个空 P， 左眼相机和右眼相机各成一 ，记
PL ,PR。由于相机 的存 ，这两个成 位 是不同的。理想情
下，由于左右相机只 x 上有位移， 此 P 的 也只 x （ 应图
的u ）上有差异。记 的左 坐标为uL ，右 坐标为uR 。 么，其
几 关 图5-6右 所示。根 △PPL PR 和△POL OR 的相 关 ，有：

稍加 理，得：

d
这里， 为左右图的 坐标之差，称为 （Disparity）。根 视差
视差，我 可以估计一个 与相机之 的 离。视差与 离成反比：
视差越 ， 离越近[5] 。同时，由于视差最 为一个 ，于是双目的
度存 一个理论上的最 ，由 确 。我 看 ，当 fb 越 时，
双目 的最 离 会越远；反之， 双目器件则只 量很近
的 离。
虽 由视差计 度的公式很 ， 视差 d本 的计 却比
难。我 需要确切 知 左眼图 某个 出现 右眼图 的哪一个位
 （即 应关 ），这件事亦属于“人 觉得 易 计 机觉得
难”的 务。当我 想计 个 的 度时，其计 量与 度 成
为问题， 且只有 图 纹理变化丰 的 方才 计 视差。由于计
量的原 ，双目 度估计仍需要 用GPU FPGA来计 。这 第13讲中
。

5.1.4　RGB-D相机模型
相比于双目相机 过视差计 度的方式，RGB-D相机的 更
为“主动”一些， 主动 量 个 的 度。目前的RGB-D相机
原理可分为两 （见图5-7）：
1. 过 红外结构光 （Structured Light）来 量 离的。
有Kinect 1代、Project Tango 1代、Intel RealSense等。
2. 过 飞行时间法 （Time-of- fl ight，ToF）原理 量 离
的。 有Kinect 2代和一些现有的ToF 感器等。
 图5-7　RGB-D相机原理示意图

无论是哪种 ，RGB-D相机 需要向 目标发 一束光 （ 常

是 光）。 构光原理中，相机根 的 构光图案，计
与 之 的 离。 ToF原理中，相机向目标发 光， 后根
发 之 的光束飞行时 ，确 与 之 的 离。ToF原理
和 光 感器十分相 ，只不过 光是 过逐 扫 来获取 离， ToF
相机则可以获得 个图 的 度，这也正是RGB-D相机的 。所
以， 果 一个RGB-D相机 开， 常会发现除了普 的 之 ，
至 会有一个发 器和一个 器。
量 度之后，RGB-D相机 常 生产时的各相机 位 ，
己 成 度与彩 图 之 的 ， 出一一 应的彩 图和 度
图。我 可以 同一个图 位 ，读取 彩 息和 离 息，计
的3D相机坐标，生成 云（Point Cloud）。 RGB-D ，既可以
图 层面进行 理，亦可 云层面 理。本讲的第二个 验 示
RGB-D相机的 云构建过程。
 RGB-D相机 时 量 个 的 离。 是，由于这种发
- 的 量方式，其 用 比 受 。用 光进行 度 量的
RGB-D相机， 易受 日光 其 感器发 的 光干 ， 此不
用，同时 用 个时也会相互干 。 于透 材质的 ， 为
不 反 光，所以无 量这些 的位 。此 ，RGB-D相机 成
本、功 方面， 有一些劣势。

5.2　图像
相机加上 ， 三 世界中的 息转 成了一张由 组成的
片，随后存 计 机中， 为后 理的 来 。 中，图
可以用一个矩阵来 ； 计 机中， 占 一 连 的磁盘
存空 ，可以用二 组来表示。这样一来，程序 不必区 理
的是一个 矩阵，还是有 际意义的图 了。
本 ，我 介 计 机图 理的一些 本操 。 ， 过
OpenCV中图 的 理，理解计 机中 理图 的常见步骤，为后
章 下 础。
计算机中图像的表示
我 从最 单的图 —— 度图说起。 一张 度图中， 个
位 (x,y ) 应一个 度 I ，所以，一张 度为 w 、高度为 h 的 图
， 上可以记为一个矩阵：

，计 机并不 表 个 空 ，所以我 只 某个
图 进行量化。 ，常见的 度图中，用0～255的 （即一个
unsigned char，1个字 ）来表 图 的 度 。 么，一张 度为
640 、高度为480 分 率的 度图 可以表示为

为什么这里的二 ×
组是480 640呢？ 为 程序中，图 以二
组形式存 。 的第一个下标是 组的行， 第二个下标则是列。
 图 中， 组的行 应图 的高度， 列 应图 的 度。
下面 这幅图 的 。图 是由 组成的。当访问某一
个 时，需要 明 所 的坐标， 图5-8所示。

图5-8　图 坐标示意图。

图5-8左 显示了 坐标 的 义方式。 坐标 原 位于

图 的左上角， X
向右， Y
向下（也 是前面所说的 坐标）。 u,v
果 还有第三个 —— Z
， 么根 右 则， 应该是向前 Z
的。这种 义方式是与相机坐标 一致的。我 平时说的图 的 度
列 ， 应着 X； 图 的行 高度，则 应着 的 。 Y
根 这种 义方式， 果我 讨论一个位于 x,y 的 ， 么
程序中的访问方式应该是

应着 度 ( I x,y
)的读 。请 意这里的 和 的顺序。虽 x y
我 不厌其 讨论坐标 的问题， 是 这种下标顺序的 误，仍
会是新 调试过程中 常碰 的，且具有一 隐蔽性的 误之一。
果 程序时不慎调 了 的坐标， 译器无x,y 息，
所 看 的只是程序运行中的一个越界 误 已。
一个 的 度可以用8位 记录，也 是一个0～255的 。当我
要记录的 息更 时，一个字 恐怕 不 了。 ， RGB-D相机的
度图中，记录了各个 与相机之 的 离。这个 离 常是以
为单位， RGB-D相机的量程 常 十几 左右，超过了255。这时，人
会 用16位 （C++中的unsigned short）来记录 度图的 息，也
 是位于0～65536的 。 成 的话，最 可以表示65 ，足 RGB-D
相机 用了。
彩图 的表示则需要 （channel）的 念。 计 机中，我
用 、 和蓝 这三种颜 的组合来表 意一种 彩。于是 于
一个 ， 要记录其R、G、B三个 ， 一个 称为一个
。 ，最常见的彩 图 有三个 ， 个 由8位 表示。
这种规 下，一个 占 24位空 。
的 量、顺序 是可以 由 义的。 OpenCV的彩 图 中，
的默认顺序是B、G、R。也 是说，当我 得 一个24位的 时，
前8位表示蓝 ，中 8位为 ，最后8位为 。同理，亦可 用
R、G、B的顺序表示一个彩 图。 果还想表 图 的透明度， 用
R、G、B、A 个 。

5.3　实践：图像的存取与访问
下面 过一个 示程序来理解， OpenCV中图 是 存取， 我
又是 访问其中的 的。

5.3.1　安装OpenCV
OpenCV[6] 了 量的开 图 ，是计 机视觉中 用极广的
图 理 库。本书也 用OpenCV 本的图 理。 用之前，
建议读 从 代码安装 。 Ubuntu下，有 从源代码安装 只安装库文 和
件 两种方式可以 ：
1.从 代码安装，是 从OpenCV 站下 所有的OpenCV 代码，并
机器上 译安装，以 用。 是可以 的版本比 丰 ， 且
看 代码，不过需要 费一些 译时 。
2.只安装库 件，是 过Ubuntu来安装由Ubuntu社区人员已
译 的库 件，这样 无须重新 译一 。
由于我 用 新版本的OpenCV，所以必须从 代码来安装 。一
来，可以调 一些 译 项，匹 程环 （ ，需不需要GPU加速
等）；再 ， 代码安装可以 用一些额 的功 。OpenCV目前 护了
两个主要版本，分为OpenCV 2.4 列和OpenCV 3 列。本书 用OpenCV
3 列。
由于OpenCV工程比 ， 不 本书的3rdparty下了。请读 从
http://opencv.org/downloads.html下 ， OpenCV for Linux版本
即可。 会获得一个 opencv-3.1.0.zip这样的压 包。 解压
意目录下，我 发现OpenCV亦是一个cmake工程。
译之前，先来安装OpenCV的 赖项：

事 上，OpenCV的 赖项很 ， 某些 译项会影响 的 分功

（不过我 也不会用 所有功 ）。OpenCV会 cmake阶 检查 赖项
是否会安装，并调 己的功 。 果 的电 上有GPU并且安装了相关
赖项，OpenCV也会 GPU加速 开。不过 于本书，上面 些 赖项
足 了。
随后的 译安装和普 的cmake工程一样，请 make之后，调用sudo
make install OpenCV安装 的机器上（ 不是仅仅 译 ）。视机
器 ，这个 译过程 需要二十分 一个 时不等。 果 的CPU
比 强，可以 用“make-j4”这样的命令，调用 个 程进行 译（-j
后面的参 是 用 的 程 量 ） 。 安 装 后 ， OpenCV 默 认 存
/usr/local目录下。 可以去寻找OpenCV 件与库 件的安装位
，看看 哪里。另 ， 果之前已 安装了OpenCV 2 列，
么建议 OpenCV 3安装 的 方（想想这应该 操 ）。

5.3.2　操作OpenCV图像
下来 过一个 程 悉一下OpenCV 图 的操 。
slambook/ch5/imageBasics.cpp
 该 程中，我 示了 下几个操 ：图 读取、显示、
历、 制、赋 等。 分的 解已 代码里面。 译该程序时，
需要 CMakeLists.txt中 加OpenCV的 件， 后 程序 库
件上。同时，由于 用了C++11标 （ nullptr和chrono），还需要设
一下 译器：

关于代码，我 给出几 说明：

1.程序从argv[1]，也 是命令行的第一个参 中读取图 位 。我
为读 了一张图 （ubuntu.png，一张Ubuntu的 纸，希望 喜
欢） 试 用。 此， 译之后， 用 下命令调用此程序： 果
Kdevelop中调用此程序，请务必确 参 同时给 。这可以 启动项
中 。

2.程序的10～17行， 用cv::imread 读取图 ，并 图 和

本 息显示出来。
3. 32～52行， 历了图 中的所有 ，并计 了 个循环所用
的时 。请 意 的 历方式并不是唯一的， 且 程给出的方式也
不是最高 的。OpenCV 了 代器， 可以 过 代器 历图 的
。 ，cv::Mat::data 了 向图 开 的 ， 可以直
过该 行计 移量， 后得 的 际 存位 。 程所用
的方式是为了 于读 理解图 的 构。 笔 的机器上（虚 机），
历这张图 用时 约12.74ms。 可以 比一下 己机器上的速度。不
过，我 用的是cmake默认的debug 式， 果 用release 式会快很
。
4.OpenCV 了许 图 进行操 的 ，我 此不一一列
举，否则本书 会变成OpenCV操 册了。 程给出了 为常见的读
取、显示操 ，以及 制图 中可 陷 的 贝误区。 程过程
中，读 还会碰 图 的旋转、 等操 ，这时 应该 行查
应的 档，以了解 的原理与 用方式。
应该 出，OpenCV并不是唯一的图 库， 只是许 图 库里 用
广 的一个。不过， 图 库 图 的表 是 同 异的。我
希望读 了解了OpenCV 图 的表示后， 理解其 库中图 的表
，从 需要 格式时 己 理。
另 ，由于cv::Mat亦是矩阵 ，除了表示图 之 ，我 也可以用
来存 位 等矩阵 。只是一 认为，Eigen 于 的矩阵
用起来 率更高一些。

5.4　实践：拼接点云
最后，我 来练习一下相机 参的 用方 。本 程序 了5张
RGB-D图 ，并且知 个图 的 参和 参。根 RGB-D图 和相机
参，我 可以计 一个 相机坐标 下的位 。同时，根 相
机位 ，又 计 这些 世界坐标 下的位 。 果 所有 的
空 坐标 出来，相当于构建一张 于 图的东西。现 我 来
练习一下。
我 了5 图 ，位于slambook/ch5/joinMap中。 color/下有
1.png 5.png 5张RGB图， depth/下有5张 应的 度图。同时，
T
pose.txt 件给出了5张图 的相机位 （以 wc 形式）。位 记录的形
式是平移向量加旋转 ：
 其中 qw 是 的 。 ，第一 图的 参为：

下面我 一 程序， 成两件事：(1).根 参计 一 RGB-D图

应的 云；(2).根 各张图的相机位 （也 是 参）， 云加
起来，组成 图。

本书的 云库 用PCL（Point Cloud Library） [7] 。PCL的安装比

易 ， 行 以 下 命 令 即 可 [8] ： 安 装 成 后 ， PCL 的 件 安装
/usr/include/pcl-1.7下，库 件位于/usr/lib/下。

现 分的程序：
slambook/ch5/joinMap/joinMap.cpp
 一 说明：
1.14～37行，读取彩 和 度图 及位 息，并 位 从
与平移向量转 为变 矩阵。 意程序里 用了boost::format进行字
符串的格式化。
2.65～78行：计 位于( u,v )、
度为 的 d 相机坐标 下的
位 ，并根 参 变 世界坐标。我 知 p
，相机坐标 c
坐标( u,v,d )的关 为

反推 pc 的形式亦非常 单。设 pc =[x,y,z ]， 么：

3.为了 译此程序，我 需3个库：Eigen、OpenCV和PCL。 此主程

序的CMake-Lists.txt应该 下：
 最后，我 生成的 云以PCD格式存 map.pcd中。用PCL 的
可视化程序 开这个 件：

随后 可以看 合的 云 图了（见图5-9）。 可以 动鼠标查

看。
 图5-9　查看 合的 云 图。

这个 程中，我 用相机 参和 参来计 世界坐标

中的位 ，并 合并成一个 云。这是一个 合性的示 ，请读
细 会并 其 。
习题
1.*寻找一 相机（ 的 机 笔记本的 即可），标 的
参。 可 会用 标 板， 己 印一张标 用的棋盘格。
2.叙 相机 参的 理意义。 果一 相机的分 率变为原来的两
其 方不变， 的 参 变化？
3. 相机（鱼眼 景相机）的标 方 。 与普 的
有 不同
4.调研 快 相机（global shutter）和卷帘快 相机（rolling
shutter）的异同。 SLAM中有 优 ？
5.RGB-D相机是 标 的？以Kinect为 ，需要标 哪些参 ？
（参 https://github.com/code-iai/iai_kinect2。）
6.除了示 程序 示的 历图 的方式， 还 举出哪些 历图
的方 ？
 7.* 读OpenCV 方 程， 习 的 本用 。

[1] 图 坐标 ，见本讲第2 。

[2] 是的， 不再直了， 是变成弯的。 果往里弯，称为桶形 真；往 弯则是枕形

真。

[3] 意 可 Z
于1，说明该 位于归一化平面后面， 可 不会 相机平面上成
， 当中要检查一次。

[4] 样的话 观会有些 。

[5] 读 可以 己用眼睛 一下。

[6] 方主页：http://opencv.org。

[7] ：http://pointclouds.org/。

[8] Ubuntu 16.04下请 过apt-get安装，想想这该怎么 。

 第6讲　非线性优化
主要目标
1.理解最 二乘 的含义和 理方式。
2.理解高斯 顿 （Gauss-Newton）、列 伯格—马夸 方
（Levenburg-Marquadt）等下降 略。
3. 习Ceres库和g2o库的 本 用方 。
前面几讲，我 介 了 典SLAM 的运动方程和观 方程。现
我 已 知 ，方程中的位 可以由变 矩阵来 ， 后用李代
进行优化。观 方程由相机成 给出，其中 参是随相机 的，
参则是相机的位 。于是，我 已 弄 了 典SLAM 视觉情
下的具 表 。
，由于 的存 ，运动方程和观 方程的等式必 不是 确
成立的。尽 相机可以非常 符合 ， 憾的是，我 得
的 常是受各种未知 影响的。即 我 有高 度的相机，运动
方程和观 方程也只 近 成立。所以，与其 设 必须符合方程，
不 来讨论 有 的 中进行 确的 态估计。
现代视觉SLAM 不需要 么高成本的 感器，甚至也不
需要 么昂贵的 理器来计 这些 ，这 是 的功劳。由于
SLAM问题中，同一个 往往会被一 相机 不同的时 次观 ，同
一 相机 个时刻观 的 也不止一个。这些 交织 一起，
我 有了更 的约束，最终 从 中恢 出我 需要
的东西。本 介 过优化 理 ，并且由这些表层逐
图优化本质，给出图优化的解 步介 并且 训练
。
6.1　状态估计问题
6.1.1　最大后验与最大似然
着前面几讲的 ，我 顾一下第2讲讨论的 典SLAM 。
由一个 态方程和一个运动方程构成， 式(2.5)所示：

过第4讲的知识，我 了解 这里的 xk 乃是相机的位 。我 可以

用变 矩阵 李代 表示 。至于观 方程，第5讲已 说明，即
相机 。为了让读 有更 的印象，我 不 讨论一下其具
参 化形式。首先，位 变量 k 可以由 k x
exp( )表 ，二 是等价 T
的。由于运动方程 视觉SLAM中 有 性，我 暂且不讨论， 主要
讨论观 方程。 设 k x
标 j 进行了一次观 ， 应 图 上的 y
位 zk,j ， 么，观 方程可以表示成

根 上一讲的 ，读 应该知 ，这里 为相机 K s

参， 为
的 离。同时，这里的 k,j 和 j z
必须以齐次坐标来 y ，且中 有
一次齐次 非齐次的转 。 果 还不 悉这个过程，请 上一讲再
细 读。
现 ， 虑 受 影响后会发生什么 变。 运动和观 方程
中，我 通常 设两个 w ,v
项 k k,j 足零 的高斯分布：

这些 的影响下，我 希望 过带 的 z 和u 推断位 x
和 y
图 （以及 的 率分布），这构成了一个 态估计问题。由于
 SLAM过程中，这些 是随时 逐 来的，所以 历史上很 一
时 ，研究 用 器， 其是 展卡 曼 器（EKF） 解
。卡 曼 器关心当前时刻的 态估计 k ， x
之前的 态则不
虑；相 ，近年来普 用的非 性优化方 ， 用所有时刻 集
的 进行 态估计，被认为优于 的 器[13] ， 成为当前视觉
SLAM的主 方 。 此，本书重 介 以非 性优化为主的优化方 ，
卡 曼 器则留 第10讲再进行讨论。本讲 介 非 性优化的
本知识， 后 第10讲、第11讲中 进行更 的分析。
首先，从 率 角度来看一下我 正 讨论什么问题。 非 性优
化中， 所有待估计的变量 一个“ 态变量”中：

现 ，我 说， 机器人 态的估计， 是已知 u 和观

z 的条件下， 计 x
态 的条件 率分布：

x u z
于 ，这里 和 也是 所有 的 称。 ，当 有
量运动的 感器， 只有一张张的图 时，即只 虑观 方程带来的
时，相当于估计 ( P x|z )的条件 率分布。 果忽略图 时 上的
， 看 一 彼此 有关 的图片，该问题也称为Structure
from Motion（SfM），即 从许 图 中重建三 空 构[22] 。 这
种情 下，SLAM可以看 图 具有时 先后顺序，需要 时 解一个SfM
问题。为了估计 态变量的条件分布， 用贝叶斯 则，有：

贝叶斯 则左 常称为 后验概率 ，右 的P (z|x )称为似然 ，

P x
另一 分 ( )称为 先验 直接求后验分布是困难的，但是求一个状
。
态 最 优 估 计 ， 使 得 在 该 状 态 下 后 验 概 率 最 大 化 （ Maximize a
Posterior， MAP），则是可行的 ：
 请 意贝叶斯 则的分 x
分与待估计的 态 无关， 可以忽
略。贝叶斯 则告诉我 ， 解最 后验 率 相当于最大化似然和先验
的乘积 。进一步，我 当 也可以说， 不起，我不知 机器人位
什么 方，此时 先验
有了 x 最大似然估计
。 么，可以 解 的
（ ，
Maximize Likelihood Estimation ）
MLE ：

直观讲， 是 “ 现 的位 下，可 产生怎样的观

”。由于我 知 观 ，所以最 估计可以理解成：“ 在什
么样的状态下，最可能产生现在观测到的数据” 。这 是最 估
计的直观意义。

6.1.2　最小二乘的引出
么 最 估计呢？我 说， 高斯分布的 设下，最
有 单的形式。 顾观 ， 于某一次观 ：

由于我 设了 项 vk ～N (0,Qk,j )，所以观 的条件 率

为

是一个高斯分布。为了计 最 化的 xk ,yj ，我 往往
用 最小化负对数 的方式来 一个高斯分布的最 。
高斯分布 负 下有 的 形式。 虑 意高 高斯分布 x～
N (µ, Σ)， 的 率 度 展开形式为

其取负 ，则变为
 原分布 最 化相当于 负 最 化。 最 化上式的 x
x
时，第一项与 无关，可以略去。于是，只要最 化右 的二次 项，
得 了 态的最 估计。代 SLAM的观 ，相当于 ：

我 发现，该式等价于最 化 项（即误差）的平方（Σ 意
义下）。 此， 于所有的运动和 意的观 ，我 义 与估计
之 的误差：

并 该误差的平方和：这样 得 了一个总 意义下的最 二乘问

题（Least Square Problem）。我 明白 的最优解等价于 态的最
估计。直观讲，由于 的存 ，当我 估计的 与 图代
SLAM的运动、观 方程中时， 并不会 成立。这时怎么办呢？
我 态的估计 进行 微调 ， 得 的误差下降一些。当 这个下
降也有 度， 一 会 极小值
一个 。这 是一个典 非 性优化的
过程。
细观 式(6.12)，我 发现SLAM中的最 二乘问题具有一些
的 构：
•首先， 个问题的目标 由许 个误差的（加权的）平方和组
成。虽 总 的 态变量 很高， 个误差项 是 单的，仅与一
两个 态变量有关。 x ,x
，运动误差只与 k- 1 k 有关，观 误差只与
xk ,yj 有关。 个误差项 是一个 规 的约束，我 之后会谈论
 进行 性近 ，最后再 这个误差项的 雅可比矩阵
的雅可比矩阵中。由于这种 ，我 称 个误差项 应的优化变量为
参数块（Parameter Block 。 ）
• 误差由很 误差项之和组成的问题，其 量方程的 解会
具有一 的稀疏性（会 第10讲详细讲解）， 得 规 时亦可
解。
•其次， 果 用李代 表示，则该问题是 的最 二乘问 无约束
题。 果用旋转矩阵（变 矩阵） 位 ，则会引 旋转矩阵
的约束（旋转矩阵必须是正交矩阵且行列式为1）。额 的约束会 优化
变得更 难。这 现了李代 的优势。
•最后，我 用了平方形式（二 ）度量误差， 是直观的，相
当于 空 中 离的平方。 也存 着一些问题，并且不是唯一的度
量方式。我 亦可 用其 的 构建优化问题。
现 ，我 介 解这个最 二乘问题。本讲 介 非线性优
化的基本知识 ， ， 这样一个 用的无约束非 性最 二乘问
题， 讨 是 解的。 后 几讲，我 会 量 用本讲的 果，
详细讨论 SLAM前端、后端中的应用。

6.2　非线性最小二乘
我 先来 虑一个 单的最 二乘问题：

这里 变量 x∈
R f
n ， 是 意非 性 ，我 设 有 m ： f (x
)∈ R m 。下面讨论 解这样一个优化问题。
果 是个 f 形式上很 单的 ， 么问题也许可以用解析形
式来 。令目标 的导 为零， 后 解 x 的最优 ， 和 二
的极 一样：
 解此方程， 得 了导 为零 的极 。 可 是极 、极
鞍 的 ，只要逐个比 的 即可。 是，这个方程是
否 易 解呢？这取 于 导 的形式。 SLAM中，我 用李代 f
来表示机器人的旋转和位移。尽 我 李代 一讲讨论了 的导 形
式， 这不代表我 顺 解上式这样一个 杂的非 性方程。
于不方 直 解的最 二乘问题，我 可以用 迭代 的方式，从
一个 出发，不断 更新当前的优化变量， 目标 下降。具
步骤可列 下：
1.给 某个 x 0 。

2. 于第 k次 代，寻找一个 量∆ xk ， 得 ‖f (xk +∆xk )‖ 2

2

极 。
3. ∆ xk 足 ，则 止。

4.否则，令 xk +1 = xk +∆xk ， 第2步。

这让 解导 为零的问题变成了一个不断寻找梯度并下降的过
程。直 某个时刻 量非常 ，无 再 下降。此时 ，目
标 了一个极 ，我 也 成了寻找极 的过程。 这个过程
中，我 只要找 代 的梯度方向即可， 无须寻找 导 为零
的情 。
下来的问题是， 量∆ xk 确 ？ 际上，研究 已 费
了 量 力 量的 解方式。我 介 两 办 ，用不同的
来寻找这个 量。目前这两种方 视觉SLAM的优化问题上被广
用， 优化库 可以 用 。

6.2.1　一阶和二阶梯度法
解 量最直观的方式是 目标 x 附近进行 勒展开：
 J ‖f x ‖
这里 是 x
( ) 2 关于 的导 （雅可比矩阵）， 则是二 H
阶导 （ 〔Hessian〕矩阵）。我 可以 留 勒展开的一阶
二阶项， 应的 解方 则为一阶梯度 二阶梯度 。 果 留一阶梯
度， 么 量的解 为

的直观意义非常 单，只要我 着反向梯度方向前进即可。

常我 还会计 该方向上的一个步 λ ， 得最快的下降方式。这种方
被称为最速下降法。
另一方面， 果 留二阶梯度 息， 么 量方程为

右 x
等式关于∆ 的导 并令 为零， 得 了 量的解：

该方 又称为 顿 。我 看 ，一阶和二阶梯度 十分直观，

只要 代 附近进行 勒展开，并 更新量 最 化即可。
由于 勒展开之后 变成了 项式，所以 解 量时只需解 性方程
即可， 了直 导 为零这样的非 性方程的 难。不过，这两
种方 也存 的问题。最速下降 过于贪心， 易走出 齿
，反 加了 代次 。 顿 则需要计 目标 的 矩阵，这 H
问题规 时非常 难，我 常 向于 的计 。所以， H
下来我 详细 介 两 更加 用的方 ：高斯 顿 和列 伯格—马
夸 方 。

6.2.2　高斯牛顿法
高斯 顿 是最优化 中最 单的方 之一。 的思想是 f (x
)进行一阶的 勒展开（请 意不是目标 f x
( )2 ）：
 J x f x x
这里 ( )为 ( )关于 的导 ， 际上是一个 矩阵，也是 m×n
一个雅可比矩阵。根 前面的框架，当前的目标是寻找下降矢量∆ ， x
得‖f x x ‖
( +∆ ) 2 最 。为了 ∆ ，我 需要解一个 性的最x
二乘问题：

这个方程与之前有什么不一样呢？根 极 条件， 上 目标
∆x 导，并令导 为零。由于这里 虑的是∆ 的导 （ 不是 x x
），我 最后 得 一个 性的方程。为此，先展开目标 的平方
项：

x
上式关于∆ 的导 ，并令其为零：

可以得 下方程组：

意，我 要 解的变量是∆ ， 此这是一个 x 线性方程组

，我
称 为 增量方程 ，也可以称为 高斯牛顿方程
（ Gauss Newton
正规方程
equation） （Normal equation）。我 左 的
H
义为 ，右 g
义为 ， 么上式变为

这里 左 记 H 是有意义的。 比 顿 可见，高斯 顿 用 T J
J 为 顿 中二阶Hessian矩阵的近 ，从 省略了计 的过程。 H 求
解增量方程是整个优化问题的核心所在 。 果我 顺 解出该方
程， 么高斯 顿 的 步骤可以 成：
1.给 x 0 。
 2. 于第 k次 代， 出当前的雅可比矩阵 J (xk )和误差f (xk )。
3. 解 量方程： H ∆xk =g 。
4. xk 足
∆ ，则 止。否则，令 xk +1 xk +∆xk ，
= 第2步。

从 步骤中可以看 ， 量方程的 解占 着主要 位。原则

上， 要 我 所用的近 H 矩阵是可 的（ 且是正 的）， 际
中计 得 的 T J J
却只有半正 性。也 是说， 用高斯 顿
J J
时，可 出现 T 为 异矩阵 病态（illcondition）的情 ，此时
量的稳 性 差，导致 不 。更严重的是， 我 设 非 H
异也非病态， 果我 出来的步 ∆ 太 ，也会导致我 x 用的
近 (6.19)不 确，这样一来我 甚至无 证 的 代 ，哪
怕是让目标 变得更 是有可 的。
尽 高斯 顿 有这些 ， 得我 去 习， 为 非
性优化里，相当 的 可以归 为高斯 顿 的变种。这些
助了高斯 顿 的思想并且 过 己的 进修正其 。 一些
线搜索方法 (line search method)，这 进 是加 了一个标量 α
x
， 确 了∆ 后进一步找 得α ( + ‖f x α x ‖
∆ ) 2
最 ，
不是 高斯 顿 样 单 令 =1。α
列 伯格—马夸 方 一 程度上修正了这些问题，一 认为
比高斯 顿 更为 。尽 的 速度可 会比高斯 顿 更
慢，被称为 阻尼牛顿法 （Damped Newton Method）， 是 SLAM里面却
被 量应用。

6.2.3　列文伯格—马夸尔特方法
由于高斯 顿 中 用的近 二阶 勒展开只 展开 附近有
的近 果，所以我 很 想 应该给∆ 加一个 赖区 x
（Trust Region），不 让 太 得近 不 确。非 性优化中有
一 列这 方 ，这 方 也被称为 信赖区域方法
（ Trust Region
Method）。 赖区 里 ，我 认为近 是有 的；出了这个区 ，
近 可 会出问题。
 么 确 这个 赖区 的 呢？一个比 的方 是根 我
的近 际 之 的差异来确 ： 果差异 ，我 让
尽可 ； 果差异 ，我 这个近 。 此， 虑 用

来
断 勒近 是否 。 ρ
的分 是 际 下降的 ，分 是
近 下降的 。 果 ρ 近于1，则近 是 的。 果 太 ，说 ρ
明 际 的 远 于近 的 ，则认为近 比 差，需要 近
。反之， 果 比 ρ ，则说明 际下降的比预计的更 ，我
可以 近 。
于是，我 构建一个 版的非 性优化框架，该框架会比高斯
顿 有更 的 果：
1.给 x 0 ，以及 优化半径 µ。
2. 于第 k次 代， 解：

这里 µ 是 赖区 的半径，D 后 说明。
3.计 ρ 。
4. 则µ =2µ 。
5. 则µ =0. 5µ 。
6. 果ρ 于某 ，则认为近 可行。令xk +1 =xk +∆xk 。
7. 断 是否 。 不 则 第2步，否则 束。
这里近 的 和 是 验 ，可以替 成 的
。 式(6.24)中，我 量 于一个半径为 的球中，认为只 µ
这个球 才是有 的。带上 之后，这个球可以看成一个椭球。 列 D
伯格 出的优化方 中， 取成单位阵 ，相当于直 D ∆ 约束 一 I x
个球中。随后，马夸 出 取成非负 D
角阵—— 际中 常用 T J
J的 角 平方根， 得 梯度 的 度上约束 更 一些。
 不论 ， 列 伯格—马夸 优化中，我 需要解式(6.24)
样一个 问题来获得梯度。这个 问题是带不等式约束的优化问题，
我 用 格朗日乘 转化为一个无约束优化问题：

这里 λ
为 格朗日乘 。 于高斯 顿 中的 ， 展开
后，我 发现该问题的核心仍是计 量的 性方程：

可以看 ， 量方程相比于高斯 顿 ， 了一项 λDT D 。 果

D I
虑 的 化形式，即 = ， 么相当于 解：

我 看 ，当参 比 λ H
时， 占主要 位，这说明二次近
该 是比 的，列 伯格—马夸 方 更 近于高斯 顿
。另一方面，当 比 λ 时， λI
占 主要 位，列 伯格—马夸
方 更 近于一阶梯度下降 （即最速下降），这说明附近的二次近
不 。列 伯格—马夸 方 的 解方式，可 一 程度上
性方程组的 矩阵的非 异和病态问题， 更稳 、更 确的
x
量∆ 。
际中，还存 许 其 的方式来 解 的 量， Dog-Leg
等方 。我 这里所介 的，只是最常见 且最 本的方式，也是视
觉SLAM中用得最 的方式。总 言之，非 性优化问题的框架，分为
Line Search和Trust Region两 。Line Search先 方向， 后
该 方 向 寻 找 步 ， 以 最 速 下 降 和 高 斯 顿 为 代 表 。 Trust
Region则先 区 ，再 虑找该区 的最优 。此 方 以列
伯格—马夸 方 为代表。 际问题中，我 常 高斯 顿
列 伯格—马夸 方 为梯度下降 略。

6.2.4　小结
 由于不希望这本书变成一本让人觉得 疼的 科书，所以这里
只 列了最常见的两种非 性优化方案——高斯 顿 和列 伯格—马
夸 方 。我 开了许 性质上的讨论。 果读 优化感兴
趣，可以进一步 读专 介 优化的书 （这是一个很 的课
题）， [23]。以高斯 顿 和列 伯格—马夸 方 为代表的优
化方 ， 很 开 的优化库中 已 现并 给用 ，我 会 下
进行 验。最优化是 理许 际问题的 本 工具，不光 视觉
SLAM中起着核心 用， 于 度 习等其 领 ， 也是 解问题
的核心方 之一。我 希望读 根 力，去了解更 的最优
化 。
也许 发现了，无论是高斯 顿 还是列 伯格—马夸 方 ，
最优化计 时， 需要 变量的 。 也许会问 ，这个
否随意设 ？当 不是。 际上非 性优化的所有 代 解方
案， 需要用 来 一个 的 。由于目标 太 杂，导致
解空 上的变化难以琢磨， 问题 不同的 往往会导致不
同的计 果。这种情 是非 性优化的 病： 易陷
极 。 此，无论是哪 科 问题，我 应该有科
， 视觉SLAM问题中，我 会用ICP、PnP之 的 优化
。总之，一个 的 最优化问题非常重要！
也许读 还会 上面 的最优化产生疑问： 解 性 量方
程组呢？我 只讲 了 量方程是一个 性方程， 是直 矩阵
进行 岂不是要进行 量的计 ？当 不是。 视觉SLAM 里，
常 x
∆ 的 度 几百 上千， 果 是要 规 的视觉三
重建， 会 常发现这个 度可以 易 几十万甚至更高的级 。要
么 个矩阵进行 是 理器无 负 的， 此存 着许
性方程组的 解方 。 不同的领 有不同的 解方式，
几乎 有一种方式是直 矩阵的 ，我 会 用矩阵分解的方
来解 性方程， QR、Cholesky等分解方 。这些方 常 矩阵论
等 科书中可以找 ，我 不 加介 。
幸运的是，视觉SLAM里这个矩阵往往有 的稀疏形式，这为 时
解优化问题 了可 性。我 第10讲中详细介 的原理。
用稀疏形式的消 、分解，最后再进行 解 量，会让 解的 率
高。 很 开 的优化库上， 度为一万 的变量 一 的PC上 可
以 几秒甚至更短的时 被 解出来，其原 也是用了更加高级的
 工具。视觉SLAM 现 时 现，也 亏了 矩阵是稀疏
的， 果矩阵是稠 的，恐怕优化这 视觉SLAM 不会被 界广
了[24,25,26] 。

6.3　实践：Ceres
我 前面说了很 理论，现 来 一下前面 的优化 。
本讲的 分中，我 主要向 介 两个C++的优化库：来 谷 的
Ceres库[27] 以及 于图优化的g2o库 [28] 。由于g2o的 用还需要介 一
图优化的相关知识，所以我 先来介 Ceres， 后介 一些图优化理
论，最后来讲g2o。由于优化 之后的“视觉里程计”和“后端”中
会出现，所以请读 务必 优化 的意义，理解程序的 。

6.3.1　Ceres简介
Ceres库面向 用的最 二乘问题的 解， 为用 ，我 需要 的
是 义优化问题， 后设 一些 项， 进Ceres 解即可。Ceres
解的最 二乘问题最一 的形式 下（带 界的核 最 二乘）：

可以看 ，目标 由许 平方项 过一个 核函数 ρ (· )之后

和组成[1] 。 最 单的情 下，取 为恒等 ρ ，则目标 即为许
项的平方和。 这个问题中，优化变量为 1 x ,···,xn ，fi 称为代
价函数 （Cost function）， SLAM中亦可理解为误差项。lj 和uj 为第
j 个优化变量的上 和下 。 最 单的情 下，取 lj =-∞,uj =∞
（不 制优化变量的 界），并且取ρ 为恒等 时， 得 了无约束
的最 二乘问题，和我 先前说的是一致的。
Ceres中，我 义优化变量 x 和 个代价 fi ，再 调用
Ceres进行 解。我 可以 用高斯 顿 列 伯格—马夸
方 进行梯度下降，并设 梯度下降的条件，Ceres会 优化之后 最优
估计 。下面，我 过一个曲 合的 验来 际操 一下
Ceres，理解优化的过程。

6.3.2　安装Ceres
为 了 用 Ceres ， 首 先 需 要 进 行 译 安 装 ！ 建 议 去 GitHub 上 下
Ceres：https://github.com/ceres-solver/ceres-solver。本书资 的
3rdparty下也附带了Ceres库。
与之前碰 的库一样，Ceres是一个cmake工程。先来安装 的 赖
项， Ubuntu中可以用apt-get安装，主要是谷 己 用的一些日志和
试工具：

后，进 Ceres库目录下， 用cmake 译并安装 。这个过程我

已 过 很 了 ， 此 不 再 赘 。 安 装 成 后 ，
/usr/local/include/ceres 下 找 Ceres 的 件 ， 并
/usr/local/lib/下找 名为libceres.a的库 件。有了这些 件，
可以 用Ceres进行优化计 了。

6.3.3　使用Ceres拟合曲线
我 的 示 验包 用Ceres和 下来的g2o进行曲 合。 设
有一条 足以下方程的曲 ：

其中 a,b,c
为曲 的参 ， 为高斯 。我 w
意 了这样一
个非 性 ，以 问题不至于太 单。现 ， 设我 有 个关于 N x,y
的观 ，想根 这些 出曲 的参 。 么，可以 解下
面的最 二乘问题以估计曲 参 ：
 请 意， 这个问题中，待估计的变量是 a,b,c ， x
不是 。我
一个程序，先根 生成 x,y
的真 ， 后 真 中 加高斯分布
的 。随后， 用Ceres从带 的 合参 。
slambook/ch6/ceres_curve_ fi tting/main.cpp
 程序中需要说明的 方 已加 。可以看 ，我 用OpenCV的
生成器生成了100个带高斯 的 ，随后 用Ceres进行 合。
Ceres的用 下：
1. 义Cost Function 。方 是书 一个 ，并 中 义带
板参 的()运 符，这样该 成为了一个 （Functor，C++术
语）。这种 义方式 得Ceres可以 调用 一样， 该 的某个 象
（比 a）调用a<double>()方 ——这 象具有 样的行为。
2.调用AddResidualBlock 误差项 加 目标 中。由于优化需
要梯度，我 有 干种 ：（1） 用Ceres的 动 导（Auto Di
ff）；（2） 用 导（Numeric Di ff）；（3） 行推导解析的导
形式， 给Ceres。其中 动 导 码上是最方 的，于是我
用 动 导。
3. 动 导需要 误差项和优化变量的 度。这里的误差是标
量， 度为1；优化的是 a,b,c
三个量， 度为3。于是， 动 导
的 板参 中设 变量 度为1、3。
4.设 问题后，调用solve 进行 解。 可以 options里
（非常详细的）优化 项。 ，可以 用Line Search还是
Trust Region、 代次 、步 ，等等。读 可以查看Options的 义，
看看有哪些优化方 可 ，当 默认的 已 可用于很广 的问题
了。
最后，我 来看看 验 果。调用build/curve_ fi tting查看优化
果：
 从Ceres给出的优化过程可以看 ， 误差 约从18248下降 了
50.9，并且梯度也是越来越 。 代22次后 ，最后的估计
为

我 设 的真 为
 相差不 。
为了更直观 显示 ，可以 画出来， 图6-1所示。其中显示
了带 的 、真 和估计 ，可以看 估计 和真
非常 近，几乎重合。我 同时记录了Ceres的运行时 ， 这样一个
100个 的优化问题，计 时 约1.3ms（虚 机上）。
希望读 过这个 单的 Ceres的 用方 有一个 致了解。
的优 是 了 动 导工具， 得不必去计 很麻 的雅可比矩
阵。Ceres的 动 导是 过 板 现的， 译时期 可以 成 动
导工 ，不过仍 是 导 。本书 分时 仍 会介 雅可比矩
阵的计 ， 为 样 理解问题更有帮助， 且 优化中更 出现问
题。此 ，Ceres的优化过程 也很丰 ， 其 合很广 的最 二乘
优化问题，包 SLAM中的各种问题。

图6-1　 用Ceres进行曲 合。真 和估计 非常 近。

6.4　实践：g2o
 本讲的第2个 分 介 另一个（主要 SLAM领 ）广为 用的
优化库：g2o（General Graphic Optimization，G2 O）。 是一个 于
图优化 的库。图优化是一种 非 性优化与图论 合起来的理论， 此
用 之前，我 一 幅介 一下图优化理论。

6.4.1　图优化理论简介
我 已 介 了非 性最 二乘的 解方式。 是由很 个误差
项之和组成的。 ，仅有一组优化变量和许 个误差项，我 并不
楚 之 的 关联 x
。比 ，某个优化变量 j 存 于 个误差项中呢？
我 证 的优化是有意义的吗？进一步，我 希望 直观 看
该优化问题长什么样 。于是， 涉 了图优化。
图优化，是 优化问题表现成图（ Graph） 的一种方式。这里的图
是图论意义上的图。一个图由 干个顶点（ Vertex） ，以及连 着这
些顶 的边（ Edge） 组成。进 ，用顶点 表示优化变量 ，用边 表示
误差项 。于是， 意一个上 形式的非 性最 二乘问题，我 可以
构建与之 应的一个图 。
图6-2是一个 单的图优化 。我 用三角形表示相机位 ，
用 形表示 标 ， 构成了图优化的顶 ；同时， 表示相机的
运动 ，虚 表示观 ， 构成了图优化的 。此时，虽
个问题的 形式仍是式(6.12) 样， 现 我 可以直观 看 问题
的结构 了。 果希望，也可以 去掉孤立顶点 优先优化边数较多
（或按图论的术语，度数较大）的顶点 这样的 进。 是最 本的图优
化是用图 来表 一个非 性最 二乘的优化问题。 我 可以 用
图 的某些性质 更 的优化。
 图6-2　图优化的 。

g2o为SLAM 了图优化所需的 。下面 示一下g2o的 用方

。

6.4.2　g2o的编译与安装
用一个库之前，我 需要 进行 译和安装。读 应该已
验过很 次这种过程了， 本 同 异。关于g2o，读 可以从
GitHub下 ：https://github.com/RainerKuemmerle/g2o， 从本书
的第三方代码库中获得。
解压代码包后， 会看 g2o库的所有 码， 也是一个cmake工
程。我 先来安装 的 赖项（ 分 赖项与Ceres重合）：

后， cmake的方式 g2o进行 译安装即可，这里略去 该过

程的说明。安装 成后，g2o的 件 位于/usr/local/g2o下，库 件
位于/usr/local/lib/下。现 ，我 重新 虑Ceres 程中的曲 合
验， g2o中 验一 。

6.4.3　使用g2o拟合曲线
为了 用g2o，首先要 曲 合问题 象成图优化。这个过程中，
只要记住 节点为优化变量，边为误差项 即可。曲 合的图优化问题可
以画成图6-3的形式。

图6-3　曲 合 应的图优化 。（莫明其 有些 华为的标志）

曲 合问题中， 个问题只有一个顶 ：曲 的参 a,b,c

； 各个带 的 ，构成了一个个误差项，也 是图优化的 。
这里的 与我 平时想的 不太一样， 是 一元边
（ Unary
Edge），即 只连接一个顶点 —— 为 个图只有一个顶 。所以 图6-
3中，我 只 画成 己连 己的样 。事 上，图优化中一条
可以连 一个、两个 个顶 ，这主要反映 个误差与 个优化变
量有关。 稍有些玄 的说 中，我 叫 超边
（Hyper Edge），
个图叫 超图
（Hyper Graph）[2] 。
弄 了这个图 之后， 下来 是 g2o中建立该 进行优化
了。 为g2o的用 ，我 要 的事主要包含以下步骤：
1. 义顶 和 的 。
2.构建图。
3. 优化 。
4.调用g2o进行优化， 果。
下面 示一下程序。
slambook/ch6/g2o_curve_ fi tting/main.cpp
 这个程序中，我 从g2o 生出了用于曲 合的图优化顶 和
：CurveFittingVertex和CurveFittingEdge，这 质上 展了g2o的
用方式。 这两个 生 中，我 重 了重要的虚 ：
1.顶 的更新 ：oplusImpl。我 知 优化过程最重要的是 量
x
∆ 的计 ， 该 x x x
理的是 k +1 = k +∆ 的过程。读 也许觉得这
并不是什么 得一 的事情， 为仅仅是个 单的加 已，为什么g2o
不帮我 成呢？ 曲 合过程中，由于优化变量（曲 参 ）本
位于 向量空间
中，这个更新计 确 是 单的加 。 是，当优化变
x
量不 向量空 中时，比 说 是相机位 ， 本 不一 有加 运
。这时， 需要重新 义 增量如何加到现有的估计上
的行为了。
第4讲的解 ，我 可 用左乘更新 右乘更新， 不是直 的加 。
2.顶 的重 ：setToOriginImpl。这是平凡的，我 估计
零即可。
3. 的误差计 ：computeError。该 需要取出 所连 的
顶 的当前估计 ，根 曲 ，与 的观 进行比 。这和最
二乘问题中的误差 是一致的。
4.存盘和读盘 ：read、write。由于我 并不想进行读/ 操
，所以留空。
义了顶 和 之后，我 main 里 明了一个图 ， 后
生成的 ，往图 中 加顶 和 ，最后调用优化 进
行优化。g2o会给出优化的 果：
 我 用列 伯格—马夸 方 进行梯度下降， 代了16次
后，最后优化 果与Ceres 验中相差无几。我 也 程序中 了 用
高斯 顿 和DogLeg下降方式，请读 去 前面的 符号， 行
比各种梯度下降方 的差异。

6.5　小结
本 介 了SLAM中 常碰 的一种非 性优化问题：由许 个误差
项平方和组成的最 二乘问题。我 介 了 的 义和 解，并且讨论
了两种主要的梯度下降方式：高斯 顿 和列 伯格—马夸 方 。
分中，分 用了Ceres和g2o两种优化库 解同一个曲 合
问题，发现 给出了相 的 果。
由于还 有详细谈Bundle Adjustment，所以 分 了曲
合这样一个 单 有代表性的 ，以 示一 的非 性最 二乘 解
方式。 ， 果用g2o来 合曲 ，必须先 问题转 为图优化，
义新的顶 和 ，这种 是有一些 的——g2o的主要目的并不
此。相比之下，Ceres 义误差项 曲 合问题则 了很 ， 为
本 即是一个优化库。 ， SLAM中更 的问题是，一个带有许 个
相机位 和许 个空 的优化问题 解。 ，当相机位 以
李代 表示时，误差项关于相机位 的导 计 ， 是一件 得详
细讨论的事。我 后 发现，g2o 了 量的顶 和 的
，非常 于相机位 估计问题。 Ceres中，我 不得不 己 现
一个Cost Function，有一些不 。
分的两个程序中，我 有去计 曲 关于三个参
的导 ， 是 用了优化库的 导，这 得理论和代码 会 一
些。Ceres库 了 于 板 的 动 导和运行时的 导， g2o
只 了运行时 导这一种方式。 是， 于 问题， 果
推导出雅可比矩阵的解析形式并告诉优化库， 可以 导中
的诸 问题。
最后，希望读 应Ceres和g2o这些 量 用 板 程的方
式。也许一开 会看上去比 吓人（ 是Ceres设 Problem和g2o
化 分的代码）， 是 悉之后， 会觉得这样的方式是 的， 且
易 展。我 SLAM后端一讲中 讨论稀疏性、核 、位 图
（Pose Graph）等问题。
习题
1.证明 性方程 Ax =b 当 矩阵 A超 时，最 二乘解为 x =(A T

A)- A b。
1 T

2.调研最速下降 、 顿 、高斯 顿 和列 伯格—马夸 方

各有什么优 。除了我 举的Ceres库和g2o库，还有哪些常用的优
化库？（ 可 会找 一些MATLAB上的库。）
3.为什么高斯 顿 的 量方程 矩阵可 不正 ？不正 有什
么几 含义？为什么 这种情 下解 不稳 了？
4.DogLeg是什么？ 与高斯 顿 和列 伯格—马夸 方 有
异同？请 相关的材料[3] 。
5. 读 Ceres 的 材 料 （ http://ceres-
solver.org/tutorial.html）以更 其用 。
6. 读g2o 带的 档， 看懂 吗？ 果还不 看懂，请
第10讲和第11讲之后 来再看。
 7.*请更 曲 合 验中的曲 ，并用Ceres和g2o进行优化
验。 ，可以 用更 的参 和更 杂的 。

[1] 核 的详细讨论见第10讲。

[2] 虽 笔 个人并不太喜欢有些 弄玄虚的说 。

[3] ，
http://www.numerical.rl.ac.uk/people/nimg/course/lectures/raphael/lectures/l
ec7slides.pdf。
 第7讲　视觉里程计1
主要目标
1.理解图 征 的意义,并 单幅图 中 取出 征 及 幅
图 中匹 征 的方 。
2.理解 极几 的原理， 用 极几 的约束，恢 出图 之 的
机的三 运动。
3.理解PNP问题，以及 用已知三 构与图 的 应关 解
机的三 运动。
4.理解ICP问题，以及 用 云的匹 关 解 机的三 运动。
5.理解 过三角化获得二 图 上 应 的三 构。
本书前面介 了运动方程和观 方程的具 形式，并讲解了以非
性优化为主的 解方 。从本讲开 ，我 束 础知识的 步
正题： 第2讲的 ，分 介 视觉里程计、优化后端、 环检 和
图构建4个 。本讲和下一讲主要介 为视觉里程计的主要理论，
后 第9讲中进行一次 。本讲关 于 征 方式的视觉里程计
。我 介 什么是 征 、 取和匹 征 ，以及 根
的 征 估计相机运动。
7.1　特征点法
顾第2讲的 ，我 说过视觉SLAM主要分为视觉前端和优化后
端。前端也称为视觉里程计（VO）。 根 相 图 的 息估计出 略
的相机运动，给后端 的 。VO的 现方 ， 是否需要
取 征，分为 征 的前端及不 征的直 前端。 于 征
的前端， 久以来（直 现 ）被认为是视觉里程计的主 方 。 运
行稳 ， 光 、动态 不 感，是目前比 成 的解 方案。 本
讲中，我 从 征 ， 习 取、匹 图 征 ， 后
估计两帧之 的相机运动和 景 构，从 现一个 本的两帧 视觉
里程计。

7.1.1　特征点
VO的主要问题是 根 图 来估计相机运动。 ，图 本 是
一个由亮度和 彩组成的矩阵， 果直 从矩阵层面 虑运动估计，
会非常 难。所以，我 习惯于 用这样一种 ：首先，从图 中
取比 有代表性 点 的 。这些 相机视角发生 量变化后会 不
变，所以我 会 各个图 中找 相同的 。 后， 这些 的 础
上，讨论相机位 估计问题，以及这些 的 位问题。 典SLAM
中， 称为路标。 视 觉 SLAM 中 ， 标 则 是 图 征
（Feature）。
根 百科的 义，图 征是一组与计 务相关的 息，计
务取 于具 的应用[29] 。 言之， 特征是图像信息的另一种数
字表达形式 。一组 的 征 于 务上的最终表现至关重要，所
以 年来研究 费了 量的 力 征进行研究。 字图 计
机中以 度 矩阵的方式存 ，所以最 单的，单个图 也是一
种“ 征”。 是， 视觉里程计中，我 希望 特征点在相机运动之后
保持稳定 ， 度 受光 、形变、 材质的影响严重， 不同图
变化非常 ，不 稳 。理想的情 是，当 景和相机视角发生 量
变时，还 从图 中 断哪些 方是同一个 ， 此仅 度 是不
的，我 需要 图 取 征 。
 征 是图 里一些 特别的地方
。以图7-1为 。我 可以 图
中的角 、 和区 当成图 中有代表性的 方。不过，我 更
易 确 出，某两幅图 中出现了同一个角 ；同一个 则稍微
难一些， 为 着该 前进，图 是相 的；同一个区 则是最
难的。我 发现，图 中的角 、 相比于 区 言更加“
”， 不同图 之 的 识度更强。所以，一种直观的 取 征的方
式 是 不同图 认角 ，确 的 应关 。 这种 中，
角 是所谓的 征。
， 应用中，单 的角 不 足我 的很 需
。 ，从远 看上去是角 的 方，当相机走近之后，可 不显
示为角 了。 ，当旋转相机时，角 的 观会发生变化，我 也
不 易 认出 是同一个角 。为此，计 机视觉领 的研究
年的研究中设计了许 更加稳 的 图 征， 著名的SIFT[30] 、
SURF[31] 、ORB[32] ，等等。相比于朴 的角 ，这些人工设计的 征
有 下的性质：
1.可重 性（Repeatability）：相同的“区 ”可以 不同的图
中找 。
2.可区 性（Distinctiveness）：不同的“区 ”有不同的表 。
3.高 率（E ffi ciency）：同一图 中， 征 的 量应远 于
的 量。
4.本 性（Locality）： 征仅与一 片图 区 相关。

图7-1　可以 为图 征的 分：角 、 、区
 征 由 关键点
（Key-point）和 描述子
（Descriptor）两 分组
成。比 ，当谈论SIFT 征时，是 “ 取SIFT关 ，并计 SIFT
”两件事情。关 是 该 征 图 里的位 ，有些 征 还
具有朝向、 等 息。 常是一个向量， 某种人为设计的
方式， 了该关 周 的 息。 是 “ 外观相似的特
征应该有相似的描述子 ”的原则设计的。 此，只要两个 征 的
向量空 上的 离相近， 可以认为 是同样的 征 。
历史上，研究 出过许 图 征。 有些很 确， 相机
的运动和光 变化下仍具有相 的表 ， 相应 需要 的计 量。
其 中 ， SIFT （ 度不变 征变 ， Scale-Invariant Feature
Transform）当属最为 典的一种。 分 虑了 图 变 过程中出现
的光 、 度、旋转等变化， 随之 来的是极 的计 量。由于 个
SLAM过程中图 征的 取与匹 仅仅是诸 环 中的一个， 目前
（2016年）为止，普 PC的CPU还无 时 计 SIFT 征，进行 位与
建图。所以 SLAM中我 甚 用这种“ ”的图 征。
另一些 征，则 虑 当降低 度和 性，以 升计 的速度。
， FAST 关 属于计 快的一种 征 （ 意这里“关
”的表 ，说明 有 ） 。 ORB （ Oriented FAST and
Rotated BRIEF） 征则是目前看来非常具有代表性的 时图 征。
[33]
进了FAST检 不具有方向性的问题，并 用速度极快的二进制
BRIEF[34] ， 个图 征 取的环 加速。根 论
中所 试， 同一幅图 中同时 取约1000个 征 的情 下，ORB约
要 费15.3ms，SURF约 费217.3ms，SIFT约 费5228.7ms。由此可以看
出，ORB 了 征 具有旋转、 度不变性的同时，速度方面 升明
显， 于 时性要 很高的SLAM来说是一个很 的 。
分 征 取 具有 的并行性，可以 过GPU等设 来加速计
。 过GPU加速后的SIFT， 可以 足 时计 要 。 是，引 GPU
带来 个SLAM成本的 升。由此带来的性 升是否足以 去 出的
计 成本，需要 的设计人员 细 量。 目前的SLAM方案中，ORB是
质量与性 之 的 中， 此，我 以ORB为代表介 取 征的
个过程。

7.1.2　ORB特征
 ORB 征 亦 由 关键点 描述子
和 两 分组成。 的关 称
为“Oriented FAST”，是一种 进的FAST角 ，关于什么是FAST角 我
下 介 。 的 称为BRIEF（Binary Robust Independent
Elementary Feature）。 此， 取ORB 征分为 下两个步骤：
1.FAST角 取：找出图 中的“角 ”。相 于原版的FAST，ORB
中计 了 征 的主方向，为后 的BRIEF 加了旋转不变 性。
2.BRIEF ： 前一步 取出 征 的周 图 区 进行 。
下面分 介 FAST和BRIEF。
FAST 关键点
FAST是一种角 ，主要检 度变化明显的 方，以速度
快著称。 的思想是： 果一个 与 的 差 （过亮 过
暗）， 么 更可 是角 。相比于其 角 检 ，FAST只需比
亮度的 ，十分快 。 的检 过程 下（见图7-2）：
1. 图 中 取 p， 设 的亮度为 Ip 。
2.设 一个 T （比 ， Ip 的20%）。
3.以 p 为中心， 取半径为3的 上的16个 。
4. 取的 上有连 的 N个 的亮度 于 Ip +T 于 Ip -T ，
么 p 可以被认为是 征 （N
常取12，即为FAST-12。其 常用
N
的 取 为9和11， 分 被称为FAST-9和FAST-11）。
5.循环以上 步， 一个 行相同的操 。
FAST-12 中，为了更高 ，可以 加一项预 试操 ，以快速
除 不是角 的 。具 操 为， 于 个 ，直 检
上的第1,5,9,13个 的亮度。只有当这4个 中有3个同时
I T
于 p + 于 p I -T
时，当前 才有可 是一个角 ，否则应该直
除。这样的预 试操 加速了角 检 。此 ，原 的FAST角
常出现“扎 ”的现象。所以 第一 检 之后，还需要用非极
制（Non-maximal suppression）， 一 区 仅 留响应极
的角 ， 角 集中的问题。
 图7-2　FAST 征 [33] 。

FAST 征 的计 仅仅是比 亮度的差异，速度非常快，

也有一些问题。首先，FAST 征 量很 且不确 ， 我 往往希
望 图 取 量的 征。 此， ORB中 原 的FAST 进行了
进。我 可以 最终要 取的角 量 ， 原 FAST角 分 计 N
N
Harris响应 ， 后 取前 个具有最 响应 的角 为最终的角
集合。
其次，FAST角 不具有方向 息。 且，由于 取半径为3的
，存 度问题：远 看着 是角 的 方， 近后看可 不是角
了。 FAST角 不具有方向性和 度的弱 ，ORB 加了 度和旋转
的 。 度不变性由构建图 字 [1] ，并 字 的 一层上检
角 来 现。 征的旋转是由 度质心 （Intensity Centroid）
现的。我 稍微介 一下。
所谓质心是 以图 度 为权重的中心。其具 操 步骤
下[35] ：
1. 一个 的图 B 中， 义图 的矩为

2. 过矩可以找 图 的质心：
 3.连 图 的几 中心 O 与质心C ，得 一个方向向量 于是
征 的方向可以 义为

过以上方 ，FAST角 具有了 度与旋转的 ，从

升了其 不同图 之 表 的 性。所以 ORB中， 这种 进后的
FAST称为Oriented FAST。
BRIEF描述子
取Oriented FAST关 后，我 个 计 其 。ORB
用 进的BRIEF 征 。我 先来介 一下BRIEF是什么。
BRIEF是一种 二进制 ，其 向量由许 个0和1组成，这里
的0和1 码了关 附近两个 （比 和 ）的p q 关 ： 果 p
比q ，则取1，反之 取0。 果我 取了128个这样的 p,q
，最后 得
128 由0、1组成的向量。 么， 和 p q 取呢？ 原 的论
中给出了 干种 方 ， 上 是 某种 率分布，随机
p q和 的位 ，读 可以 读BRIEF论 OpenCV 码以查看其具
现[34] 。BRIEF 用了随机 的比 ，速度非常快， 且由于 用了二
进制表 ，存 起来也十分方 ， 用于 时的图 匹 。原 的BRIEF
不具有旋转不变性， 此 图 发生旋转时 易丢 。 ORB
FAST 征 取阶 计 了关 的方向，所以可以 用方向 息，计
了旋转之后的“Steer BRIEF” 征 ORB的 具有 的旋转不
变性。
由于 虑 了旋转和 ， 得ORB 平移、旋转和 的变 下仍
有 的表现。同时，FAST和BREIF的组合也非常高 ， 得ORB 征
时SLAM中非常受欢 。我 图7-3中展示了一张图 取ORB之后的
果，下面来介 不同的图 之 进行 征匹 。
 图7-3　OpenCV 的ORB 征 检 果。

7.1.3　特征匹配
征匹 （ 图7-4所示）是视觉SLAM中极为关 的一步，
说， 征匹 解 了SLAM中的 关 问题（dataassociation），即确
当前看 的 标与之前看 的 标之 的 应关 。 过 图 与图
图 与 图之 的 进行 确匹 ，我 可以为后 的 态
估计、优化等操 量负 。 ，由于图 征的 性，误
匹 的情 广 存 ， 且 期以来一直 有得 有 解 ，目前已
成为视觉SLAM中制约性 升的一 瓶颈。 分原 是 景中 常存
量的重 纹理， 得 征 非常相 。 这种情 下，仅 用
征解 误匹 是非常 难的。
 图7-4　两帧图 的 征匹 。

不过，让我 先来看正确匹 的情 ，等 验再 去讨论误

匹 问题。 虑两个时刻的图 。 果 图 I t 中 取 征
图 It 中 取 征 寻找
暴力匹配（
+1

这两个集合 的 应关 呢？最 单的 征匹 方 是
）
Brute-Force Matcher 。即 一个 征 与所有的 量
的 离， 后 序，取最近的一个 为匹 。 离表示了两
个 征之 的 相似程度 ，不过 际运用中还可以取不同的 离度量
。 于 的 ， 用 离进行度量即可。 于二进
制的 （比 BRIEF这样的），我 往往 用 明 离（Hamming
distance） 为度量——两个二进制串之 的 明 离， 的是其 不同
位数的个数 。
，当 征 量很 时，暴力匹 的运 量 变得很 ，
是当想要匹 某个帧和一张 图的时 。这不符合我 SLAM中的
时性需 。此时 快速近似最近邻（ FLANN 更加 合于匹 ） 量
极 的情 。由于这些匹 理论已 成 ， 且 现上也已集成
OpenCV，所以这里 不再 的技术细 了。感兴趣的读 可以参
读 献[36]。

7.2　实践：特征提取和匹配

图7-5　 验 用的两帧图 。

目前主 的几种图 征 OpenCV开 图 库中 已 集成，我

可以很方 进行调用。下面 来 际练习一下OpenCV的图 征
取、计 和匹 的过程。我 为此 验 了两张图 ，位于
slambook/ch7/下的1.png和2.png， 图7-5所示。 是来 公开
集[37]中的两张图 ，我 看 相机发生了微 的运动。本 程序 示
取ORB 征并进行匹 。下一个程序 示 估计相机运动。
征 取与匹 代码：
slambook/ch7/feature_extraction.cpp
 运行此程序（需要 两个图 位 ）， 出运行 果：

图7-6显示了 程的运行 果。我 看 未 的匹 中带有 量的

误匹 。 过一次 之后，匹 量 了许 ， 匹 是
正确的。这里， 的 汉明距离小于最小距离的两倍
是 ，这是一种
工程上的 验方 ，不一 有理论 。不过，尽 示 图 中
出正确的匹 ， 我 仍 不 证 所有其 图 中得 的匹
是正确的。 此， 后面的运动估计中，还需要 用去除误匹 的
。
下来，我 希望根 匹 的 估计相机的运动。这里由于相机
的原理不同，情 发生了变化：
1.当相机为单目时，我 只知 2D的 坐标， 问题是根 两
组 点
2D 估计运动。该问题用对极几何 来解 。
 2.当相机为双目、RGB-D时， 过某种方 得 了 离 息，
么问题 是根 两组 点
3D 估计运动。该问题 常用ICP来解 。
3. 果有3D 及其 相机的 影位 ，也 估计相机的运动。该问
题 过PnP 解。
此，下面几 来介 这三种情形下的相机运动估计。我 从
最 本的2D-2D情形出发，看看 解， 解过程又具有哪些麻 的
问题。

图7-6　 征 取与匹 果。

7.3　2D-2D：对极几何
7.3.1　对极约束
现 ， 设我 从两张图 中得 了一 的 征 ， 图7-7
所示。 果有 干 这样的匹 ， 可以 过这些二 图 的 应
关 ，恢 出 两帧之 机的运动。这里“ 干 ”具 是
呢？我 会 下 介 。下面先来看看两个图 当中的匹 有什么几
关 。
 图7-7　 极几 约束。

以图7-7为 ，我 I ,I 之 的运动，设第一
希望 取两帧图
帧 第二帧的运动为R,t 。两个相机中心分 为O ,O 。现 ， 虑I
1 2

中有一个 征 p ， I 中 应着 征 p 。我 知 两 是
1 2

确 是同一个空间点在
1 1 2 2

过 征匹 得 的。 果匹 正确，说明
两个成像平面上的投影 。这里需要一些术语来 之 的几 关
。首先，连 和连 三 空 中会相交于 P 。这时
O ,O ,P 三 个 可 以 确 一 个 平 面 ， 称 为 极平面（ Epipolar
plane） 。O O 连 与 平面I ,I 的交 分 为 e ,e 。 e
1 2

,e 称为极点（ Epipoles） ，O O 被称为基线（ Baseline） 。

1 2 1 2 1 2 1

我 称极平面与两个 平面 I ,I 之 的相交 l ,l 为 极线（

2 1 2

Epipolar line） 。
1 2 1 2

直观讲，从第一帧的角度看， 是某个像素可能出现的空间
位置 —— 为该 上的所有 会 影 同一个 。同时， 果
不知 P 的位 ， 么当我 第二幅图 上看时，连 也 是
第二幅图 中的极 ） 是P 可 出现的 影的位 ，也 是
第二个相机中的 影。现 ，由于我 过 征 匹 确 了p 的
推断P 的空 位 ，以及相机的运动。要
2

位 ，所以 读 的
是，这完全多亏了正确的特征匹配 。 果 有 征匹 ，我 确
p 底 极 的哪个位 了。 时， 必须 极 上 以获得正确
2

的匹 ，这 第13讲中 。
 现 ，我 从代 角度来看一下这里出现的几 关 。 第一帧的
坐标 下，设 的空 位 为 P

根 第5讲介 的 相机 ，我 知 两个 p ,p1 2 的
位 为

K
这里 为相机 参矩阵， 为两个坐标 的相机运动（ R,t 果我
愿意，也可以 成李代 形式）。 果 用齐次坐标，也可以 上式
成 乘以非零常 下成立的（up to a scale）等式[2] ：

现 ，取：

这里的 x ,x
1 2 是两个 的归一化平面上的坐标。代 上式，
得：

两 同时左乘 t∧ 。 忆 ∧ 的 义，这相当于两 同时与 t

积：

后，两 同时左乘

观 等式左 ， t∧ x 2 是一个与 t 和x 2 直的向量。 再和 x

2 积时， 得 0。 此，我 得 了一个 的式 ：
 重新代 p ,p
1 2 ，有：

这两个式 称为 对极约束 ， 以形式 著名。 的几 意义是

O ,P,O 2 三 面。 极约束中同时包含了平移和旋转。我 中
F
1

分记 两个矩阵： 础矩阵（Fundamental Matrix） 和本质矩阵

E
（Essential Matrix） ，于是可以进一步 化 极约束：

极约束 给出了两个匹 的空 位 关 。于是，相机位

估计问题变为以下两步：
1.根 的 位 出 E F。
2.根 E F 出 R,t 。
E F
由于 和 只相差了相机 参， 参 SLAM中 常是已知的[3] ，
所以 当中往往 用形式更 单的 E E
。我 以 为 ，介 上面两个
问题 解。

7.3.2　本质矩阵
根 义，本质矩阵 = ∧ E t R 。 是一个3× 3的矩阵， 有9个未
知 。 么，是不是 意一个3 × 3的矩阵 可以被当成本质矩阵呢？从
E的构造方式上看，有以下 得 意的 方：
•本质矩阵是由 极约束 义的。由于 极约束是 等式为零 的约
束，所以 E
乘以 意非零常 后， 对极约束依然满足 。我 这件事
情称为 E
不同 度下是等价的。

•根 = ∧ E t R
，可以证明[3] ，本质矩阵 E的 异 必 σ,σ,
是[
0]T 的形式。这称为 。 本质矩阵的内在性质
•另一方面，由于平移和旋转各有3个 由度， t∧ R 有6个 由
度。 由于 度等价性， 际上有5个 由度。 E
 E 具有5个 由度的事 ，表明我 最 可以用5 来 解E 。
是，E 的 性质是一种非 性性质， 解 性方程时会带来麻 ，
此，也可以只 虑 的尺度等价性 ， 用8 来估计 E ——这 是
典的 八点法（ Eight-point-algorithm ） 。八 只 用了 E [38,39]

的 性性质， 此可以 性代 框架下 解。下面我 来看八 是

工 的。

虑一 匹 ， 的归一化坐标为 x 1 =[u ,v , 1] ,x
1 1
T
2 =
[u ,v , 1]
2 2
T
。根 极约束，有：

我 矩阵 E 展开， 成向量的形式：

么 极约束可以 成与 e 有关的 性形式：

同理， 于其 也有相同的表示。我 所有 一个方

程中，变成 性方程组（ i i 表示第 个 征 u ,v i ，以此 推）：
 这8个方程构成了一个 性方程组。 的 矩阵由 征 位 构
成， 为8 × e
9。 位于该矩阵的零空 中。 果 矩阵是 秩的
（即秩为8）， 么 的零空 为1，也 是 构成一条 。这与 e e
的 度等价性是一致的。 果8 匹 组成的矩阵 足秩为8的条件，
么 的各 E 可由上 方程解得。
下来的问题是 E
根 已 估得的本质矩阵 ，恢 出相机的运
动 R,t 。这个过程是由 E
异 分解（SVD）得 的。设 的SVD分解为

其中 U,V 为正交阵，Σ为 异 矩阵。根 E 的 性质，我 知

Σ=diag( σ,σ, 0)。 SVD分解中， 于 意一个E ，存 两个可
的 t,R
与 应：

其中 表示 Z
旋转90 ° 得 的旋转矩阵。同时，由于 和 -E
E等价，所以 t
意一个 取负号，也会得 同样的 果。 此，从 E
分解 t,R
时，一 存 4 可 的解。 个
 图7-8形象 展示了分解本质矩阵得 的4个解。我 已知空
相机（蓝 ）上的 影（ ），想要 解相机的运动。
不变的情 下，可以画出4种可 的情 。不过幸运的是，只有第一
种解中 P
两个相机中 具有正的 度。 此，只要 意一 代 4种
解中，检 该 两个相机下的 度， 可以确 哪个解是正确的了。

图7-8　分解本质矩阵得 的4个解。 影 （ ）不变的情 下，两

个相机及空 一 有4种可 的情 。

果 用 的 E
性质， 么 只有5个 由度。所以最 可以 过
5 来 解相机运动[40,41] 。 这种 形式 杂，从工程 现角度
虑，由于平时 常会有几十 乃至上百 的匹 ，从8 至5 意
义并不明显。为 单，我 这里 只介 本的八 。
剩下的问题还有一个：根 性方程解出的 ，可 不 足 的 E E
性质—— 的 异 不一 为 0的形式。这时， σ,σ,
SVD时我
会刻意 Σ矩阵调 成上面的样 。 常的 是， 八 得的 E
进行SVD分解后，会得 异 矩阵Σ=diag( 1 3 )，不 σ ,σ ,σ
σ σ σ
2

设 1 ≥ 2 ≥ 3 。取：

这相当于是 出来的矩阵 影 了 所 的 形上。当 ，更 E

单的 是 异 矩阵取成diag(1 1 0)， 为 具有 度等价性， , , E
所以这样 也是合理的。

7.3.3　单应矩阵
 除了 本矩阵和本质矩阵，还有单应矩阵（Homography） ， H
了两个平面之 的映 关 。 景中的 征 落 同一平面上
（比 、 面等），则可以 过单应性来进行运动估计。这种情
无人机 带的俯视相机 扫 机 带的顶视相机中比 常见。由于之前
有 过单应， 此这里稍微介 一下。
单应矩阵 常 于 同平面上的一些 两张图 之 的变
关 。 虑 图 I 和I 有一 匹 的 征 1 和 2 。这些 p p
P 上，设这个平面
1 2

征 落 平面 足方程：

稍加 理，得：

后， 顾本 开 的式(7.1)，得：

于是，我 得 了一个直 图 坐标 p 和p 之 的变 ，
H ，于是：
1 2

中 这 分记为

的 义与旋转、平移及平面的参 有关。与 础矩阵 ，单 F

H ×
应矩阵 也是一个3 3的矩阵， 解时的思 也和 ，同样可以先 F
根 匹 计 ， 后 H分解以计 旋转和平移。 上式展开，
得：
 请 意，这里的等号是 非零 下成立的。我 际 理中
常乘以一个非零 h
得 9 =1（ 取非零 时）。 后根 第3行，
去 这个非零 ，于是有：

理得：

这样一组匹 可以构造出两项约束（事 上有三个约束，

是 为 性相关，只取前两个），于是 由度为8的单应矩阵可以 过4
匹 征 出（ 意，这些 征 不 有三 的情 ），即
h
解以下的 性方程组（当 9 =0时，右 为零）：
 这种 H
矩阵看成了向量， 过解该向量的 性方程来恢 H
，又称直 性变 （Direct Linear Transform）。与本质矩阵相
， 出单应矩阵以后需要 其进行分解，才可以得 相应的旋转矩阵 R
t
和平移向量 。分解的方 包 [42,43]
与解析 [44] 。与本质矩阵
的分解 ，单应矩阵的分解同样会 4组旋转矩阵与平移向量，并且
同时可以计 出 分 应的 景 所 平面的 向量。 果已知成
的 图 的 度 为正 （即 相机前方），则又可以 除两组解。
最后仅剩两组解，这时需要 过更 的先验 息进行 断。 常我 可
以 过 设已知 景平面的 向量来解 ， 景平面与相机平面平
n
行， 么 向量 的理论 为1T 。
单应性 SLAM中具有重要意义。当 征 面 相机发生 旋转
时， 础矩阵的 由度下降，这 出现了所谓的 化（degenerate）。
现 中的 总包含一些 ，这时 果 用八 解 础矩
阵， 础矩阵 出来的 由度 会主要由 。为了
化现象造成的影响， 常我 会同时估计 础矩阵 和单应矩阵 ， F H
重 影误差比 的 个 为最终的运动估计矩阵。

7.4　实践：对极约束求解相机运动
下面，我 来练习一下 过本质矩阵 解相机运动。上一
分的程序 了 征匹 ， 这次我 用匹 的 征 来计
E,F H
和 ，进 分解 得 E R,t
。 个程序 用OpenCV 的 进
行 解。我 上一 的 征 取 装成 ，以 后面 用。本 只
展示位 估计 分的代码。
slambook/ch7/pose_estimation_2d2d.cpp（片 ）
 该 了从 征 解相机运动的 分， 后，我 主
中调用 ， 得 相机的运动：
slambook/ch7/pose_estimation_2d2d.cpp（片 ）
 我 中 E,F 和H 的
出了 ， 后验证了 极约束是否成
t R E
立，以及 ∧ 和 非零 乘下等价的事 。现 ，调用此程序即可看
出 果：
 程序的 出 果可以看出， 极约束的 足 度约 10 - 3 量
级。根 前面的讨论，分解得 的 R,t
一 有4种可 性。不过，OpenCV
会替我 用三角化检 角 的 度是否为正，从 出正确的解。
 需要 意的 方是，我 要弄 程序 解出来的 R,t 是什么意义。
程的 义，我 的 极约束是从

得 的。这里的 R,t 组成的变 矩阵，是第一个图 第二个图的坐

标变 矩阵：

请读 中务必 楚这里 用的变 顺序（ 为有时我 会用 T

12 ）， 非常容易搞反 。

讨论
从 E F
示程序中可以看 ， 出的 和 之 相差了相机 参矩阵。
虽 上并不直观， 可以验证 的 关 。从 和 E,F H
H
可以分解出运动，不过 需要 设 征 位于平面上。 于本 验的
，这个 设是不 的，所以我 这里主要用 来分解运动。 E
E
得一 的是，由于 本 具有 度等价性， 分解得 的 也 t,R
有一个 度等价性。 R∈
SO(3) 具有约束，所以我 认为 具有一 t
个 尺度。 言之， 分解过程中， t
乘以 意非零常 ，分解 是成
立的。 此，我 常 进行 t 归一化
，让 的 度等于1。
尺度不确定性
t 度的归一化，直 导致了单目视觉的尺度不确定性（ Scale
Ambiguity） 。 ，程序中 出的 t 第一 约为0.822。这个0.822究
竟是 0.822 还是0.822厘 ，我 是 确 的。 为 t 乘以 意
比 常 后， 极约束 是成立的。 言之， 单目SLAM中，
和 图同时 意 ，我 得 的图 是一样的。这 第2讲中
已 向读 介 过了。
单目视觉中，我 两张图 的 归一化相当于t 。虽 固定了尺度
我 不知 的 际 度是 ， 我 以这时的 为单位1，计 相 t
机运动和 征 的3D位 。这被称为单目SLAM的 。 化之初始化
后， 可以用3D-2D来计 相机运动了。 化之后的 和 图的单
位， 是 化时 的 度。 此，单目SLAM有一步不可 的 初始
化。 化的两张图 必须有一 程度的平移， 后的 和 图
以此步的平移为单位。
除了 t
进行归一化之 ，另一种方 是令 化时所有的 征
平 度为1，也可以 一个 度。相比于令 t 度为1的 ，
征 度归一化可以控制 景的规 ， 计 上更稳 些。
不过这并 有理论上的差 。
初始化的纯旋转问题
从 E 分解 R,t 的过程中， 果相机发生的是 旋转，导致 t 为
零， 么，得 的E 也 为零，这 导致我 无从 解R 。不过，此时
我 可以 靠H 取旋转， 仅有旋转时，我 无 用三角 量估计
征 的空 位 （这 下 ），于是，另一个 论是，单目初始
化不能只有纯旋转，必须要有一定程度的平移 。 果 有平移，单目
无 化。 当中， 果 化时平移太 ，会 得位 解
与三角化 果不稳 ，从 导致 败。相 ， 果 相机左右移动
不是原 旋转， 易让单目SLAM 化。 ，有 验的SLAM研究人
员， 单目SLAM情 下 常 让相机进行左右平移以顺 进行
化。
多于 8对点的情况
当给 的 于8 时（比 ， 程找 了79 匹 ），我 可以
计 一个最 二乘解。 忆式(7.12)中 性化后的 极约束，我 左
的 矩阵记为 ： A

于八 ， 的 A ×
为8 9。 果给 的匹 于8，该方程
构成一个超 方程，即不一 存 得上式成立。e 此，可以 过最
化一个二次 来 ：

于是 E
出了 最 二乘意义下的 矩阵。不过，当可 存 误匹
的情 时，我 会更 向于 用 随机采样一致性（
Random Sample
，
Concensus RANSAC）来 ， 不是最 二乘。RANSAC是一种 用的
， 用于很 带 误 的情 ，可以 理带有 误匹 的 。
7.5　三角测量
之前两 我 用 极几 约束估计了相机运动，也讨论了这种方
的 性。 得 运动之后，下一步我 需要用相机的运动估计 征
的空 位 。 单目SLAM中，仅 过单张图 无 获得 的 度
息，我 需要 过三角测量（ Triangulation 的方 来 ）（或三角化）
估计 图 的 度， 图7-9所示。

图7-9　三角化获得 图 度

三角 量是 ， 过 两 观 同一个 的夹角，从 确 该 的
离。三角 量最早由高斯 出并应用于 量 中， 天 、 理
的 量中 有应用。 ，我 可以 过不同 观 的星星的角
度，估计 离我 的 离。 SLAM中，我 主要用三角化来估计
的 离。
和上一 ， 虑图 I
I ，以左图为参 ，右图的变 矩
和
阵为T 。相机光心为O 和O 。 I 中有 征 p ， 应 I 中有
1 2

征 p 。理论上直 O p 与O p 景中会相交于一 P ，
1 2 1 1 2

2 1 1 2 2

该 即两个 征 所 应的 图 三 景中的位 。 由于
的影响，这两条直 往往无 相交。 此，可以 过最二 乘 解。
 极几 中的 义，设 x ,x1 2 为两个 征 的归一化坐标，
么 足：

现 我 已 知 了 R,t ，想要 解的是两个 征 的 s ,s

度
s 果要 s ，
1 2

。当 这两个 度是可以分开 的，比 ，先来看 2 。 2

么先 上式两 左乘一个 x∧ 1 ，得：

该式左 为零，右 可看成 s 的一个方程，可以根 直 得 s

s s
2

。有了 ， 也非常 易 出。于是，我 得 了两帧下的 的

R,t
2 2 1

度，确 了 的空 坐标。当 ，由于 的存 ，我 估得的

不一 确 式(7.24)为零，所以更常见的 是 最 二乘解 不是
零解。

7.6　实践：三角测量
7.6.1　三角测量代码
下面，我 示 根 之前 用 极几 解的相机位 ， 过
三角化 出上一 征 的空 位 。我 调 用 OpenCV 的
triangulation 进行三角化。
slambook/ch7/triangulation.cpp（片断）
同时， main 中 加三角 量 分，并验证重 影关 ：
 我 印了 个空 两个相机坐标 下的 影坐标与 坐标
P
——相当于 的 影位 与看 的 征 位 。由于误差的存 ，
会有一些微 的差异。以下是某一 征 的 息：
 可以看 ，误差的量级 约 后第3位。可以看 ，三角化
征 的 离 约为15。 由于 度不确 性，我 并不知 这里的15究
竟是 。

7.6.2　讨论
关于三角 量，还有一个必须 意的 方。
三角 量是由 平移
得 的，有平移才会有 极几 中的三角形，才
谈得上三角 量。 此， 旋转是无 用三角 量的， 为 极约束
远 足。 平移存 的情 下，我 还要关心三角 量的不确
性，这会引出一个 三角测量的矛盾
。
图7-10所示，当平移很 时， 上的不确 性 导致 的
度不确 性。也 是说， 果 征 运动一个 ， 得视 角变 δx
化了一个角度 δθ
， 么 量 度 有 δd
的变化。从几 关 可
以看 ，当t 时， δd
明显变 ，这说明平移 时， 同样的
相机分 率下，三角化 量 更 确。 该过程的 量分析可以 用正
弦 理得 ，不过这里先 虑 性分析。
此，要 高三角化的 度，其一是 高 征 的 取 度，也
是 高图 分 率—— 这会导致图 变 ， 加计 成本。另一方式
是 平移量 。 是，这会导致图 的 外观
发生明显的变化，比
原先被 住的 面显示出来，又比 反 光发生变化，等等。 观变
化会 得 征 取与匹 变得 难。总 言之，再 平移，会导致匹
； 平移太 ，则三角化 度不 ——这 是三角化的矛盾。
 图7-10　三角 量的矛盾。

虽 本 只介 了三角化的 度估计， 只要我 愿意，也

量 计 位置 不确定性
个 征 的 及 。所以， 果 设 征 服从
高斯分布，并且不断 进行观 ， 息正确的情 下，我
期望它的方差会不断减小乃至收敛 。这 得 了一个 滤波器
，称为
深度滤波器（ ）
Depth Filter 。不过，由于 的原理 杂，我
留 第13讲再详细讨论 。下面，我 来讨论从3D-2D的匹 来估计相
机运动，以及3D-3D的估计方 。

7.7　3D-2D：PnP
PnP（Perspective-n-Point）是 解3D 2D 运动的方 。
了当知 n 个3D空 及其 影位 时， 估计相机的位 。前面
说 ，2D-2D的 极几 方 需要8个 8个以上的 （以八 为
），且存 着 化、 旋转和 度的问题。 ， 果两张图 中
其中一张 征 的3D位 已知， 么最 只需3个 （需要至 一个额
验证 果） 可以估计相机运动。 征 的3D位 可以由三角化
RGB-D相机的 度图确 。 此， 双目 RGB-D的视觉里程计中，我
可以直 用PnP估计相机运动。 单目视觉里程计中，必须先进行
化， 后才 用PnP。3D-2D方 不需要 用 极约束，又可以
很 的匹 中获得 的运动估计，是最重要的一种 态估计方 。
 PnP问题有很 种 解方 ， ，用3 估计位 的P3P[45] 、直
性变 （DLT）、EPnP（E ffi cientPnP） [46] 、UPnP[47] ，等等。
此 ，还 用 非线性优化 的方式，构建最 二乘问题并 代 解，也
是万 式的Bundle Adjustment。我 先来看DLT， 后再讲解Bundle
Adjustment。

7.7.1　直接线性变换
虑某个空 P， 的齐次坐标为 P =(X,Y,Z, 1)
T
。 图 I 1

中， 影 征 x
u ,v , 1) （以 归一化平 面齐次 坐标 表
=( T

示）。此时相机的位 R,t 是未知的。与单应矩阵的 解

1 1 1

，我
义 广矩阵[R|t ]为一个3× 4的矩阵，包含了旋转与平移 息 。我 [4]

其展开形式列 下：

用最后一行 s 消去，得 两个约束：为了 化表示， 义 T 的行

向量：

于是有：

和
 t
请 意， 是待 的变量，可以看 ， 个 征 了两个关于 t
的 性约束。 设一 有 个 征 ，则可以列出 下 N 性方程组：

t
由于 一 有12 ， 此，最 过6 匹 即可 现矩阵 的 T
性 解，这种方 称为直 性 变 （ Direct Linear Transform ，
DLT）。当匹 于6 时，也可以 用SVD等方 超 方程 最 二
乘解。
DLT 解中，我 直 矩阵看成了12个未知 ，忽略了 T 之
的 。 为旋转矩阵 R∈
SO(3)，用DLT 出的解不一 足该约
束， 是一个一 矩阵。平移向量比 办， 属于向量空 。 于旋
R
转矩阵 ，我 必须 DLT估计的 左 3 3的矩阵 ，寻找一个最 T ×
的旋转矩阵 进行近 。这可以由QR分解 成[3,48] ，相当于 果
从矩阵空 重新 影 SE(3) 形上，转 成旋转和平移两 分。
需要解 的是，我 这里的 x 用了归一化平面坐标，去 了 参
K 参K
1

矩阵 的影响——这是 为 SLAM中 常 设为已知。即 参

未知，也 用PnP去估计 K,R,t 三个量。 由于未知量 ， 果会差
一些。

7.7.2　P3P
下面讲的P3P是另一种解PnP的方 。 仅 用3 匹 ， 要
， 此这里也 单介 一下（这 分推导 了 献[49]）。
P3P需要 用给 的3个 的几 关 。 的 为3 3D-2D匹
。记3D 为 ，2D 为 A,B,C
，其中 字 代表的 为 应 a,b,c
字 代表的 相机成 平面上的 影， 图7-11所示。此 ，P3P
还需要 用一 验证 ，以从可 的解中 出正确的 一个（ 于
极几 情形）。记验证 D-d
为 ，相机光心为 。请 意，我 知 O
的是A,B,C 世界坐标系中的坐标 ， 不是 。 在相机坐标系中的坐标
一旦3D 相机坐标 下的坐标 出，我 得 了3D-3D的 应
， PnP问题转 为了ICP问题。
首先，显 三角形之 存 应关 ：

图7-11　P3P问题示意图。

来 虑Oab 和OAB 的关 。 用 弦 理，有：

于其 两个三角形亦有 性质，于是有：

以上三式 除以 OC 2
，并且记 x =OA /OC,y =OB /OC ，得：
 记 v =AB /OC ,uv =BC /OC ,wv =AC /OC
2 2 2 2 2 2
，有：

我 可以 第一个式 中的 v 等式一 ，并代 其后两式，

得：

意这些方程中的已知量和未知量。由于我 知 2D 的图 位
，3个 弦角cos〈a,b〉, 〈b,c〉, 〈a,c〉
cos cos 是已知的。同
u BC AB ,w AC AB
时， = 2
/ 2 A,B,C
= 2 / 2 可以 过 世界坐标 下的坐
标 出，变 相机坐标 下之后，这个比 并不 变。该式中的 x,y
是未知的，随着相机移动会发生变化。 此，该方程组是关于 的一 x,y
个二 二次方程（ 项式方程）。解析 解该方程组是一个 杂的过
程，需要用吴消 。这里不展开 该方程解 的介 ，感兴趣的读
请参 献[45]。 E
于分解 的情 ，该方程最 可 得 4个解，
我 可以用验证 来计 最可 的解，得 相机坐标 下的3D A,B,C
坐标。 后，根 3D-3D的 ，计 相机的运动 。这 分 7.9 R,t
介 。
从P3P的原理可以看出，为了 解PnP，我 用了三角形相 性
质， 解 影 a,b,c
相机坐标 下的3D坐标，最后 问题转 成一
个3D 3D的位 估计问题。 后 看 ，带有匹 息的3D-3D位
解非常 易，所以这种思 是非常有 的。其 的一些方 ，
EPnP，亦 用了这种思 。 ，P3P也存 着一些问题：
1.P3P只 用3个 的 息。当给 的 于3组时，难以 用更
的 息。
 2. 果3D 2D 受 影响， 存 误匹 ，则 。
所以后 人 还 出了许 的方 ， EPnP、UPnP等。 用
更 的 息， 且用 代的方式 相机位 进行优化，以尽可 消除
的影响。不过，相 于P3P来说，原理会更加 杂一些，所以我 建
议读 读原 的论 ， 过 来理解PnP过程。 SLAM当中， 常
的 是先 用P3P/EPnP等方 估计相机位 ， 后构建最 二乘优化
问题 估计 进行调 （Bundle Adjustment）。 下来我 从非 性优
化角度来看一下PnP问题。

7.7.3　Bundle Adjustment
除了 用 性方 之 ，我 还可以 PnP问题构建成一个 义于李
代 上的非 性最 二乘问题。这 用 本书第4讲和第5讲的知识。前
面说的 性方 ，往往是 先求相机位姿，再求空间点位置
， 非 性优
化则是 看成优化变量， 一起优化。这是一种非常 用的
解方式，我 可以用 PnP ICP给出的 果进行优化。 PnP中，这个
Bundle Adjustment问题，是一个最 化 重投影误差（
Reprojection
error） 的问题。我 本 给出此问题 两个视图下的 本形式，
后 第10讲讨论 规 的BA问题。
虑 n 个三 空 P 及其 影 p ，我 希望计 相机的位 R,t
， 的李代 ξ 。 设某空
表示为 坐标为 Pi =[Xi ,Yi ,Zi ]
T
，其
影的 坐标为ui =[ui ,vi ] 。根
T
第5讲的 ， 位 与空
位 的关 下：

除了用 表示相机 ξ 态之 ， 的 和前面的 义 一致。 成

矩阵形式 是：
 请读 补中 隐含着的齐次坐标 非齐次的转 ，否则 矩阵的
乘 来说， 度是不 的 [5] 。现 ，由于相机位 未知及观 的
，该等式存 一个误差。 此，我 误差 和，构建最 二乘问
题， 后寻找最 的相机位 ， 最 化：

该问题的误差项，是 坐标（观 的 影位 ）与3D

当前估计的位 进行 影得 的位 相比 得 的误差，所以称为 重投
影误差 。 用齐次坐标时，这个误差有3 。不过，由于 最后一 为 u
1，该 度的误差一直为零， 我 更 时 用非齐次坐标，于是误
差 只有2 了。 图7-12所示，我 过 征匹 知 了 1 和 2 是 p p
同一个空 P的 影， 是不知 相机的位 。 中， P 的
p
影ˆ 2 与 际的p 2 之 有一 的 离。于是我 调 相机的位 ，
得这个 离变 。不过，由于这个调 需要 虑很 个 ，所以最后
个 的误差 常 不会 确为零。
最 二乘优化问题已 第6讲介 过了。 用李代 ，可以构建无
约束的优化问题，很方 过高斯 顿 、列 伯格—马夸 方
等优化 进行 解。不过， 用高斯 顿 和列 伯格—马夸
方 之前，我 需要知 个误差项关于优化变量的导 ，也 是 线性
化：
 图7-12　重 影误差示意图。

J
这里的 的形式是 得讨论的，甚至可以说是关 所 。我
可以 用 导 ， 果 推导出解析形式，则我 会优先 虑解
析导 。现 ，当 为 e
坐标误差（2 ）， 为相机位 （6 ） x
时，J ×
是一个2 6的矩阵。我 来推导 的形式。 J
忆李代 的 ，我 介 了 用 动 来 李代 的导
。首先，记变 相机坐标 下的空 坐标为 P′ ，并且 其前3
取出来：

么，相机 影 相 于 P′ 为

展开：

用第3行消去 s（ 际上 是P′ 的 离），得：

 这与之前讲的相机 是一致的。当我 误差时，可以 这里的
u,v
与 际的 量 比 ， 差。 义了中 变量后，我 ∧ 左 ξ
乘 动量 δξ e
， 后 虑 的变化关于 动量的导 。 用 式 则，
可以列 下：

这里的 ⊕ 李代 上的左乘 动。第一项是误差关于 影 的导

， 式(7.40)已 列出了变量之 的关 ，易得：

第二项为变 后的 关于李代 的导 ，根 4.3.5 中的推导，

得：

P′ 的 义中，我 取出了前3 ，于是得：

这两项相乘， 得 了2 × 6的雅可比矩阵：
 这个雅可比矩阵 了重 影误差关于相机位 李代 的一阶变化
关 。我 留了前面的负号，这是 为误差是由 义 观测值减预测值
的。 当 也可反过来， 义成“预 观 ”的形式。 种情
下，只要去 前面的负号即可。此 ， 果se(3)的 义方式是旋转
前，平移 后，只要 这个矩阵的前3列与后3列 调即可。
另一方面，除了优化位 ，我 还希望优化 征 的空 位 。
此，需要讨论 关于空 e 的导 P 。所幸这个导 矩阵相 来说 易
一些。仍 用 式 则，有：

第一项 前面已推导，关于第二项， 义

我 发现 P′ P 导后 只剩下R 。于是：

于是，我 推导出了观 相机方程关于相机位 与 征 的两个导

矩阵。 十分重要 ， 优化过程中 重要的梯度方向， 导
优化的 代。

7.8　实践：求解PnP
7.8.1　使用EPnP求解位姿
下面，我 过 验理解一下PnP的过程。首先，我 用OpenCV
的EPnP 解PnP问题， 后 过g2o 果进行优化。由于PnP需要 用3D
，为了 化带来的麻 ，我 用了RGB-D相机中的 度图
（1_depth.png） 为 征 的3D位 。首先来看OpenCV 的PnP
：
 slambook/ch7/pose_estimation_3d2d.cpp（片 ）

程中，得 征 后，我 第一个图的 度图中寻找

的 度，并 出空 位 。以此空 位 为3D ，再以第二个图 的
位 为2D ，调用EPnP 解PnP问题。程序 出 下：
 读 可以 比先前2D-2D情 下 解的 R,t
看看有什么不同。可以看
R
， 有3D 息时，估计的 几乎是相同的， 相差得 t
。这是由
于引 了新的 度 息所致。不过，由于Kinect 集的 度图本 会有
一些误差，所以这里的3D 也不是 确的。我 会希望 位 和所有 ξ
三 征 P
同时优化。

7.8.2　使用BA优化
下面 示 进行BundleAdjustment。我 用前一步的估计
为 。优化可以 用前面讲的Ceres g2o库 现，这里 用g2o
为 。
g2o的 本知识 第6讲中已 介 过了。 用g2o之前，我 要
问题建 成一个最 二乘的图优化问题， 图7-13所示。
 图7-13　PnP的Bundle Adjustment的图优化表示。

这个图优化中， 和 的 下：
1. 节点 ：第二个相机的位 ξ∈ se(3)，所有 征 的空 位
P∈ R 。 3

2.边 ： 个3D 第二个相机中的 影，以观 方程来 ：

由于第一个相机位 为零，我 有 优化变量里，

习题中，希望 第一个相机的位 与观 也 虑进来。现 我
根 一组3D 和第二个图 中的2D 影，估计第二个相机的位 。所
以我 第一个相机画成虚 ，表明不希望 虑 。
g2o 了许 关于BA的 和 ，我 不必 己从 现所有的计
。 g2o/types/sba/types_six_dof_expmap.h中则 了李代 表
的 和 。请读 开这个 件，找 VertexSE3Expmap（李代 位
）、VertexSBAPointXYZ（空 位 ）和EdgeProjectXYZ2UV（ 影
方程 ）这三个 。我 来 单看一下 的 义，
VertexSE3Expmap：
 请 意 的 板参 。第一个参 6表示 存 的优化变量
度，可以看 这是一个6 的李代 。第二个参 是优化变量的 ，这
里 用了g2o 义的相机位 ：SE3Quat。这个 用了四元数加位
移向量来存 位 ， 同时也 李代 上的运 ， 映 （log
）和李代 上 量（update ）等操 。我 可以 的 现
代码，看看g2o 李代 是 操 的：
 这里 不 个 义 过来了。从 板参 可以看 ，空
位 的 度为3， 是 Eigen 的 Vector3D 。 另 一 方 面 ，
EdgeProjectXYZ2UV连 了前面说的两个顶 ， 的观 为2 ，由
Vector2D表示， 际上 是空 的 坐标。 的误差计 表
了 影方程的误差计 方 ，也 是我 前面 z-h ξ,P
的 ( )的方
式。
现 ，进一步观 EdgeProjectXYZ2UV的linearizeOplus 的
现。这里用 了我 前面推导的雅可比矩阵：
 细研究此 代码，我 会发现 与式(7.45)和(7.47)是一致的。
成 员 变 量 “_jacobianOplusXi” 是 误 差 空 的 导
，“_jacobianOplusXj”是误差 相机位 的导 ，以李代 的左乘
动表 。稍有差 的是，g2o的相机里用 一 f
x y ，并且李代 f ,f
义顺序不同（g2o是旋转 前，平移 后；我 是平移 前，旋转
后），所以矩阵前3列和后3列与我 的 义是相反的，此 一致。
得一 的是，我 亦可 己 现相机位 ，并 用
Sophus::SE3来表 位 ， 的 导过程。 ，既 g2o已
了这样的 ， 有额 要 的情 下， 己重新 现 有必要
了。现 ，我 上 一 个 PnP 程 的 础 上 ， 加 上 g2o 的 Bundle
Adjustment。
slambook/ch7/pose_estimation_3d2d.cpp（片 ）
 程序 上和第6讲的g2o 。我 首先 明了g2o图优化，并
优化 解器和梯度下降方 。 后根 估计 的 征 ， 位 和空
图中。最后调用优化 进行 解。读 可以看 优化的 果
下：

代11轮后，列 伯格—马夸 方 发现优化目标 近不

变，于是 止优化。我 出了最后得 的位 变 矩阵 ， 比之前 T
直 PnP的 果， 约 后第3位发生了一些变化。这主要是由
于我 同时优化了 征 和相机位 导致的。
Bundle Adjustment是一种 用的 。 可以不 于两幅图 。我
可以 幅图 匹 的位 和空 进行 代优化，甚至可
以 个SLAM过程 进来。 种 规 ，主要 后端 用，我
会 第10讲再次 这个问题。 前端，我 常 虑 相机位 和
征 的 Bundle Adjustment问题，希望 进行 时 解和优化。

7.9　3D-3D：ICP
 最后，我 来介 3D-3D的位 估计问题。 设我 有一组 的
3D （比 我 两幅RGB-D图 进行了匹 ）：

现 ，想要找一个 变 R,t ， 得：

这个问题可以用 代最近 （Iterative Closest Point，ICP）

解。读 应该 意 了，3D-3D位 估计问题中并 有出现相机 ，也
是说，仅 虑两组3D 之 的变 时，和相机并 有关 。 此，
光SLAM中也会碰 ICP，不过由于 光 征不 丰 ，我 无从知
两个 集之 的 匹配关系
，只 认为 离最近的两个 为同一个，所
以这个方 称为 代最近 。 视觉中， 征 为我 了 的
匹 关 ，所以 个问题 变得更 单了。 RGB-D SLAM中，可以用这
种方式估计相机位 。下 我 用ICP 代 匹配好的
两组 的运动估
计问题。
和PnP ，ICP的 解也分为两种方式： 用 性代 的 解（主
要 是 SVD ） ， 以 及 用 非 性 优 化 方 式 的 解 （ 于 Bundle
Adjustment）。下面分 进行介 。

7.9.1　SVD方法
首先来看以SVD为代表的代 方 。根 前面 的ICP问题，我
先 义第 i
的误差项：

后，构建最 二乘问题， 误差平方和 极 的R,t ：

下面来推导 的 解方 。首先， 义两组 的质心：

 请 意，质心是 有下标的。随后， 误差 中 下的 理：

意 交叉项 分中( pi -p-R (pi ′ -p′ )) 和之后为零， 此

优化目标 可以 化为

细观 左右两项，我 发现左 只和旋转矩阵 相关， 右 R 既

R t
有 也有 ， 只和质心相关。只要我 获得了 ，令第二项为零 R
得 t
。于是，ICP可以分为以下三个步骤 解：
1.计 两组 的质心位 p,p′ ， 后计 个 的 去质心坐标 ：

2.根 以下优化问题计 旋转矩阵：

3.根 第2步的 R计 t：

我 看 ，只要 出了两组 之 的旋转，平移量是非常 易得

的。所以我 重 关 R的计 。展开关于 的误差项，得： R
 意 第一项和 R 无关，第二项由于R R =I ，亦与 R 无关。
T

此， 际上优化目标 变为

下来，我 介 怎样 过SVD解出上 问题中最优的 。关于最优 R

性的证明 为 杂，感兴趣的读 请参 献[50,51]。为了解 ，先 R
义矩阵：

W 是一个3× 3的矩阵， W 进行SVD分解，得：

其中，Σ为 异 组成的 角矩阵， 角 从 列， U

V
和 为 角矩阵。当 W 秩时， 为R

解得R 后， 式(7.53) 解t 即可。

7.9.2　非线性优化方法
解ICP的另一种方式是 用非 性优化，以 代的方式去找最优
。该方 和我 前面讲 的PnP非常相 。以李代 表 位 时，目标
可以 成
 单个误差项关于位 的导 前面已推导， 用李代 动 即
可：

于是， 非 性优化中只需不断 代， 找 极 。 且，可

以证明 [6] ，ICP问题存 唯一解 无穷 解的情 。 唯一解的情
下，只要 找 极 解， 么 这个极小值就是全局最优值
—— 此不
会 极 非 最 的情 。这也意味着ICP 解可以 意
。这是已匹 时 解ICP的一 。
需要说明的是，我 这里讲的ICP是 已由图 征给 了匹 的情
下进行位 估计的问题。 匹 已知的情 下，这个最 二乘问题
际上具有解析解[52,53,54] ，所以并 有必要进行 代优化。ICP的研究
往往更加关心匹 未知的情 。不过， RGB-D SLAM中，由于一个
的 度 可 量不 ，所以我 可以 合着 用PnP和ICP优化：
于 度已知的 征 ，建 的3D-3D误差； 于 度未知的 征
，则建 3D-2D的重 影误差。于是，可以 所有的误差 同一个问
题中 虑， 得 解更加方 。

7.10　实践：求解ICP
7.10.1　SVD方法
下面 示一下 用SVD及非 性优化来 解ICP。本 我 用
两幅RGB-D图 ， 过 征匹 获取两组3D ，最后用ICP计 的位
变 。由于OpenCV目前还 有计 两组带匹 的ICP的方 ， 且
的原理也并不 杂，所以我 己来 现一个ICP。
slambook/ch7/pose_estimation_3d3d.cpp（片 ）
 ICP的 现方式和前 讲 的是一致的。我 调用Eigen进行SVD，
后计 R,t 矩阵。我 出了匹 后的 果，不过请 意，由于前面的
推导是 pi =Rp′ i +t 进行的，这里的 R,t 是第二帧 第一帧的变
，与前面PnP 分是相反的。所以 出 果中，我 同时 印了 变
：
 读 可以比 一下ICP与PnP、 极几 的运动估计 果之 的差
异。可以认为， 这个过程中我 用了越来越 的 息（ 有 度—
有一个图的 度—有两个图的 度）， 此， 度 确的情 下，得
的估计也 越来越 确。 是，由于Kinect的 度图存 ， 且
有可 存 丢 的情 ， 得我 不得不丢弃一些 有 度 的
征 。这可 导致ICP的估计不 确，并且， 果 征 丢弃得太
，可 引起由于 征 太 ，无 进行运动估计的情 。

7.10.2　非线性优化方法
 下面 虑用非 性优化来计 ICP。我 用李代 来表 相机
位 。与SVD思 不同的 方 于， 优化中我 不仅 虑相机的位 ，
同时会优化3D 的空 位 。 我 来说，RGB-D相机 次可以观
标 的三 位 ，从 产生一个3D观 。不过，由于g2o/sba中 有
3D 3D的 ， 我 又想 用g2o/sba中李代 现的位 ，所
以最 的方式是 义一种这样的 ，并向g2o 解析 导方式。
slambook/ch7/pose_estimation_3d3d.cpp
 这 是 一 个 一 ， 于 前 面 的
g2o::EdgeSE3ProjectXYZ，不过观 量从2 变成了3 ， 有相机
，并且只关 一个 。请读 意这里雅可比矩阵的书 ，
必须与我 前面的推导一致。雅可比矩阵给出了关于相机位 的导 ，
是一个3× 6的矩阵。
 调用g2o进行优化的代码是相 的，我 设 图优化的 和 即
可。这 分代码请读 查看 件，这里不再列出。现 ，来看看优化
的 果：

我 发现，只 代一次后总 误差 已 稳 不变，说明仅 一次

代之后 即已 。从位 解的 果可以看出， 和前面SVD给出
的位 果几乎一 一样，这说明SVD已 给出了优化问题的解析解。所
以，本 验中可以认为SVD给出的 果是相机位 的最优 。
需要说明的是， 本 的ICP中，我 用了 两个图 有 度读
的 征 。 事 上，只要其中一个图 度确 ，我 用 于
PnP的误差方式， 也加 优化中来。同时，除了相机位 之 ，
空 也 为优化变量 虑，亦是一种解 问题的方式。我 应当
楚， 际的 解是非常 的，不必 于某种 的形式。 果同时
虑 和相机， 个问题 变得 更自由 了， 可 会得 其 的解。比
，可以让相机 转一些角度， 移动一些。这从另一 面反映
出， Bundle Adjustment里面，我 会希望有尽可 的约束， 为
次观 会带来更 的 息， 我 更 确 估计 个变量。
7.11　小结
本讲介 了 于 征 的视觉里程计中的几个重要的问题。包 ：
1. 征 是 取并匹 的。
2. 过2D-2D的 征 估计相机运动。
3. 从2D-2D的匹 估计一个 的空 位 。
4.3D-2D的PnP问题，其 性解 和Bundle Adjustment解 。
5.3D-3D的ICP问题，其 性解 和Bundle Adjustment解 。
本讲 为丰 ，且 合应用了前几讲的 本知识。读 觉得
理解有 难，可以 前面知识稍加 顾。最 亲 一 验，以理解
个运动估计的 。
需要解 的是，为 证行 畅，我 省略了 量关于某些 情
的讨论。 ， 果 极几 解过程中给 的 征 面，会发
生什么情 （这 单应矩阵 中 了）？ H又会发生什么情 ？
PnP和ICP中 给 这样的解，又会导致什么情 ？ 解 否识 这
些 的情 ，并报告所得的解可 不可靠？尽 是 得研究和
的， 的讨论势必让本书变得 琐。 且 工程 现
中，这些情 甚 出现，所以本书介 的方 ，是 际工程中
有 运行的方 ，我 了 些 见的情 并不发生。 果 关心这
些 见的情 ，可以 读[3]等论 ， 献中我 常会研究一些
情 下的解 方案。
习题
1.除了本书介 的ORB 征 ， 还 找 哪些 征 ？请说说
SIFT SURF的原理，并 比 与ORB之 的优劣。
2.设计程序调用OpenCV中的其 种 征 。 计 取1000个
征 时 的机器上所用的时 。
3.*我 发现，OpenCV 的ORB 征 图 当中分布不 匀。
是否 找 出让 征 分布更加 匀的方 ？
4.研究FLANN为 快速 理匹 问题。除了FLANN之 ，还有哪
些可以加速匹 的 ？
 5. 示程序 用的EPnP 成其 PnP方 ，并研究 的工 原
理。
6. PnP优化中， 第一个相机的观 也 虑进来，程序应 书
？最后 果会有 变化？
7. ICP程序中， 空 也 为优化变量 虑进来，程序应 书
？最后 果会有 变化？
8.* 征 匹 过程中，不可 会 误匹 的情 。 果我
误匹 PnP ICP中，会发生怎样的情 ？ 想 哪些
误匹 的方 ？
9.* 用Sophus的SE3 ， 己设计g2o的 与 ， 现PnP和ICP的
优化。
10.* Ceres中 现PnP和ICP的优化。

[1] 字 是 图 进行不同层次的降 样，以获得不同分 率的图 。

[2] 也 是说， 等式一 乘以 意非零常 时，我 认为等式仍是成立的。

[3] SfM研究中则有可 是未知 有待估计的。

[4] 请 意，这和SE(3)中的变 矩阵 是不同的。 T

ξ P × K ×
[5] exp( ∧ ) i 果是4 1的， 其左 的 是3 3的，所以必须 exp( ξ∧
P
) i 的前三 取出来，变成三 的非齐次坐标。这 前面讲过。
样章 此 束

需要 版

扫下面二 码

加微 ：shuyou099

领取
 附录B　ROS入门
ROS是机器人研究领 一个广为 讨的主题。为了 本书 读
太高，我 有 正 和 程中 。 近年来，ROS正逐步
各 高校的 生中 得 推广， 为人 所 知和 受，所以这
里也介 一下ROS，希望 读 有所帮助。
B.1ROS是什么
ROS（Robot Operating System）是Willow Garage公司于2007年
发布的一个开 机器人操 ， 为软件开发人员开发机器人应用
程序 了许 优秀的工具和库。同时，还有优秀的开发 不断 为
贡献代码。本质上，ROS并不是一个真正意义上的操 ， 更
是 于操 之上的一个软件包。 了众 际机器人中可
的 ：导 、 、 径规 ，等等。
ROS的版本代号是 字 顺序来 的，并随着Ubuntu 发布
更新。 常一个ROS版本会 两 三个Ubuntu 版本。ROS从Box
Turtle开 ， 止 本书 时（2016年），已 更新 了Kinetic
Kame（见图B-1）。同时，ROS也已 彻底重构，推出了 时性更强的
2.0版本。
ROS 很 操 ， 最 善的是Ubuntu及其衍生版本
（Kubuntu、Linux Mint、Ubuntu GNOME等）， 其 Linux发布版
本、Windows等的 也有，不过 有 么 善。 此，推荐读 用
Ubuntu操 来进行开发和研究。
ROS 目前被广 用的面向 象的 程语言C++，以及 本语
言Python。 可以 己喜欢的语言进行开发。
 图B-1　ROS各版本命名方式

B.2ROS的特点
ROS的设计 衷， 是 机器人开发 计 机开发一样，屏蔽
底层硬件及其 口的不一致性，最终 得软件可以 用。
软件 用也正是软件工程优 性最集中的 现之一，ROS 以
一消息格式来 得 只需要关 层面的设计， 底层硬件的
根本目的是 各种各样的消息， 图 、 等。各个硬件厂商
的 一 ROS所规 的 一消息格式下，即可让用 方
用各种开 的机器人相关 。
第 14 讲 中 的 常 见 的 开 SLAM 方 案 中 ， ORB-SLAM 、 ORB-
SLAM2 、 LSDSLAM 、 SVO 、 DVO 、 RTAB-MAP 、 RGBD-SLAM-V2 、 Hector
SLAM、Gmapping、ROVIO等 有ROS版本的开 代码， 可以很方
ROS中运行、调试和修 。
调试SLAM程序时， 的来 常有3种： 感器、 集，以
及bag 件。 有相应的 感器， 常 需要 用虚 的 来
跑 SLAM 程序 。其 中 ， 最 方 的方式当属 用ROS下的bag 件发 布
topic， 后SLAM程序 可以监视topic发出的 ， 用真 的
感器 集 一样。后面我 会 单介 一下 用bag 件来
真 的 感器 。
B.3如何快速上手ROS
 ROS有 善的 。 ， 的介 机器上安装 应
版 本的 ROS： http://wiki.ros.org/ROS/Installation； 后， 读
ROS 带的 程序即可。 会 习 ROS的 本 念、主题的发布和
订 ，以及用Python和C++控制 。 果 觉得麻 ，也可以 用
ROS 制 的 Ubuntu ：
http://www.aicrobo.com/ubuntu_for_ros.html。
除了 本知识之 ， 还可以 一些ROS的常用工具， ：
1.rqt。rqt是ROS下的一个软件框架， 以 件的方式 了各种
各样方 用的GUI（用 图形界面）。rqt的功 非常强 ，可以
时 查看ROS中 动的消息。
2.rosbag。rosbag是ROS 的一个非常 用的录制及 topic
的工具。当 想 际跑一下SLAM程序， 囿于 有 际的
感器时，可以 虑 用公开 的bag 件来进行图 的
，这种方式与 用一个真 的 感器 感觉上并无不同。rosbag的
用方式请参 ROS的 页面。此 ，许 公开 集也会 bag
格式的 件。
3.rviz。rviz是ROS 的可视化 ， 可以 过 时 查看
ROS中的图 、 云、 图、规 的 径，等等，从 更方 调试程
序。
我 相 ，机器人的硬件层面和软件层面一 会向着 一架构
的方向前行， ROS正是软件架构层面标 化一个重要的里程碑。其
中，ROS 1.x 之前被 量用于 验 的研究， 公司产品demo的研
发阶 ， ROS2则解 了ROS 时性的问题，未来很有可 被直 用于
际产品的研发，为推进工业级机器人和服务机器人的应用 出重要
的贡献。
本附录 性 介 了有关ROS的历史、优 ，以及 用ROS
中的一些可视化工具来 助SLAM程序开发等。我 希望读
习ROS，并 用ROS开发 己的SLAM程序。
 参考文献
[1]L.Haomin,Z.Guofeng,andB.Hujun,“Asurveyofmonocularsimu
ltaneouslocalizationandmapping,” Journal of Computer-Aided
Design and Compute Graphics ,vol.28,no.6,pp.855–868,2016.in
Chinese.
[2]A.Davison,I.Reid,N.Molton,and
O.Stasse,“Monoslam:Real-time single camera IEEE
SLAM,”
Transactions on Pattern Analysis and Machine Intelligence
,vol.29,no.6,pp.1052–1067,2007.
[3]R.Hartley and A.Zisserman, Multiple View Geometry in
Computer Vision .Cambridge university press,2003.
[4]R.C.Smith and P.Cheeseman,“On the representation and
estimation of spatial uncertainty,” International Journal of
Robotics Research ,vol.5,no.4,pp.56–68,1986.
[5]S.Thrun,W.Burgard,and D.Fox,Probabilistic robotics
.MIT Press,2005.
[6]T.Barfoot,“State estimation for robotics:A matrix lie
group approach,”2016.
[7]A.Pretto,E.Menegatti,andE.Pagello,“Omnidirectionalden
selarge-scalemappingandnavigation based on meaningful
triangulation,” 2011 IEEE International Conference on
Robotics and Automation(ICRA 2011) ,pp.3289–96,2011.
[8]B.RueckauerandT.Delbruck,“Evaluationofevent-
basedalgorithmsforoptical fl owwithgroundtruth from inertial
measurement sensor,” Frontiers in neuroscience,vol.10,2016.
[9]C.Cesar,L.Carlone,H.C.,Y.Latif,D.Scaramuzza,J.Neira,I.
D.Reid,andL.JohnJ.,“Past,present,and future of simultaneous
localization and mapping:Towards the robust-perception
age,” arXiv preprint arXiv:1606.05830 ,2016.
[10]P.Newman and K.Ho,“Slam-loop closing with visually
salient features,”in proceedings of the 2005 IEEE
International Conference on Robotics and Automation
,pp.635–
642,IEEE,2005.
[11]R.Smith,M.Self,and P.Cheeseman,“Estimating uncertain
spatial relationships in robotics,”in Autonomous robot
vehicles ,pp.167–193,Springer,1990.
[12]P.Beeson,J.Modayil,andB.Kuipers,“Factoringthemapping
problem:Mobilerobotmap-building in the hybrid spatial
semantic hierarchy,” International Journal of Robotics
Research ,vol.29,no.4,pp.428–459,2010.
[13]H.Strasdat,J.M.Montiel,and A.J.Davison,“Visual
slam:Why fi lter?,” Image and Vision Computing
,vol.30,no.2,pp.65–77,2012.
[14]M.Liang,H.Min,and R.Luo,“Graph-based slam:A
survey,” ROBOT
,vol.35,no.4,pp.500–512,2013.in Chinese.
[15]J.Fuentes-Pacheco,J.Ruiz-Ascencio,and J.M.Rendón-
Mancha,“Visual simultaneous localization and mapping:a
survey,” Arti fi cial Intelligence Review,vol.43,no.1,pp.55
–81,2015.
[16]J.Boal,Á.Sánchez-Miralles,and Á.Arranz,“Topological
simultaneous localization and mapping:a survey,” Robotica
,vol.32,pp.803–821,2014.
[17]S.Y.Chen,“Kalman fi lter for robot vision:A
survey,” IEEE Transactions on Industrial Electronics
,vol.59,no.11,pp.4409–4420,2012.
[18]Z.Chen,J.Samarabandu,andR.Rodrigo,“Recentadvancesins
imultaneouslocalizationandmapbuilding using computer
vision,” Advanced Robotics ,vol.21,no.3-4,pp.233–265,2007.
[19]J.Stuelpnagel,“On the parametrization of the three-
dimensional rotation group,” SIAM Review
,vol.6,no.4,pp.422–
430,1964.
[20]V.S.Varadarajan, Lie groups,Lie algebras,and their
representations ,vol.102.Springer Science&Business
Media,2013.
[21]H.Strasdat, Localaccuracyandglobalconsistencyfore ffi
cientvisualslam .PhDthesis,Citeseer,2012.
[22]S.Agarwal,N.Snavely,I.Simon,S.M.Seitz,and
R.Szeliski,“Building rome in a day,”in2009 IEEE 12th
international conference on computer vision ,pp.72–
79,IEEE,2009.
[23]J.Nocedal and S.Wright, Numerical Optimization
.Springer Science&Business Media,2006.
[24]M.I.Lourakis and A.A.Argyros,“Sba:A software package
for generic sparse bundle adjustment,” ACM Transactions on
Mathematical Software(TOMS) ,vol.36,no.1,p.2,2009.
[25]G.Sibley,“Relativebundleadjustment,” DepartmentofEng
ineeringScience,OxfordUniversity,Tech.Rep
,vol.2307,no.09,2009.
[26]B.Triggs,P.F.McLauchlan,R.I.Hartley,and
A.W.Fitzgibbon,“Bundle adjustment:a modern synthesis,”in
Vision algorithms:theory and practice
,pp.298–
372,Springer,2000.
[27]S.Agarwal,K.Mierle,and Others,“Ceres
solver.”http://ceres-solver.org.
[28]R.Kummerle,G.Grisetti,H.Strasdat,K.Konolige,and
W.Burgard,“G2o:a general framework for graph
optimization,”in IEEE International Conference on Robotics
and Automation(ICRA) ,pp.3607–3613,IEEE,2011.
[29]Wikipedia,“Feature(computervision).”"https://en.wik
ipedia.org/wiki/Feature_(computer_vision)",2016.
[Online;accessed 09-July-2016].
[30]D.G.Lowe,“Distinctive image features from scale-
invariant keypoints,” International Journal of Computer
Vision ,vol.60,no.2,pp.91–110,2004.
[31]H.Bay,T.Tuytelaars,and L.Van Gool,“Surf:Speeded up
robust features,”in Computer Vision–ECCV 2006,pp.404–
417,Springer,2006.
[32]E.Rublee,V.Rabaud,K.Konolige,and G.Bradski,“Orb:an e
ffi cient alternative to siftor surf,”in 2011 IEEE
International Conference on Computer Vision(ICCV) ,pp.2564–
2571,IEEE,2011.
[33]E.Rosten and T.Drummond,“Machine learning for high-
speed corner detection,”in European conference on computer
vision ,pp.430–443,Springer,2006.
[34]M.Calonder,V.Lepetit,C.Strecha,and
P.Fua,“Brief:Binary robust independent elementary
features,”in European conference on computer vision ,pp.778
–792,Springer,2010.
[35]P.L.Rosin,“Measuringcornerproperties,” ComputerVisio
nandImageUnderstanding,vol.73,no.2,pp.291–307,1999.
[36]M.Muja and D.G.Lowe,“Fast approximate nearest
neighbors with automatic algorithm con fi guration.,”in
VISAPP(1) ,pp.331–340,2009.
[37]J.Sturm,N.Engelhard,F.Endres,W.Burgard,and
D.Cremers,“A benchmark for the evaluation of rgb-d SLAM
systems,”in 2012 IEEE/RSJ International Conference on
Intelligent Robots and Systems(IROS) ,pp.573–580,IEEE,2012.
[38]R.I.Hartley,“In defense of the eight-point
algorithm,” IEEE Transactions on pattern analysis and machine
intelligence ,vol.19,no.6,pp.580–593,1997.
[39]H.C.Longuet-Higgins,“A computer algorithm for
reconstructing a scene from two projections,” Readings in
Computer Vision:Issues,Problems,Principles,and Paradigms,MA
Fischler and O.Firschein,eds ,pp.61–62,1987.
[40]H.Li and R.Hartley,“Five-point motion estimation
made easy,”in 18th International Conference on Pattern
Recognition(ICPR’06),vol.1,pp.630–633,IEEE,2006.
[41]D.Nistér,“An e ffi cient solution to the fi ve-point
relative pose problem,” IEEE Transactions on Pattern Analysis
and Machine Intelligence,vol.26,no.6,pp.756–770,2004.
[42]O.D.Faugeras and F.Lustman,“Motion and structure
from motion in a piecewise planar
environment,” InternationalJournal ofPattern
RecognitionandArti fi cial Intelligence ,vol.2,no.03,pp.485–
508,1988.
[43]Z.Zhang and A.R.Hanson,“3d reconstruction based on
homography mapping,” ARPA Image Understanding Workshop
,pp.1007–1012,1996.
[44]E.Malis and Deeper understanding of the
M.Vargas,
homography decomposition for vision-based control .PhD
thesis,INRIA,2007.
[45]X.-S.Gao,X.-R.Hou,J.Tang,and H.-F.Cheng,“Complete
solution classi fi cation for the perspective-three-point
problem,” IEEE Transactions on Pattern Analysis and Machine
Intelligence ,vol.25,pp.930–943,Aug 2003.
 [46]V.Lepetit,F.Moreno-Noguer,and P.Fua,“Epnp:An
accurate o(n)solution to the pnp problem,” International
Journal of Computer Vision ,vol.81,no.2,pp.155–166,2008.
[47]A.Penate-Sanchez,J.Andrade-Cetto,and F.Moreno-
Noguer,“Exhaustive linearization for robust camera pose and
focal length estimation,” IEEE Transactions on Pattern
Analysis and Machine Intelligence ,vol.35,no.10,pp.2387–
2400,2013.
[48]L.Chen,C.W.Armstrong,and D.D.Raftopoulos,“An
investigation on the accuracy of threedimensional space
reconstruction using the direct linear transformation
technique,” Journal of Biomechanics ,vol.27,no.4,pp.493–
500,1994.
[49]iplimage,“P3p(blog).”"http://iplimage.com/blog/p3p-
perspective-point-overview/",2016.
[50]K.S.Arun,T.S.Huang,and S.D.Blostein,“Least-squares
fi tting of two 3-d point sets,” Pattern Analysis and Machine
Intelligence,IEEE Transactions on,no.5,pp.698–700,1987.
[51]F.Pomerleau,F.Colas,and R.Siegwart,“A review of
point cloud registration algorithms for mobile
robotics,” Foundations and Trends in Robotics(FnTROB)
,vol.4,no.1,pp.1–104,2015.
[52]O.D.Faugeras and M.Hebert,“The
representation,recognition,and locating of 3-d objects,” The
International Journal of Robotics Research ,vol.5,no.3,pp.27
–52,1986.
[53]B.K.Horn,“Closed-form solution of absolute
orientation using unit quaternions,” JOSA A
,vol.4,no.4,pp.629–642,1987.
[54]G.C.Sharp,S.W.Lee,andD.K.Wehe,“Icpregistrationusingi
nvariantfeatures,” IEEETransactions on Pattern Analysis and
Machine Intelligence ,vol.24,no.1,pp.90–102,2002.
[55]G.Silveira,E.Malis,and P.Rives,“An e ffi cient
direct approach to visual slam,” IEEE Transactions on
Robotics ,vol.24,no.5,pp.969–979,2008.
[56]C.Forster,M.Pizzoli,and D.Scaramuzza,“Svo:Fast semi-
direct monocular visual Robotics and
odometry,”in
Automation(ICRA),2014 IEEE International Conference on
(rs,ed.),pp.15–22,IEEE,2014.
[57]J.Engel,T.Schöps,and D.Cremers,“Lsd-slam:Large-scale
direct monocular slam,”in Computer Vision–ECCV 2014
,pp.834
–849,Springer,2014.
[58]J.Engel,V.Koltun,and D.Cremers,“Direct sparse
odometry,” arXiv preprint arXiv:1607.02565,2016.
[59]J.Engel,J.Sturm,and D.Cremers,“Semi-dense visual
Proceedings of the IEEE
odometry for a monocular camera,”in
International Conference on Computer Vision ,pp.1449–
1456,2013.
[60]V.Usenko,J.Engel,J.Stueckler,and D.Cremers,“Direct
visual-inertial odometry with stereo IEEE
cameras,”in
International Conference on Robotics and Automation(ICRA)
,May 2016.
[61]Wikipedia,“Randomsampleconsensus.”"https://en.wikip
edia.org/wiki/Random_sample_consensus",2016.[Online;accessed
09-July-2016].
[62]V.Sujan and S.Dubowsky,“E ffi cient information-
based visual robotic mapping in unstructured
environments,” International Journal of Robotics Research
,vol.24,no.4,pp.275–293,2005.
[63]F.Janabi-Shari fi and M.Marey,“A kalman- fi lter-
based method for pose estimation in visual servoing,” IEEE
Transactions on Robotics ,vol.26,no.5,pp.939–947,2010.
[64]S.Li and P.Ni,“Square-root unscented kalman fi lter
based simultaneous localization and mapping,”in Information
and Automation(ICIA),2010 IEEE International Conference on
,pp.2384–2388,IEEE,2010.
[65]R.Sim,P.Elinas,and J.Little,“A study of the rao-
blackwellised particle fi lter for e ffi cient and accurate
vision-based slam,” International Journal of Computer Vision
,vol.74,no.3,pp.303–318,2007.
[66]J.S.Lee,S.Y.Nam,and W.K.Chung,“Robust rbpf-slam for
indoor mobile robots using sonar sensors in non-static
environments,” Advanced Robotics ,vol.25,no.9-10,pp.1227–
1248,2011.
[67]A.Gil,O.Reinoso,M.Ballesta,and M.Julia,“Multi-robot
visual slam using a rao-blackwellized particle fi
lter,” Robotics and Autonomous Systems ,vol.58,no.1,pp.68–
80,2010.
[68]G.Sibley,L.Matthies,and G.Sukhatme,“Sliding window
fi lter with application to planetary landing,” Journal of
Field Robotics ,vol.27,no.5,pp.587–608,2010.
[69]L.M.Paz,J.D.Tardós,and J.Neira,“Divide and
conquer:Ekf slam in o(n),” IEEE Transactions on Robotics
,vol.24,no.5,pp.1107–1120,2008.
[70]O.G.Grasa,J.Civera,and J.Montiel,“Ekf monocular slam
with relocalization for laparoscopic sequences,”in Robotics
and Automation(ICRA),2011 IEEE International Conference on
,pp.4816–4821,IEEE,2011.
[71]E.Süli and D.F.Mayers, An Introduction to Numerical
Analysis .Cambridge university press,2003.
 [72]L.Polok,V.Ila,M.Solony,P.Smrz,and
P.Zemcik,“Incremental block cholesky factorization for
nonlinear least squares in robotics.,”in Robotics:Science
and Systems ,2013.
[73]R.Mur-Artal,J.Montiel,and J.D.Tardos,“Orb-slam:a
versatile and accurate monocular slam system,” arXiv preprint
arXiv:1502.00956 ,2015.
[74]S.Leutenegger,S.Lynen,M.Bosse,R.Siegwart,and
P.Furgale,“Keyframe-based visual–inertial odometry using
nonlinear optimization,” The International Journal of
Robotics Research ,vol.34,no.3,pp.314–334,2015.
[75]“Bundle adjustment in the
large.”http://grail.cs.washington.edu/projects/bal/.
[76]J.Sherman and W.J.Morrison,“Adjustment of an inverse
matrix corresponding to a change in one element of a given
matrix,” The Annals of Mathematical Statistics
,vol.21,no.1,pp.124–127,1950.
[77]H.Strasdat,A.J.Davison,J.M.M.Montiel,and
K.Konolige,“Double window optimisation for constant time
visual SLAM,” 2011 IEEE International Conference On Computer
Vision(ICCV) ,pp.2352–2359,2011.
[78]G.Dubbelman and B.Browning,“Cop-slam:Closed-form
online pose-chain optimization for visual
slam,” Robotics,IEEE Transactions on ,vol.31,pp.1194–
1213,Oct 2015.
[79]D.Lee and H.Myung,“Solution to the slam problem in
low dynamic environments using a pose graph and an rgb-d
sensor,” Sensors ,vol.14,no.7,pp.12467–12496,2014.
[80]Y.Latif,C.Cadena,and J.Neira,“Robust loop closing
over time for pose graph slam,” The International Journal of
Robotics Research ,vol.32,no.14,pp.1611–1626,2013.
 [81]G.Klein and D.Murray,“Parallel tracking and mapping
for small ar workspaces,”in Mixed and Augmented
Reality,2007.ISMAR 2007.6th IEEE and ACM International
Symposium on ,pp.225–234,IEEE,2007.
[82]D.Koller and N.Friedman,Probabilistic graphical
models:principles and techniques .MIT press,2009.
[83]M.Kaess,A.Ranganathan,and
F.Dellaert,“isam:Incremental smoothing and mapping,” IEEE
Transactions on Robotics ,vol.24,no.6,pp.1365–1378,2008.
[84]M.Kaess,H.Johannsson,R.Roberts,V.Ila,J.J.Leonard,and
F.Dellaert,“isam2:Incremental smoothing and mapping using
the bayes tree,” The International Journal of Robotics
Research ,p.0278364911430419,2011.
[85]D.M.Rosen,M.Kaess,and J.J.Leonard,“Rise:An
incremental trust-region method for robust onlinesparseleast-
squaresestimation,” IEEETransactionsonRobotics
,vol.30,no.5,pp.1091–1108,2014.
[86]J.Sola,“Course on
slam.”https://github.com/joansola/slamtb/raw/graph/courseSLA
M.pdf,2016.
[87]F.Dellaert,“Factor graphs and gtsam:A hands-on
introduction,”2012.
[88]F.Endres,J.Hess,J.Sturm,D.Cremers,andW.Burgard,“3-
dmappingwithanrgb-dcamera,” IEEE Transactions on Robotics
,vol.30,no.1,pp.177–187,2014.
[89]D.Hahnel,W.Burgard,D.Fox,and S.Thrun,“An e ffi cient
fastslam algorithm for generating maps of large-scale cyclic
environments from raw laser range measurements,”in
Intelligent Robots and Systems,2003.
(IROS2003).Proceedings.2003IEEE/RSJInternationalConferenceon
,vol.1,pp.206–211,IEEE,2003.
 [90]I.Ulrich and I.Nourbakhsh,“Appearance-based place
recognition for topological localization,”in Robotics and
Automation,2000.Proceedings.ICRA’00.IEEE International
Conference on ,vol.2,pp.1023–1029,Ieee,2000.
[91]X.Gao and T.Zhang,“Robust rgb-d simultaneous
localization and mapping using planar point
features,” Robotics and Autonomous Systems ,vol.72,pp.1–
14,2015.
[92]S.Lloyd,“Leastsquaresquantizationinpcm,” IEEEtransac
tionsoninformationtheory,vol.28,no.2,pp.129–137,1982.
[93]D.Arthur and S.Vassilvitskii,“K-means++:The
advantages of careful seeding,”in Proceedings of the
eighteenth annual ACM-SIAM symposium on Discrete algorithms
,pp.1027–1035,Society for Industrial and Applied
Mathematics,2007.
[94]M.Cummins and P.Newman,“Fab-map:Probabilistic
localization and mapping in the space of appearance,” The
International Journal of Robotics Research
,vol.27,no.6,pp.647–665,2008.
[95]M.Cummins and P.Newman,“Accelerating fab-MAP with
concentration inequalities,” IEEE Transactions On Robotics
,vol.26,no.6,pp.1042–1050,2010.
[96]M.CumminsandP.Newman,“Appearance-
onlyslamatlargescalewithfab-map2.0,” International Journal of
Robotics Research ,vol.30,no.9,pp.1100–1123,2011.
[97]C.ChowandC.Liu,“Approximatingdiscreteprobabilitydist
ributionswithdependencetrees,” IEEE transactions on
Information Theory ,vol.14,no.3,pp.462–467,1968.
[98]D.Galvez-Lopez and J.D.Tardos,“Bags of binary words
for fast place recognition in image sequences,” IEEE
Transactions On Robotics,vol.28,no.5,pp.1188–1197,2012.
 [99]J.L.Bentley,“Multidimensionalbinarysearchtreesusedfo
rassociativesearching,” Communications of the ACM
,vol.18,no.9,pp.509–517,1975.
[100]J.SivicandA.Zisserman,“Videogoogle:Atextretrievalap
proachtoobjectmatchinginvideos,”in Computer
Vision,2003.Proceedings.Ninth IEEE International Conference
on ,pp.1470–1477,IEEE,2003.
[101]S.Robertson,“Understandinginversedocumentfrequency:
ontheoreticalargumentsforidf,” Journal of documentation
,vol.60,no.5,pp.503–520,2004.
[102]D.Nister and H.Stewenius,“Scalable recognition with
2006 IEEE Computer Society Conference
a vocabulary tree,”in
on Computer Vision and Pattern Recognition(CVPR’06)
,vol.2,pp.2161–2168,IEEE,2006.
[103]C.Cadena,D.Galvez-Lopez,J.D.Tardos,and
J.Neira,“Robust place recognition with stereo
sequences,” IEEE Transactions on Robotics ,vol.28,no.4,pp.871
–885,2012.
[104]X.Gao and T.Zhang,“Loop closure detection for
visual slam systems using deep neural networks,”in Control
Conference(CCC),2015 34th Chinese,pp.5851–5856,IEEE,2015.
[105]X.GaoandT.Zhang,“Unsupervisedlearningtodetectloopsu
singdeepneuralnetworksforvisual slam system,” Autonomous
Robots ,pp.1–18,2015.
[106]B.Williams,M.Cummins,J.Neira,P.Newman,I.Reid,andJ.Ta
rdós,“Acomparisonofloopclosing techniques in monocular
slam,” Robotics and Autonomous Systems,vol.57,no.12,pp.1188
–1197,2009.
[107]M.LabbéandF.Michaud,“Onlinegloballoopclosuredetecti
onforlarge-scalemulti-
sessiongraphbasedslam,”in 2014IEEE/RSJInternationalConference
onIntelligentRobotsandSystems ,pp.2661–2666,IEEE,2014.
[108]R.F.Salas-
Moreno,R.A.Newcombe,H.Strasdat,P.H.J.Kelly,and
A.J.Davison,“Slam++:Simultaneous localisation and mapping at
the level of objects,” 2013 IEEE Conference on Computer
Vision and Pattern Recognition(CVPR) ,pp.1352–9,2013.
[109]M.Pizzoli,C.Forster,andD.Scaramuzza,“Remode:Probabi
listic,monoculardensereconstruction in real time,”in 2014
IEEE International Conference on Robotics and
Automation(ICRA) ,pp.2609–2616,IEEE,2014.
[110]“Correlation based similarity measure-
summary.”https://siddhantahuja.wordpress.com/tag/stereo-
matching/.
[111]H.HirschmullerandD.Scharstein,“Evaluationofcostfunc
tionsforstereomatching,”in 2007IEEE Conference on Computer
Vision and Pattern Recognition ,pp.1–8,IEEE,2007.
[112]G.VogiatzisandC.Hernández,“Video-based,real-
timemulti-viewstereo,” ImageandVisionComputing
,vol.29,no.7,pp.434–441,2011.
[113]A.Handa,R.A.Newcombe,A.Angeli,and
A.J.Davison,“Real-time camera tracking:When is high frame-
rate best?,”in European Conference on Computer Vision
,pp.222–235,Springer,2012.
[114]J.Montiel,J.Civera,andA.J.Davison,“Uni fi
edinversedepthparametrizationformonocularslam,” analysis
,vol.9,p.1,2006.
[115]J.Civera,A.J.Davison,and J.M.Montiel,“Inverse depth
parametrization for monocular slam,” IEEE transactions on
robotics ,vol.24,no.5,pp.932–945,2008.
 [116]M.Kazhdan,M.Bolitho,and H.Hoppe,“Poisson surface
reconstruction,”in Proceedings of the fourth Eurographics
symposium on Geometry processing ,vol.7,2006.
[117]J.Stuckler and S.Behnke,“Multi-resolution surfel
Journal
maps for e ffi cient dense 3d modeling and tracking,”
of Visual Communication and Image Representation
,vol.25,no.1,pp.137–147,2014.
[118]A.Hornung,K.M.Wurm,M.Bennewitz,C.Stachniss,and
W.Burgard,“Octomap:An e ffi cient
probabilistic3dmappingframeworkbasedonoctrees,” AutonomousRob
ots ,vol.34,no.3,pp.189–206,2013.
[119]M.Burri,H.Oleynikova,M.W.Achtelik,and
R.Siegwart,“Real-time visual-inertial mapping,re-
localization and planning onboard mavs in unknown
environments,”in Intelligent Robots and Systems(IROS),2015
IEEE/RSJ International Conference on ,pp.1872–
1878,IEEE,2015.
[120]R.A.Newcombe,A.J.Davison,S.Izadi,P.Kohli,O.Hilliges,
J.Shotton,D.Molyneaux,S.Hodges,D.Kim,and
A.Fitzgibbon,“Kinectfusion:Real-time dense surface mapping
and tracking,”in 2011 10th IEEE international symposium on
Mixed and augmented reality(ISMAR) ,pp.127–136,IEEE,2011.
[121]R.A.Newcombe,D.Fox,and
S.M.Seitz,“Dynamicfusion:Reconstruction and tracking of
nonrigid scenes in real-time,”in Proceedings of the IEEE
conference on computer vision and pattern recognition ,pp.343
–352,2015.
[122]T.Whelan,S.Leutenegger,R.F.Salas-
Moreno,B.Glocker,and A.J.Davison,“Elasticfusion:Dense slam
without a pose graph,” Proc.Robotics:Science and
Systems,Rome,Italy ,2015.
 [123]M.Dou,S.Khamis,Y.Degtyarev,P.Davidson,S.R.Fanello,A.
Kowdle,S.O.Escolano,C.Rhemann,D.Kim,J.Taylor, et al.
,“Fusion4d:real-time performance capture of challenging
scenes,” ACM Transactions on Graphics(TOG)
,vol.35,no.4,p.114,2016.
[124]M.Innmann,M.Zollhöfer,M.Nießner,C.Theobalt,and
M.Stamminger,“Volumedeform:Realtime volumetric non-rigid
reconstruction,” arXiv preprint arXiv:1603.08161
,2016.
[125]A.J.Davison,“Real-
C
timesimultaneouslocalisationandmappingwithasinglecamera,”in
omputer Vision,2003.Proceedings.Ninth IEEE International
Conference on ,pp.1403–1410,IEEE,2003.
[126]C.Kerl,J.Sturm,andD.Cremers,“Robustodometryestimati
onforrgb-dcameras,”in Roboticsand Automation(ICRA),2013 IEEE
International Conference on ,pp.3748–3754,IEEE,2013.
[127]C.Kerl,J.Sturm,and D.Cremers,“Dense visual slam for
rgb-d cameras,”in 2013 IEEE/RSJ International Conference on
Intelligent Robots and Systems ,pp.2100–2106,IEEE,2013.
[128]J.Gui,D.Gu,S.Wang,and H.Hu,“A review of visual
inertial odometry from fi ltering and optimisation
perspectives,” Advanced Robotics
,vol.29,pp.1289–1301,Oct 18
2015.
[129]A.Martinelli,“Closed-formsolutionofvisual-
inertialstructurefrommotion,” InternationalJournal of
Computer Vision ,vol.106,no.2,pp.138–152,2014.
[130]M.Bloesch,S.Omari,M.Hutter,and R.Siegwart,“Robust
visual inertial odometry using a direct ekf-based
approach,”in Intelligent Robots and Systems(IROS),2015
IEEE/RSJ International Conference on ,pp.298–304,IEEE,2015.
[131]M.Li and A.I.Mourikis,“High-precision,consistent
ekf-based visual-inertial odometry,” International Journal of
Robotics Research ,vol.32,pp.690–711,MAY 2013.
[132]G.Huang,M.Kaess,andJ.J.Leonard,“Towardsconsistentvi
2014IEEE International Conference
sual-inertialnavigation,”in
on Robotics and Automation(icra) ,IEEE International
Conference on RoboticsandAutomationICRA,pp.4926–
4933,2014.IEEEInternationalConferenceonRobotics and
Automation(ICRA),Hong Kong,PEOPLES R CHINA,MAY 31-JUN
07,2014.
[133]C.Forster,L.Carlone,F.Dellaert,andD.Scaramuzza,“Imu
preintegrationonmanifoldfore ffi cient visual-inertial
maximum-a-posteriori estimation,”in Robotics:Science and
Systems XI ,no.EPFLCONF-214687,2015.
[134]M.Tkocz and K.Janschek,“Towards consistent state
and covariance initialization for monocular slam fi
lters,” Journal of Intelligent&Robotic Systems
,vol.80,pp.475
–489,DEC 2015.
[135]A.NüchterandJ.Hertzberg,“Towardssemanticmapsformobi
lerobots,” RoboticsandAutonomous Systems
,vol.56,no.11,pp.915
–926,2008.
[136]J.Civera,D.Gálvez-López,L.Riazuelo,J.D.Tardós,and
J.Montiel,“Towards semantic slam using a monocular
camera,”in Intelligent Robots and Systems(IROS),2011
IEEE/RSJ International Conference on ,pp.1277–
1284,IEEE,2011.
[137]H.S.Koppula,A.Anand,T.Joachims,and
A.Saxena,“Semantic labeling of 3d point clouds for indoor
scenes,”in Advances in Neural Information Processing Systems
,pp.244–252,2011.
[138]A.Anand,H.S.Koppula,T.Joachims,and
A.Saxena,“Contextually guided semantic labeling and search
for three-dimensional point clouds,” The International
Journal of Robotics Research ,p.0278364912461538,2012.
[139]N.Fioraio and L.Di Stefano,“Joint
detection,tracking and mapping by semantic bundle
adjustment,” 2013 IEEE Conference on Computer Vision and
Pattern Recognition(CVPR) ,pp.1538–45,2013.
[140]R.F.Salas-Moreno,B.Glocken,P.H.Kelly,and
A.J.Davison,“Dense planar slam,”in Mixed and Augmented
Reality(ISMAR),2014 IEEE International Symposium on ,pp.157–
164,IEEE,2014.
[141]J.Stückler,N.Biresev,and S.Behnke,“Semantic mapping
using object-class segmentation of rgb-d images,”in 2012
IEEE/RSJ International Conference on Intelligent Robots and
Systems ,pp.3005–3010,IEEE,2012.
[142]I.Kostavelis and A.Gasteratos,“Learning spatially
semantic representations for cognitive robot
navigation,” Robotics and Autonomous Systems
,vol.61,no.12,pp.1460–1475,2013.
[143]C.Couprie,C.Farabet,L.Najman,and Y.LeCun,“Indoor
semantic segmentation using depth information,” arXiv
preprint arXiv:1301.3572 ,2013.
[144]J.Deng,W.Dong,R.Socher,L.-J.Li,K.Li,and L.Fei-
Fei,“Imagenet:A large-scale hierarchical image database,”in
CVPR09 ,2009.
[145]A.Krizhevsky,I.Sutskever,and G.E.Hinton,“Imagenet
classi fi cation with deep convolutional neural networks,”in
Advances in neural information processing systems ,pp.1097–
1105,2012.
[146]K.He,X.Zhang,S.Ren,and J.Sun,“Deep residual
learning for image recognition,” arXiv preprint
arXiv:1512.03385 ,2015.
 [147]S.Ren,K.He,R.Girshick,andJ.Sun,“Fasterr-
cnn:Towardsreal-timeobjectdetectionwithregion proposal
networks,”in Advances in neural information processing
systems ,pp.91–99,2015.
[148]J.Long,E.Shelhamer,and T.Darrell,“Fully
convolutional networks for semantic segmentation,” arXiv
preprint arXiv:1411.4038 ,2014.
[149]S.Zheng,S.Jayasumana,B.Romera-
Paredes,V.Vineet,Z.Su,D.Du,C.Huang,and P.Torr,“Conditional
random fi elds as recurrent neural networks,”in
International Conference on Computer Vision(ICCV) ,2015.
[150]S.Gupta,P.Arbeláez,R.Girshick,and J.Malik,“Indoor
scene understanding with rgb-d images:Bottom-up
segmentation,object detection and semantic
segmentation,” International Journal of Computer Vision,pp.1
–17,2014.
[151]K.Konda and R.Memisevic,“Learning visual odometry
with a convolutional network,”in International Conference on
Computer Vision Theory and Applications
,2015.
[152]A.Kendall,M.Grimes,and R.Cipolla,“Posenet:A
convolutional network for real-time 6-dof camera
relocalization,”in Proceedings of the IEEE International
Conference on Computer Vision ,pp.2938–2946,2015.
[153]Y.Hou,H.Zhang,and S.Zhou,“Convolutional neural
network-based image representation for visual loop closure
detection,” arXiv preprint arXiv:1504.05241
,2015.
[154]S.Y.An,J.G.Kang,L.K.Lee,and S.Y.Oh,“Line segment-
based indoor mapping with salient line feature
extraction,” Advanced Robotics ,vol.26,no.5-6,pp.437–
460,2012.
 [155]H.Zhou,D.Zou,L.Pei,R.Ying,P.Liu,andW.Yu,“Structslam
:Visualslamwithbuildingstructure lines,” Vehicular
Technology,IEEE Transactions on ,vol.64,pp.1364–1375,April
2015.
[156]D.Benedettelli,A.Garulli,and
A.Giannitrapani,“Cooperative slam using m-space
representation of linear features,” Robotics and Autonomous
Systems ,vol.60,no.10,pp.1267–1278,2012.
[157]J.P.Saarinen,H.Andreasson,T.Stoyanov,and
A.J.Lilienthal,“3d normal distributions transform occupancy
maps:An e ffi cient representation for mapping in dynamic
environments,” The International Journal of Robotics Research
,vol.32,no.14,pp.1627–1644,2013.
[158]W.Maddern,M.Milford,and G.Wyeth,“Cat-
slam:probabilistic localisation and mapping using a
continuousappearance-
basedtrajectory,” InternationalJournalofRoboticsResearch
,vol.31,no.4SI,pp.429–451,2012.
[159]H.Wang,Z.-G.Hou,L.Cheng,and M.Tan,“Online mapping
with a mobile robot in dynamic and
unknownenvironments,” InternationalJournalofModelling,Identi
fi cationandControl ,vol.4,no.4,pp.415–423,2008.
[160]D.Zou and P.Tan,“Coslam:Collaborative visual SLAM
IEEE Transactions On Pattern
in dynamic environments,”
Analysis And Machine Intelligence ,vol.35,no.2,pp.354–
366,2013.
[161]T.A.Vidal-
Calleja,C.Berger,J.Sola,andS.Lacroix,“Largescalemultiplerobo
tvisualmappingwith heterogeneous landmarks in semi-structured
terrain,” Robotics and Autonomous Systems ,vol.59,no.9,pp.654
–674,2011.