什么是SLAM？
同時定位與地圖構建 （simultaneous localization and mapping， SLAM）是機器人進入未知環境遇到的第一個問題。它是指機器人搭載特定傳感器，在沒有環境先驗信息的情況下，於運動過程中對周圍環境建模並同時估計自身的位姿。如果傳感器主要為相機，那么就稱為視覺 SLAM（VSLAM）。SLAM 技術已經研究和發展了三十多年，研
究人員已經做了大量的工作，近十年來，隨着計算機視覺的發展，VSLAM 以其硬件成本低廉、輕便、高精度等優勢獲得了學術界和工業界的青睞。
接下來，我們將有一系列的文章帶大家來領略SLAM的魅力。
視覺SLAM系統框架
VSLAM 是利用多視圖幾何理論，根據相機拍攝的圖像信息對相機進行定位並同時構建周圍環境地圖。按照相機的分類，有單目、雙目、 RGBD、魚眼、全景等。
VSLAM 主要包括視覺里程計（visual odometry， VO）、后端優化、回環檢測、建圖。 其中 VO 研究圖像幀間變換關系完成實時的位姿跟蹤， 對輸入的圖像進行處理，計算姿態變化， 得到相機間的運動關系；但是隨着時間的累計，誤差會累積，這是由於僅僅估計兩個圖像間的運動造成的，后端主要是使用優化方法，減小整個框架誤差（包括相機位姿和空間地圖點）。回環檢測，又稱閉環檢測，主要是利用圖像間的相似性來判斷是否到達過先前的位置，以此來消除累計誤差，得到全局一致性軌跡和地圖。建圖，根據估計的軌跡，建立與任務要求對應的地圖。
現在比較通常的慣例是把 VSLAM 分為前端和后端。前端為視覺里程計和回環檢測，相當於是對圖像數據進行關聯；后端是對前端輸出的結果進行優化，利用濾波或非線性優化理論，得到最優的位姿估計和全局一致性地圖。
1 前端：圖像數據的關聯
1.1 視覺里程計
前端中的視覺里程計， 為通過采集的圖像得到相機間的運動估計，視覺里程計問題可由下圖描述（雙目立體視覺里程計）。
視覺系統在運動過程中，在不同時刻獲取了環境的圖像，而且相鄰時刻的圖像必須有足夠的重疊區域，則視覺系統的相對旋轉和平移運動可被估算出來，然后將每兩個相鄰時刻之間視覺系統的運動串聯起來,可以得到累計的視覺系統相對於參考坐標系的旋轉和平移。
如上圖所示， 視覺里程計的任務就是已知 k = 0 的初始位置 C0 （這可以根據情況自己定義），求相機的運動軌跡當前的位置 Ck 通過 Tk 和上一時刻的位置Ck-1 來計算，Tk 為 K 和 K+1 時刻的相機相對位置變化，可根據相應時刻采集的圖像計算出來，從而恢復相機的運動軌跡。
視覺里程計可分為特征點法和直接法。
特征點法主要是根據圖像上的特征匹配關系得到相鄰幀間的相機運動
估計，它需要對特征進行提取和匹配，然后根據匹配特征構建重投影誤差函數，並將其最小化從而得到相機的相對運動；
直接法是假設兩幀圖像中的匹配像素的灰度值不變，構建光度誤差函數， 也將其最小化求解幀間的相機運動。
特征點法和直接法的直觀對比
1.1.1 特征點法
特征點法的原理為：通過提取和匹配相鄰圖像的特征點估計該幀間對應的相機相對運動。特征點法的步驟包括特征檢測、匹配、運動估計和優化。
1．特征檢測與匹配
特征點可以稱為興趣點、顯著點、關鍵點等。以點的位置來表示的點特征是一種最簡單的圖像特征。 特征點可以分為關鍵點和描述子兩部分。事實上，特征點是一個具有一定特征的局部區域的位置標志，稱其為點，是將其抽象為一個位置概念，以便於確定兩幅圖像中同一個位置點的對應關系，所以在特征匹配過程中是以該特征點為中心，將鄰域的局部特征進行匹配。
也就是說在進行特征匹配時首先要為這些特征點建立特征描述，這種特征描述通常稱之為描述子。
2．運動估計
運動估計就是根據特征點的匹配情況，恢復出兩幀間的相機運動。針對特征點匹配的情況，運動估計分為 2D-2D、3D-2D、 3D-3D。其求解方法可以分為幾何方法和優化方法。幾何方法主要是根據對極幾何理論得到兩幀間的對應關系；而優化方法主要是構建兩幀間的重投影誤差並使其最小，從而得到幀間變換。
2D-2D：
2D-2D 主要是針對單目相機的初始化過程，在不知道空間中 3D 點的情況下（如未進行初始化）通過兩幀間匹配的特征點進行幀間相機運動估計。這涉及到對極幾何中本質矩陣（E）或單應性矩陣（H）的相關理論及其分解，通常在圖像的特征匹配中難免會有“外點”，可以采用隨機采樣一致（RANSAC）得到最大“內點”子集的 E 或 H。
2D-2D示意圖
3D-2D：
3D-2D 就是 PnP（perspective-n-point）求解 3D 到 2D 點對運動的方法，描述的是當知道 N 個 3D 空間點及其投影位置時（例如單目，已經初始化完畢，知道特征點的 3D 位置），如何估計相機位姿。當然雙目或者深度相機可以直接使用PnP。對它的求解有 DLT、 P3P、 EPnP、 UPnP。現在常用的做法是先采用 P3P 得到初始解，然后構建重投影誤差，使之最小化。
使用李代數上的擾動模型分析其導數，並通過高斯牛頓等優化方法得到兩幀間的相對變換，具體做法又叫作捆集優化（bundle adjustment， BA），在編程上一般采用General Graph Optimization（G2O） 等優化庫實現。
3D-3D：
3D-3D 主要是激光 SLAM 采用迭代最近點（ICP）求解。在 VSLAM 中，可以在 RGB-DSLAM 中使用，但由於 RGB-D相機的限制，僅僅適用室內，而且適用小的場景。這是由於深度的估計不准，導致誤差比 3D-2D 大。直觀的感覺是， 相機得到的 3D 位置誤差較大（相機方向性好， 距離信息誤差大）， 3D-2D 只使用一次深度信息，但是 3D-3D 采用兩次深度信息，導致計算的精確度降低，所以在普通相機中一般回避 3D-3D 的方式。
特征點法的問題
特征點法有幾個問題：
a) 關鍵點的提取和描述子的計算非常耗時，如果保證SLAM 實時運行，需要 30 Frame/s，也就是每幀圖像的處理時間約 30 ms，而實時性最好的 ORB也需要近 20 ms/Frame；
b) 特征點法僅僅使用了圖像中幾百個特征點，占整個圖像幾十萬個像素的很小部分， 丟棄了大量可以利用的圖像信息；
c) 特征點的尋找是根據人類自己設計的檢測算法，並不完善，有些圖像沒有明顯的紋理，有些圖像的紋理比較相似，這種情況下特征點法的 VSLAM 就很難運行；
d) 特征點法只能得到空間的稀疏三維點雲。離稠密地圖尚有一定的距離，與用於機器人導航的地圖差距就更大了。
1.1.2 直接法
直接法根據像素灰度信息估計相機的運動，幾乎不用計算關鍵點和描述子，省去了計算關鍵點和描述子的時間，可以在特征點缺失但是有圖像灰度梯度的場合（當然對於一張白牆， 它也無能為力）。相比於特征點法只能構建稀疏點雲地圖（構建半稠密或稠密需要采取其他技巧），直接法具備構建半稠密和稠密地圖的能力。
直接法的不變量是對應像素點的灰度值。首先假設兩個像素點在第一幀與第二幀之間灰度值保持不變，如下圖所示， P1 和 P2 的灰度值是一樣的，直接法的思路是根據當前相機的位姿估計來尋找 P2 的位置，如果相機位姿不好， P2 和 P1 的外觀會有明顯差別。
為了減少這個差別， 直接法優化相機位姿，尋找與 P1更相似的 P2。這就是在灰度不變假設下，直接采用兩幀圖像中的匹配像素的灰度值，構建光度誤差的優化函數，改變相機位姿使之最小化。根據圖像像素 P 的情況，直接法分為稀疏、半稠密和稠密直接法。P 如果是稀疏關鍵點， 稱之為稀疏直接法；P 如果是圖像中梯度明顯的點， 稱之為半稠密法；P 如果是圖像中的所有像素， 稱之為稠密法。
光流法
-
光流法保留特征點，但只計算關鍵點，不計算描述子。同時，使用光流法來跟蹤特征點的運動。
-
光流描述了像素在圖像中的運動，跟蹤源圖像某個點在其他圖像中的運動。遵循灰度不變假設：同一個空間點的像素灰度值，在各個圖像中是固定不變的（強假設，不一定成立）。
-
計算部分像素運動的稱為稀疏光流（LK），計算所有像素的稱為稠密光流（HS）。LK光流中，假設某一個窗口內的像素具有相同的運動。
-
可以看成最小化像素誤差的非線性優化；每次使用泰勒一階近似。在離優化點較遠時效果不佳，往往需要迭代多次，運動較大時需要使用金字塔；LK光流可以用於跟蹤圖像中稀疏關鍵點的運動軌跡，得到匹配點后，后續計算與特征點法VO相同；按方法可以分為正向/反向+平移/組合的方式。
-
光流法可以加速基於特征點的視覺里程計算法，避免計算和匹配描述子的過程，但要求相機運動較慢（或采集頻率較高）。
直接法
直接法也有自己的局限，首先它需要滿足光度不變性假設，這對相機提出了很高的要求， 而且稠密法因為需要計算圖像的所有像素（640*480 就是 30 萬個像素），很難在現有 CPU 上實時運行。
在前端，特征點法和直接法最大的區別在於，直接法是依賴於梯度搜索，如果兩幀采集時間過大，可能圖像運動距離過大，導致灰度不規則變化，從而梯度搜索的優化函數進入局部最小，無法給出較好的優化解；而特征點法對運動和光照有一定的魯棒性， 是根據特征點對距離和光照的魯棒性來決定的，這也是未來 SLAM 發展的決定因素之一。
1.2 回環檢測
回環檢測就是利用傳感器有效地檢測出以前經過這里，它對於 SLAM 系統意義非常重要，因為無論你的數據多么的精確，模型多么的優秀，系統的累積誤差始終存在。如果能正確地檢測到回環，對構建全局一致性地圖是非常有幫助的；從另一方面，可以利用回環檢測對跟蹤失敗后的情況進行重定位。
在 VLSAM 中回環檢測大多數做法是基於外觀，比較圖像間的相似性。如果用特征點的方式，比如采用 SIFT 特征描述一幅圖像，首先每個 SIFT 矢量都是 128 維的，假設每幅圖像通常都包含 1 000 個 SIFT 特征，在進行圖像相似度計算時，這個計算量非常大，所以通常不會直接采用特征點，而是采用詞袋模型。
詞袋模型通過提取圖像特征，再將特征進行分類構建視覺字典，然后采用視覺字典中的單詞集合可以表征任一幅圖像。換句話說，通過 BoW 可以把一張圖片表示成一個向量。這對判斷圖像間的關聯很有幫助，所以目前比較流行的回環解決方案都是采用的 BoW 及其基礎上衍生的算
法 IAB-MAP。
回環檢測主要由 BoW 模塊、 算法模塊、 驗證模塊三個部分組成。
目前還沒有專門針對直接法的回環檢測方法，主流的回環檢測都是利用特征點采取 BOW 方式。換句話說回環檢測還是依賴於特征點，從這個角度來看，特征點法有很大的優勢：特征點法已經提取了特征，直接用這些特征去做回環檢測；而直接法沒有提取特征，如果想做回環檢測，必須要另外提取特征。這也是 ORBSLAM 和 LSDSLAM 中的回環檢測采取的不同的方式。
ORBSLAM 中的回環檢測與整個系統結合得比較緊密，整個系統都是采用的 ORB 特征，首先離線訓練得到 ORB 詞典，在搜索時因為ORBSLAM 本身就已經計算了特征點和描述，可以直接用特征來搜索，而且 ORBSLAM 采用正向和反向兩種輔助指標：反向指標在節點（單詞）上儲存到達這個節點的圖像特征的權重信息和圖像編號，因此可用於快速尋找相似圖像。正向指標則儲存每幅圖像上的特征以及其對應的節點在詞典樹上的某一層父節點的位置，因此可用於快速特征點匹配（只需要匹配該父節點下面的單詞）。
LSDSLAM 是采用 OpenFABMAP（OpenCV 上實現的FAB-MAP）來完成回環功能。FAB-MAP 在貝葉斯框架下，采用 Chou-Liu tree估計單詞的概率分布，能夠完成大規模環境下的閉環檢測問題，但是它通過連續的當前幀數據與歷史幀數據比較，效率較低，不能滿足實時地回環檢測。個人感覺 LSDSLAM 中的回環檢測是為了完成這個大的系統，額外添加的模塊，其實與系統契合度不是很高。
結語
該篇作為SLAM系列的第一篇，主要介紹了視覺SLAM系統主要結構，並詳細介紹了前端中的視覺里程計和回環檢測兩大模板。接下來，SLAM系列文章會持續更新，會繼續和大家介紹后端優化算法，視覺SLAM的主流框架，VIO多傳感器融合以及激光SLAM等內容，歡迎大家持續關注~
獲取文中思維導圖完整版，請關注公眾號留言或掃描個人微信。
參考：
1.視覺十四講 高翔
2.https://www.cnblogs.com/gaoxiang12/p/3695962.html
3.https://zhuanlan.zhihu.com/p/64720052
推薦閱讀
目標檢測系列
語義分割系列
面試求職系列
一起學C++系列
競賽與工程項目分享系列
【AI算法修煉營】期待你的加入！
-END-
轉自：