0.引言
從六月末到八月初大概一個月時間一直在啃SLAM十四講[1]這本書，這本書把SLAM中涉及的基本知識點都涵蓋了，所以在這里做一個復習，對這本書自己學到的東西做一個梳理。
書本地址：http://www.broadview.com.cn/book/4938
書本代碼：https://github.com/gaoxiang12/slambook
1.SLAM概述
SLAM：即時定位與地圖構建（Simultaneous Localization and Mapping）
數學描述：
一個典型的SLAM系統如下圖所示[1]:
典型的SLAM系統中，輸入傳感器數據，前端提取特征，估計運動，后端優化數據，再通過loop closure優化整個路徑。若沒有加loop closure的SLAM稱為視覺里程計。
2. 三維空間剛體運動
第2部分和第3部分是基礎的數學只是，本部分主要是對機器人位姿的描述方法的介紹。
坐標描述：
- 平面坐標
- 三維坐標
在SLAM中通常采用射影空間的齊次坐標表示位姿轉換。原因是齊次坐標下點和直線（高維空間里是超平面）能夠使用同樣的表達；且坐標能囊括無窮遠點和無窮遠超平面；齊次坐標可以方便地將平移與旋轉放在同一個矩陣中。
3. 李群和李代數
群：一種集合加上一種運算的代數結構
性質：
- 封閉性 ∀a1,a2,a1⋅a2∈A
- 結合律∀a1,a2,a3,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
- 幺元∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a
- 逆∀a∈A,∃a-1∈A,s.t.a⋅a-1 =a0
李群是指具有連續性質的群。並且，一般連續群上的運算還是無限可微，乃至解析的。三維旋轉矩陣可以構成特殊正交群，變換矩陣構成特殊歐式群
李代數:每個李群都有與之對應的李代數。李代數由一個集合V，一個數域F和一個二元運算[,]組成。如果它們滿足以下幾條性質，稱(V,F,[,]) 為一個李代數，記作g。
性質：
- 封閉性 ∀X,Y∈V,[XY]∈V
- 雙線性 ∀X,Y,Z∈V,a,b∈F,∀X,Y,Z∈V,a,b∈F, 有[aX+bY,Z]=a[XZ]+b[YZ][Z,aX+bY]=a[ZX]+b[ZY]
- 自反性 ∀X∈V,[XX]=0
- 雅可比等價 ∀X,Y,Z∈V,[X,[YZ]]+[Z,[YX]]+[Y,[ZX]]
三維旋轉和變換的李群李代數轉換如下圖所示，使用李代數可以很方便的進行優化，利用擾動模型進行求導。
旋轉矩陣是自身帶有約束的（正交且行列式為0），優化時引入額外的約束，優化困難，轉化為李代數之后把位姿轉為無約束的優化問題。：
4. 相機模型
相機幾何模型：將三維空間中的點（單位為米）映射到二維圖像平面（單位為像素）的過程。
針孔模型是常見且有效的模型。
- 內參矩陣K：相機坐標系到圖像像素坐標系產生的中間量（固定）
- 外參矩陣P：相機的位姿R,t
- 存在畸變
單目相機模型：
雙目相機模型：
5. 前端
特征匹配法：一共六組實驗。首先是ORB特征提取，通常是一些邊和角等灰度特征比較明顯的地方，描述子用來表示這些特征的周圍。對極約束用在兩組2d的點，都不知道深度的情況。3D-2D就是匹配的一組點知道深度，一組不知道。
直接法：最小化光度誤差
相比特征匹配法，直接法只計算特征點，不計算描述子，不需要知道點和點的匹配關系，計算量小。
直接法有一個很強的假設——灰度不變假設：同一個空間點的像素灰度值在各個圖像中是固定不變的。
光度誤差是P的兩個像素的亮度誤差。左邊的實驗叫稀疏直接法，示意圖中匹配的P來自稀疏特征點，右邊是半稠密直接法，不同就是P的特征點選取的多。
6. 后端
前端可以給出一個短時間內的軌跡和地圖，但由於不可避免的誤差累積，如果時間長了這個地圖是不准確的。所以我們希望構建一個尺度、規模更大的優化問題，以考慮長時間內的最優軌跡和地圖。
EKF
- EKF目前已經被圖優化所取代，曾經應用非常廣泛
Bundle Adjustment：BA是在只有觀測方程的情況下，優化相機的位姿以及路標三維坐標的圖優化。
- 優化相機位姿和路標點坐標
- 節點表示相機和路標，邊為它們之間的觀測
Pose Graph：在大規模的定位和建圖中，時間越久，BA計算量越大。因此提出pose graph，由關鍵幀之間通過特征匹配之后得到的運動估計來給定初始值，然后不再估計路標點
- 優化相機位姿
- 節點表示相機位姿，邊是兩個位姿節點之間的運動估計（來自特征點法或直接法）
左圖為Bundle Adjustment示意圖，右圖為Pose Graph示意圖。
7. 回環檢測
前端用於提供特征點的提取和軌跡，地圖的初值。后端負責這些數據的優化，但是視覺里程計僅考慮相鄰時間上的關聯，會有累計誤差，無法構建全局一致的軌跡和地圖。第一個圖表示真實的軌跡，第二個圖紅色表示位姿的偏移，而如果有回環檢測的話則可以將后面偏離的軌跡拉回到重合處。這就是回環檢測的好處。這里介紹詞袋模型，一共三步。
詞袋模型
- 創建字典（聚類）
K叉樹字典
- 用單詞出現的情況描述圖像——向量表示
- 比較上一步中的描述的相似程度
8. 建圖
建圖是SLAM的兩大目標之一，建圖的作用可歸納為定位，導航，避障，重建及交互。
單目稠密重建實驗：1.假設所有像素深度滿足某個初始的高斯分布
2.當新數據產生時，通過極線搜索和塊匹配確定投影點位置
3.根據幾何關系計算三角化后的深度及不確定性
4. 將當前觀測融合進上一次的估計中。若收斂則停止計算，否則返回第2步
RGB-D相機的稠密建圖：可以轉化為點雲地圖亦或是存儲空間更小的八叉樹地圖
總結
我把一些章節合並了，有許多細節都沒有說，整本書啃下來感覺最重要的還是代碼，代碼，代碼，重要的事要說三遍。C++還要繼續學習。
[1] 高翔, 張濤, 劉毅. 視覺 SLAM 十四講——從理論到實踐[M]. 2017
[2] Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J., ... & Leonard, J. J. (2016). Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age. IEEE Transactions on Robotics, 32(6), 1309-1332.