机器人感知：因子图在SLAM中的应用

本书共通过7章，深入浅出地介绍了因子图数学定义、推断方法，以及真实环境中机器人上的各种应用。在涵盖了详细的背景知识及数学论证的同时，提供了充足的SLAM应用案例以供读者参考。

Frank Dellaert，2001年于卡内基梅隆大学获博士学位，现于佐治亚理工学院交互计算学院任终身教授。他目前的研究兴趣主要集中在机器人与计算机视觉的交叉领域，尤其是用图模型方法解决大规模三维重建与地图构建问题。

Michael Kaess，现于卡内基梅隆大学任助理教授。Michael于2008年在佐治亚理工学院获博士学位，之后于麻省理工学院先后就任博士后与研究员。他目前的研究兴趣包括移动机器人智能问题，具体集中在大规模三维建图与定位问题中的概率图模型与线性代数的联系。

译者简介

刘富强，泡泡机器人创始人。

董靖，美国佐治亚理工学院计算机科学博士，主要研究方向机器人学。

本书作者是促成SLAM从经典卡尔曼滤波形式向图优化模型转变的关键人物，在SLAM领域的造诣非常深厚，此书是他们在因子图上的研究成果的汇集和整理，是一本非常好的SLAM方面的书籍。

译者序1

《机器人感知：因子图在SLAM 中的应用》译稿终于完成了。翻译一本书付出的劳动远远大于阅读一本书。在翻译的过程中，我反反复复阅读原版书籍及翻译版加起来不下30 遍。当将终稿交给编辑时，心里还是满满的收获和兴奋。

2017 年8 月22 日中午13:30，董靖在泡泡机器人微信群中分享了这本由他的老师Frank Dellaert（GTSAM 的作者）和Michael Kaess（iSAM 的作者）合写的书，我没有任何犹豫，当天就给Frank 发了邮件，问他是否可以允许我来翻译这本书。Frank 非常热情地答应了，同时还推荐他的得意弟子董靖跟我一起翻译，并且说，相信我们可以合作得很好。

Frank Dellaert 是佐治亚理工学院的教授，Michael Kaess 从佐治亚理工学院毕业后，去麻省理工学院做了几年博士，目前在卡内基梅隆大学任职。两人都是SLAM 界的大牛，他们的开源的iSAM 和GTSAM 利用因子图对位姿及地图进行高效优化。除了在SLAM 领域，iSAM 和GTSAM 在其他许多机器人领域也得到了非常广泛的应用，如机械臂路径规划、空间–时序重建、大规模三维场景重建等。

目前，市面上关于SLAM 的书非常少。希望本书的出版能够为推动国内SLAM 的研究贡献出一份力量。本书从iSAM 和GTSAM 所用到的理论基础出发，系统、完整地对其进行了介绍，相信大家读完本书后，会对基于因子图的优化有深入的理解。书中不仅有概率推断、贝叶斯网络、因子图、非线性优化、流形及在其上的优化、贝叶斯树、QR 分解、乔里斯基分解、边缘化（Marginalization）等基础知识的讲解，还有增量平滑与地图构建（iSAM）的理论基础的讲解，同时在第7 章还专门介绍了因子图在机器人领域的各种应用案例。

真正理解这本书的一般性内容需要花一些时间，融会贯通则更需要下功夫。在品尝主食（本书）的同时，我们为大家推荐3 个配菜。

1. 源码：iSAM 及GTSAM 的代码均已开源，在看本书的过程中，可以配合源码一起看，这样能够更好地理解本书的理论内容。

2. 在【泡泡机器人SLAM】（ID：paopaorobot_slam）微信公众号上面，搜索董靖讲解的“GTSAM Tutorial”公开课，可以帮助你快速了解GTSAM的整体框架和应用。

3. 在泡泡论坛（http://paopaorobot.org）上进行交流。在看书过程中遇到的任何问题都可以在论坛上提问，只有互相交流才能更好地理解书上的内容。

