本書系統介紹了視覺SLAM（同時定位與地圖構建）所需的基本知識與核心算法，既包括數學理論基礎，如三維空間的剛體運動、非線性優化，又包括計算機視覺的算法實現，例如多視圖幾何、回環檢測等。
此外，還提供了大量的實例代碼供讀者學習研究，從而更深入地掌握這些內容。本書可以作為對SLAM感興趣的研究人員的入門自學材料，也可以作為SLAM相關的高校本科生或研究生課程教材使用。

高翔，清華大學自動化學院博士研究生，主要研究興趣為基於RGB-D相機的視覺SLAM技術。

先後獲得清華大學新生獎學金、張明為獎學金並三次獲得國家勵志獎學金，相關研究成果發表於Robotics and Autonomous Systems、Autonomous Robot、CCC等期刊和會議。

張濤：清華大學教授。

第1講預備知識1 
1.1本書講什麼1 
1.2如何使用本書3 
1.2.1組織方式3 
1.2.2代碼5 
1.2.3面向的讀者6 
1.3風格約定6 
1.4致謝和聲明7 

第2講初識SLAM 9 
2.1引子：小蘿蔔的例子11 
2.2經典視覺SLAM框架17 
2.2.1視覺里程計17 
2.2.2後端優化19 
2.2.3回環檢測20 
2.2.4建圖21 
2.3 SLAM問題的數學表述22 
2.4實踐：編程基礎25 
2.4.1安裝Linux操作系統25 
2.4.2 Hello SLAM27 
2.4.3使用cmake28 
2.4.4使用庫30 
2.4.5使用IDE32 

第3講三維空間剛體運動37 
3.1旋轉矩陣39 
3.1.1點和向量，坐標系39 
3.1.2坐標系間的歐氏變換40 
3.1.3變換矩陣與齊次坐標42 
3.2實踐：Eigen 44 
3.3旋轉向量和歐拉角48 
3.3.1旋轉向量48 
3.3.2歐拉角50 
3.4四元數51 
3.4.1四元數的定義51 
3.4.2四元數的運算53 
3.4.3用四元數表示旋轉55 
3.4.4四元數到旋轉矩陣的轉換55 
3.5 *相似、仿射、射影變換56 
3.6實踐：Eigen幾何模塊57 
3.7可視化演示60 

第4講李群 與李代數62 
4.1李群與李代數基礎64 
4.1.1群64 
4.1.2李代數的引出65 
4.1.3李代數的定義67 
4.1.4李代數so(3) 67 
4.1.5李代數se( 3)68 
4.2指數與對數映射69 
4.2.1 SO(3)上的指數映射69 
4.2.2 SE(3)上的指數映射.70 
4.3李代數求導與擾動模型72 
4.3.1 BCH公式與近似形式72 
4.3.2 SO(3)李代數上的求導73 
4.3.3李代數求導74 
4.3.4擾動模型（左乘）75 
4.3.5 SE(3)上的李代數求導76 
4.4實踐：Sophus76 
4.5 *相似變換群與李代數.79 
4.6小結81 

第5講相機與圖像82 
5.1相機模型84 
5.1.1針孔相機模型84 
5.1.2畸變87 
5.1.3雙目相機模型90 
5.1. 4 RGB-D相機模型92 
5.2圖像93 
5.3實踐：圖像的存取與訪問95 
5.3.1安裝OpenCV95 
5.3.2操作OpenCV圖像96 
5.4實踐：拼接點雲99 

第6講非線性優化104 
6.1狀態估計問題106 
6.1.1最大後驗與最大似然106 
6.1.2最小二乘的引出108 
6.2非線性最小二乘109 
6.2.1一階和二階梯度法110 
6.2.2高斯牛頓法 111 
6.2.3列文伯格—馬誇爾特方法113 
6.2.4小結114 
6.3實踐：Ceres115 
6.3.1 Ceres簡介116 
6.3.2安裝Ceres116 
6.3.3使用Ceres擬合曲線117 
6.4實踐：g2o121 
6.4. 1圖優化理論簡介121 
6.4.2 g2o的編譯與安裝122 
6.4.3使用g2o擬合曲線123 
6.5小結128 

第7講視覺里程計1130 
7.1特徵點法132 
7.1.1特徵點132 
7.1.2 ORB特徵134 
7.1.3特徵匹配137 
7.2實踐：特徵提取和匹配138 
7.3 2D?2D:對極幾何141 
7.3.1對極約束141 
7.3.2本質矩陣143 
7.3.3單應矩陣146 
7.4實踐：對極約束求解相機運動148 
7.5三角測量153 
7.6實踐：三角測量154 
7.6.1三角測量代碼154 
7.6.2討論156 
7.7 3D?2D：PnP157 
7.7.1直接線性變換158 
7.7.2 P3P159 
7.7.3 Bundle Adjustment 161 
7.8實踐：求解PnP165 
7.8.1使用EPnP求解位姿165 
7.8.2使用BA優化166 
7.9 3D?3D：ICP172 
7.9.1 SVD方法173 
7.9.2非線性優化方法175 
7.10實踐：求解ICP176 
7.10.1 SVD方法176 
7.10.2非線性優化方法178 
7.11小 結180 