与董靖合作翻译的过程非常愉快，他是本书作者之一Frank 的弟子，对于本书的内容非常熟悉，我们经常高密度地对有疑问的内容进行讨论，在这个过程中，他也给了我非常多的启发。

在本书的翻译过程中，我得到了很多人的帮助和支持。首先要感谢电子工业出版社的郑柳洁女士及白涛老师，没有你们就没有这本书的面世。郑编辑也为我们处理了所有翻译之外的事情，让我们能不受干扰地完成这本书的翻译。

感谢泡泡机器人学术组织的章国锋老师、黄山老师，以及周平、蔡育展、鲁涛、刘畅、王慧国、陈世浪等同学的反馈意见。

在这里，我想把这本书送给我的妻子李明晓，遇到你是我这辈子最大的幸运，你的境界、心态和思想都深深地影响着我，让我变得更加成熟稳重。有你的地方就是家，就是可以栖息的港湾。我爱你。同时，也将此书献给我的女儿刘天琦，爸爸希望你能够有天地般宽广的胸怀，同时能够一生都对所有事物保持好奇心，做一个快乐的科学家。还有我的奶奶、爸爸妈妈、岳父岳母。我之所以可以活得这么快乐简单，后面是你们的负重前行，谢谢你们。感谢麻省理工学院的Wanda，与她的每次交流都让我受益匪浅，从她的身上我学到了非常多好的习惯。感谢我的老师王宗义教授在我读博期间对我的言传身教，他让我体会到了做实业的快乐和成就感，也让我养成了良好的科研习惯。

严复翻译《天演论》时，在译例言中讲到：“译事三难：信、达、雅”。“信”是指翻译要准确，“达”是指不拘泥于原文，“雅”是指语句要得体适当。我们尽了自己最大的努力保证翻译的“信”，同时期望可以达到“达”和“雅”的境界。尽管如此，限于译者水平，译文的缺点和错误在所难免，诚恳地希望读者批评指正。

刘富强

译者序2

同步定位与地图构建，也就是大家熟知的SLAM，是机器人学领域非常重要的一个子领域。近几年来，随着无人机、无人车、虚拟现实及增强现实的逐渐商用与普及，SLAM 作为以上领域的重要驱动技术越来越受到大家的重视。

5 年前我有幸加入Frank 的课题组，并在Frank 的指导下开始从事SLAM 相关领域的研究。5 年博士期间我与Frank 的合作愉快而充实，从他那里我不仅学到了机器人学、数学乃至C++ 编程的很多知识，更有他严谨治学、踏实工作的态度。所以在收到Frank 的邮件，邀请我参与Factor Graphs for Robot Perception（下称原书）中文翻译时，我的内心是非常激动的。在感谢Frank 对我SLAM 领域专业水平信任的同时，我也感受到推广SLAM 技术、推广因子图工作的责任之重。

近十年来，在Frank 等业内专家的努力下，因子图作为一种高度灵活的概率图模型，在SLAM 领域得到了广泛的推广与应用。原书作为两位作者在SLAM 领域研究应用因子图十余年经验的总结，通过7 个章节，深入浅出地介绍了因子图数学定义、推断方法，以及真实环境中机器人上的各种应用。原书在涵盖了详细的背景知识及数学论证的同时，也提供了充足的SLAM 应用案例以供参考。原书可以作为高年级研究生课程教材，也可以供SLAM 领域专业人士参考。

目前市面上关于SLAM 领域工作的中文著作并不多，涉及因子图、近期SLAM 界进展的著作更是少之又少。我参与本书翻译的最大动力便是希望将因子图这一工具及业界关于SLAM 研究的最新进展介绍给大家，抛砖引玉。希望本书能辅助读者了解因子图这一工具，或者对SLAM 领域相关的研究带来启发。