第8講視覺里程計2182 
8.1直接法的引出184 
8.2光流（Optical Flow）185 
8.3實踐：LK光流187 
8.3.1使用TUM公開數據集187 
8.3.2使用LK光流188 
8.4直接法（Direct Method）192 
8.4.1直接法的推導192 
8.4.2直接法的討論195 
8.5實踐：RGB-D的直接法196 
8.5.1稀疏直接法196 
8.5.2定義直接法的邊197 
8.5.3使用直接法估計相機運動199 
8.5.4半稠密直接法200 
8.5.5直接法的討論202 
8.5.6直接法優缺點總結203 

第9講實踐：設計前端205 
9.1搭建VO框架206 
9.1.1確定程序框架207 
9.1.2確定基本數據結構208 
9.1.3 Camera類210 
9.1.4 Frame類212 
9.1.5 MapPoint類213 
9.1.6 Map類213 
9.1.7 Config類214 
9.2基本的VO：特徵提取和匹配216 
9.2.1兩兩幀的視覺里程計216 
9.2.2討論224 
9.3改進：優化PnP的結果224 
9.4改進：局部地圖227 
9.5小結233 

第10講後端1 235 
10.1概述237 
10.1.1狀態估計的概率解釋237 
10.1.2線性系統和KF239 
10.1.3非線 系統和EKF242 
10.1.4 EKF的討論243 
10.2 BA與圖優化245 
10.2.1投影模型和BA代價函數245 
10.2.2 BA的求解247 
10.2.3稀疏性和邊緣化248 
10.2.4魯棒核函數255 
10.2.5小結256 
10.3實踐：g2o257 
10.3.1 BA數據集257 
10.3.2 g2o求解BA258 
10.3.3求解262 
10.4實踐：Ceres 264 
10.4.1 Ceres求解BA 265 
10.4.2求解267 
10.5小結269 

第11講後端2 270 
11.1位姿圖（Pose Graph）271 
11.1.1 Pose Graph的意義271 
11.1.2 Pose Graph的優化272 
11.2實踐：位姿圖優化274 
11.2.1 g2o原生位姿圖274 
11.2.2李代數上的位姿圖優化278 
11.2.3小結284 
11.3 *因子圖優化初步285 
11.3.1貝葉斯網絡285 
11.3.2因子圖286 
11.3.3增量特性288 
11.4 *實踐：gtsam 289 
11.4.1安裝gtsam 4.0289 
11.4.2位姿圖優化290 

第12講回環檢測297 
12.1回環檢測概述299 
12.1.1回環檢測的意義299 
12.1.2方法300 
12.1.3準確率和召回率301 
12.2詞袋模型303 
12.3字典305 
12.3.1字典的結構305 
12.3.2實踐：創 字典306 
12.4相似度計算309 
12.4.1理論部分309 
12.4.2實踐：相似度的計算310 
12.5實驗分析與評述314 
12.5.1增加字典規模314 
12.5.2相似性評分的處理316 
12.5.3關鍵幀的處理316 
12.5.4檢測之後的驗證317 
12.5.5與機器學習的關係317 

第13講建圖319 
13.1概述320 
13.2單目稠密重建322 
13.2.1立體視覺322 
13.2.2極線搜索與塊匹配323 
13.2.3高斯分佈的深度濾波器325 
13.3實踐：單目稠密重建328 
13.4實驗分析與討論339 
13.4.1像素梯度的問題339 
13.4.2逆深度340 
13.4.3圖像間的變換341 
13.4.4並行化：效率的問題342 
13.4.5其他的改進343 
13.5 RGB-D稠密建圖343 
13.5.1實踐：點雲地圖344 
13.5.2八叉樹地圖347 
13.5.3實踐：八叉樹地圖350 
13.6 *TSDF地圖和Fusion系列352 
13.7小結356 

第14講SLAM：現在與未來357 
14.1當前的開源方案358 
14.1.1 MonoSLAM358 
14.1.2 PTAM359 
14.1.3 ORB-SLAM361 
14.1.4 LSD-SLAM363 
14.1.5 SVO 364 
14.1.6 RTAB-MAP366 
1 4.1.7其他367 
14.2未來的SLAM話題367 
14.2.1視覺+慣性導航SLAM367 
14.2.2語義SLAM369 
14.2.3 SLAM的未來370 

附錄A高斯分佈的性質371 
A.1高斯分佈371 
A.2高斯分佈的運算371 
A.2.1線性運算371 
A.2.2乘積372 
A.2.3複合運算372 
A.3複合的例子372 

附錄B ROS入門374 
B.1 ROS是什麼374 
B.2 ROS的特點375 
B.3如何快速上手ROS375 

參考文獻377