本书的翻译是由富强兄所主导的，在本书中富强兄付出了非常多的时间和心血，在此表示感谢，没有富强兄的工作就不可能有本书的出版！同时一并感谢参与题序的老师们、参加审阅校对的各位老师同学，以及电子工业出版社的各位编辑，感谢你们为本书顺利出版付出的辛勤工作！

最后，限于译者水平，缺陷甚至错误在所难免，恳请读者批评指正。

董靖

第1 章引言3

1.1 机器人领域中的推断问题 4

1.2 概率建模 5

1.3 生成模型的贝叶斯网络 6

1.4 指定概率密度函数 8

1.5 在贝叶斯网络中进行模拟 9

1.6 最大后验概率推断 10

1.7 因子图推断 12

1.8 因子图支持的计算 14

1.9 路线图 15

1.10 文献评论 16

第2 章平滑与地图构建17

2.1 SLAM 中的因子图 18

2.2 非线性因子图的最大后验概率推断 19

2.3 线性化 20

2.4 最小二乘问题的直接求解方法 22

机器人感知：因子图在SLAM 中的应用

2.5 最大后验概率推断的非线性优化 24

2.5.1 梯度下降法 25

2.5.2 高斯–牛顿法 25

2.5.3 列文伯格–马夸尔特算法 25

2.5.4 Dogleg 最小化法 27

2.6 文献评论 28

第3 章探索稀疏性31

3.1 关于稀疏性 32

3.1.1 启发性的例子 32

3.1.2 稀疏雅可比矩阵及其因子图 33

3.1.3 稀疏信息矩阵及其图表示 34

3.2 消元算法 36

3.3 利用变量消元进行稀疏矩阵分解 38

3.3.1 稀疏高斯因子 39

3.3.2 生成乘积因子 39

3.3.3 利用部分QR 分解进行变量消元 40

3.3.4 多波前QR 分解 41

3.4 稀疏乔里斯基分解与贝叶斯网络 43

3.4.1 线性高斯条件概率密度 43

3.4.2 反向替代求解贝叶斯网络 44

3.5 讨论 44

3.6 文献评论 45

第4 章消元顺序49

4.1 消元的时间复杂度 50

4.2 变量顺序的影响 51

4.3 填充的概念 54

4.4 启发式排序 55

4.4.1 最小度排序 55

4.4.2 嵌套分割排序 55

4.5 机器人领域中的启发式排序 57

4.6 嵌套分割和SLAM 60

4.7 文献评论 62

第5 章增量平滑与地图构建65

5.1 增量推断 66

5.2 更新矩阵分解 68

5.3 卡尔曼滤波及平滑 70

5.3.1 边缘化 71

5.3.2 固定滞后平滑与滤波 72

5.4 非线性滤波及平滑 74

5.4.1 贝叶斯树 75

5.4.2 更新贝叶斯树 76

5.4.3 增量平滑与地图构建 79

5.5 文献评论 81

第6 章流形上的优化83

6.1 姿态与航向估计 84

6.1.1 增量旋转 85

6.1.2 指数映射 86

6.1.3 局部坐标 86

6.1.4 结合朝向信息 88

6.1.5 平面旋转 89

6.2 位姿SLAM 90

6.2.1 位姿表示 91

6.2.2 局部位姿坐标 91

6.2.3 位姿的优化 92

6.2.4 位姿SLAM 93

6.3 李群及任意流形上的优化 94

6.3.1 矩阵李群 94

6.3.2 一般流形与归约 95

6.3.3 归约和李群 96

6.4 文献评论 97

第7 章应用99

7.1 惯性导航 100

7.2 稠密三维地图构建 102

7.3 现场机器人学 105

7.4 鲁棒估计与非高斯推断 108

7.5 长期运行和稀疏化 109

7.6 大规模及分布式SLAM 110

7.7 总结 114

参考文献117

附录A 多波前乔里斯基分解139

附录B 李群及其他流形141