【轉】SLAM | 視覺SLAM中特征點法開源算法：PTAM、ORB-SLAM

本文轉載自查看原文 2020-04-09 15:23 851 視覺導航

前面的話

VSLAM 是利用多視圖幾何理論，根據相機拍攝的圖像信息對相機進行定位並同時構建周圍環境地圖。VSLAM 前端為視覺里程計和回環檢測，相當於是對圖像數據進行關聯；后端是對前端輸出的結果進行優化，利用濾波或非線性優化理論，得到最優的位姿估計和全局一致性地圖。

前面已經介紹了系列一：VSLAM的前端：視覺里程計和回環檢測，和系列二：VSLAM中的后端優化和建圖。

按照特征法和直接法的分類，各種 VSLAM 具備不同的處理速度、軌跡精度等指標。隨着 VSLAM 的研究如火如荼地開展，許多研究者發表研究成果以及公開相關代碼，供學者學習與研究。

這次，我們將詳細介紹視覺SLAM中特征點法的經典開源算法。

3 特征點法開源算法

3.1 PTAM

PTAM是 2007 年由牛津大學主動視覺實驗室的 Georg Klein 和 David Murray 提出的。當時給研究者們帶來了極大震撼，它有如下創新點：

a) PTAM 第一個使用非線性優化。之前人們未認識到后端優化的稀疏性，所以覺得優化后端無法實時處理那樣大規模的數據，主流的 SLAM 均采用 EKF 濾波器等濾波方法。而 PTAM 則是一個顯著的反例，將 VSLAM 研究逐漸轉向了以非線性優化為主導的后端。

b) PTAM 引入了關鍵幀機制。不必精細地處理每一幅圖像，而僅僅處理較少的關鍵幀圖像，然后優化其軌跡和地圖。

c) PTAM 引入了多線程機制。將跟蹤和建圖過程分開。因為跟蹤部分需要實時響應圖像數據，而地圖則沒必要實時地優化，只需在后台進行處理。這是 VSLAM 中首次區分出前后端的概念，初步確定了 VSLAM 的框架。

3.1.1 主要結構

PTAM 主要分為跟蹤、建圖兩部分。

PTAM 的跟蹤分為粗階段和精階段。

在粗階段中選用圖像金字塔最高層的 50 個特征點，利用恆速模型和擴大范圍搜索，從這些測量中得出一個新姿態；再將近千個特征點重新投影到圖像中，執行更嚴格的塊搜索（FAST 特征的局部 8*8的方塊構成 patch 作為描述符），並構建重投影誤差得到最優的相機姿態。

地圖構建主要是建立三維地圖點的過程。它分為地圖的初始化和地圖的更新。

首先，系統初始化時使用三角測量構建初始地圖；在此之后，隨着添加新的關鍵幀，地圖將不斷地進行細化和擴展。

具體為：系統初始化時，根據前兩個關鍵幀提供的特征對應關系，采用 5 點算法和隨機采樣一致（RANSAC）估計本質矩陣（或使用平面情況的單應性分解）並三角化得到初始地圖。然后當插入關鍵幀時，使用極線搜索和塊匹配（零均值距離平方和 ZMSSD）計算得到精確匹配，從而精細化地圖。

PTAM 系統框圖如下圖所示。

3.1.2 優缺點

PTAM 不僅僅是 VSLAM 的程序，還將相機的標定和增強現實（AR）都包括進來，而且試圖在手機上實現，從另外的角度也可以說它是面向小場景的一個增強現實軟件。

PTAM 的最開始的版本是建議采用 5 點算法分解本質矩陣得到相機姿態，該方法用於非平面場景的初始化。后來 PTAM的初始化改變為使用單應性，其中場景假定為 2D 平面。

以現在的知識來看， PTAM 的 DEMO 可能有點過時，比如它的初始化需要用戶的輸入來捕捉地圖中的前兩個關鍵幀，而且它要求用戶在第一與第二關鍵幀之間采取平行於觀察場景的緩慢和平滑的平移運動。因為它采用的 2D-2D 的圖像匹配算法為不考慮特征仿射變換的 ZMSSD 算法，所以容易受到運動模糊和相機旋轉的影響。

PTAM 是為小場景 AR 設計的，沒考慮全局的回環，而且存在明顯的缺陷：場景小（實際情況是 6 000 個點和 150個關鍵幀）、跟蹤容易丟失等，但是在當時確實是一個里程碑的標志。

3.2 ORB-SLAM

ORBSLAM由西班牙 Raul 博士於 2015 年公布，其論文發表在《IEEE Transactions on Robotics》。到目前為止，ORBSLAM 是最完整的基於特征點法 VSLAM，它可以看做是 PTAM 的一個延伸，相比 PTAM， ORBSLAM 增加了一個回環檢測（loop closing）的線程。

該系統框架包括跟蹤、建圖、閉環三個線程，均基於 ORB 特征實現（ORB特征在之前系列文章中有介紹，可以參考系列一），所有優化環節都通過優化框架 G2O 實現。

ORBSLAM 有如下創新點：

初始化采用自動機制，不需要手工輸入，也不需要假設場景是否為平面。通過匹配 ORB 特征同時計算單應性和基礎矩陣並評分，選用分數高的方案。

將改進后的 ORB 特征貫穿整個工程始終，包括特征檢測、匹配以及用於閉環的詞袋模型（bag-of-words， BoW）。

使用 DBOW 模塊，不只是用於 loop closing 時的檢測，而且用於系統的重定位。 更大的意義是在圖像幀間匹配時，使用詞典對描述子進行分類的結果進行比對， 這種方法不僅有效，還可以大大簡化運算。

后端優化是亮點， ORB 在每一層估計中都大量采用G2O 優化，不僅有單幀位姿估計到局部地圖的位姿估計，而且有局部地圖點與位姿聯合估計，還有利用回環結果的全局位姿估計。

3.2.1 具體流程

ORBSLAM 的具體流程為：

跟蹤。跟蹤線程主要是得到相機位姿和關鍵幀。

具體為：先對圖像進行 ORB 特征提取和匹配，系統初始化得到 R，t 和 3D 點雲（如果系統未初始化）；然后采用參考關鍵幀模型或運動模型和 BoW 模塊加速匹配（如果跟蹤失敗也是將當前幀和所有關鍵幀通過 BoW 加速匹配），再構建局部小圖和重投影誤差優化函數；最后得到優化位姿和關鍵幀。

建圖。建圖線程主要是更新 3D 點和插入關鍵幀。

具體為：取出一個關鍵幀，計算特征點的 BoW 關系，更新關鍵幀間的連接關系，將關鍵幀插入地圖，驗證加入的地圖點，利用三角法生成新的地圖點，對相鄰關鍵幀和對應的 3D 點進行局部 BA，剔除冗余關鍵幀，將關鍵幀加入閉環；

閉環。閉環線程主要是糾正尺度漂移和全局優化。

具體為：取出一個關鍵幀，計算當前關鍵幀與每個共視關鍵幀的 BoW 得分，在所有關鍵幀中找出閉環備選幀，通過連續性檢測驗證候選幀，去做 sim3 優化（糾正尺度漂移，使其尺度一致），利用優化結果尋找更多的特征匹配，再做一遍優化，如果內點足夠，接收這個閉環，最后固定回環幀和當前幀再做全局優化。

更加具體，細節化來說，可以是：

1、地圖自動初始化：需要估計兩幀間的相機運動，並利用三角化估計路標點的3D位置，最后構建初始地圖。其具體步驟：

①ORB特征點提取與匹配，ORB特征點具有良好的旋轉和縮放不變性，以及計算效率；

②隨機一致性檢驗 RANSAC，同時通過 4 個匹配點計算H單應矩陣，8個匹配點計算F 基礎矩陣。分別計算兩個模型評分。選擇F本質矩陣和H單應矩陣在RANSAC過程中同時取得最好評分的情況，否則，返回①；

③F或者 H模型的選擇。

1)平面場景或低視差的情況，應該選擇 H單應矩陣。在這兩種情形，F計算誤差大，三角化誤差也大。因為低時差情況，三角化誤差大，所以不進行地圖初始化；

2)具有足夠視差的非平面場景，應該選擇F基礎矩陣，因為H的計算本身存在誤差；

④運動估計和三角化。模型選擇完之后，通過分解H單應矩陣或者F本質矩陣使用SVD矩陣分解估計相機運動 R,t。並利用三角化計算匹配點的 3D 坐標；

⑤使用 local BA 圖優化同時優化位姿和 3D 點。

2、跟蹤模塊：主要包括了當前幀位姿估計，局部地圖跟蹤和判斷新的關鍵幀幾個部分。

①從上一幀初始化位姿估計：如果上一幀跟蹤成功，則用相同的運動模型來預測當前幀的位姿。將上一幀中地圖點投影到當前幀，在小范圍內進行特征匹配。若有足夠多的匹配點，則直接PnP估計位姿；否則，加大在當前幀的搜索范圍進行特征匹配，利用足夠多的匹配點，通過PnP估計位姿；

②從全局定位初始化估計位姿：跟蹤失敗，執行全局重定位。利用 BoW 尋找匹配關鍵幀，運用 PnP 計算當前幀與各關鍵幀轉移矩陣，構建位姿圖，利用 g2o 優化獲得當前幀位姿；

③局部地圖跟蹤：利用多視圖進一步優化當前幀，將局部地圖中的 3D點投影到當前幀，並找到相應的匹配點。通過最小化匹配點的重投影誤差來進一步優化當前幀的位姿；

④寬松選擇關鍵幀。

關鍵幀插入需要同時滿足以下四點要求：

距離上一次全局重定位已經超過20幀(保證良好的重定位)
局部建圖進程閑置，或者距離上一次關鍵幀插入已經超過20幀(會阻斷局部BA)
當前幀至少跟蹤50個地圖點(保證當前是良好的跟蹤)
當前幀跟蹤的點數比參考幀跟蹤點數的90%還要少(保證有較大的視覺變化)

3、 局部地圖構建模塊：創建新的路標點以及剔除冗余的關鍵幀。

①插入新的關鍵幀；

②三角化新的地圖點，利用local BA 優化局部地圖中關鍵幀的位姿和地圖點3D位置；

③嚴格剔除冗余的關鍵幀和 3D 點，如果關鍵幀中 90%以上的地圖點能被其它至少3 個共視關鍵幀觀測到，則剔除該關鍵幀。

該模塊由於計算量較大導致耗時多，作為一個獨立的后台線程。但是新的地圖點會影響到下一時刻圖像幀是否能夠跟蹤成功，因此不能夠及時創建新的地圖點是導致ORB-SLAM2 算法跟丟的原因之一。可以將三角化新的地圖點放到能夠實時計算的跟蹤模塊，來解決這一不足。

4、回環檢測模塊：建立當前幀與過去關鍵幀之間的關聯，減少長時間的累積誤差，並通過相似變換矯正尺度偏移。

①利用基於 Bo W 的場景識別的方法檢測回環，並計算Sim3相似變換，構建優化圖；

②基於Sim3 變換，在本質視圖上全局優化位姿圖。

3.2.2 具體線程

ORB-SLAM算法和PTAM具有相同的算法框架，采用多線程構架，四個主線程：前端位姿跟蹤、局部地圖構建與優化、閉環檢測與優化、顯示與交互。

1、前端位姿跟蹤線程采用恆速模型，並通過優化重投影誤差優化位姿。

2、局部地圖線程通過MapPoints維護關鍵幀之間的共視關系，通過局部BA優化共視關鍵幀位姿和MapPoints。

3、閉環檢測線程通過bag-of-words加速閉環匹配幀的篩選，並通過Sim3優化尺度，通過全局BA優化Essential Graph和MapPoints。

4、使用bag-of-words加速匹配幀的篩選，並使用EPnP算法完成重定位中的位姿估計。

3.2.3 有關知識點

共視圖和本質圖

作者通過構造共視圖存儲各個關鍵幀之間的關系，以便在跟蹤、重定位、局部建圖、回環檢測等多個地方發揮其作用。

那么這究竟是個什么結構呢？其實就是一個無向加權圖，以關鍵幀為節點，關鍵幀之間如果有共同觀測的地圖點則產生邊，共同觀測的數量就是邊的權重。

這么說可能還是有點抽象，那就以下面這張圖為例子吧。

假設有六個關鍵幀(紅黃綠藍紫黑)，對應顏色框里的地圖點為各自觀測到的點，可以看出紅黃兩幀共同觀測到的地圖點個數為5，紅綠共同觀測到1個，黃綠共視個數為3，以此類推。

於是就可以得到右邊的圖結構，紅黃之間邊的權重為5，紅綠權重為1，黃綠權重為3等等。

這有什么作用呢？直觀來說，只要揪到一個，就可以把同伙都一網打盡！

比如，在重定位的時候需要在KeyFrameDatabase中搜索候選關鍵幀，由於關鍵幀之間是有重疊視圖的，所以在得到匹配的候選關鍵幀之后，繼續把每一個候選關鍵幀在共視圖中的所有“同伙”揪出來，從他們之中重新挑選分數最高的作為最新候選關鍵幀，同時把他們的分數求和作為新候選幀的分數。

最后，返回分數高於新候選關鍵幀中最高分75(>0.75*bestScore)的所有候選幀，用於接下來的重定位匹配等操作。

那么本質圖就是濃縮版的共視圖，因為它是在共視圖的基礎上，只保留權重比較大(>100)的邊，所以本質圖意味着其中的關鍵幀共視程度較高。

它主要用在回環檢測中，當檢測到回環時，利用本質圖對相似變換Sim3進行位姿圖優化。

下圖展示了共視圖和本質圖的區別所在，邊的減少可以降低位姿圖優化的負擔。

論文中給出的可視化結果圖對比就更明顯了。

花式Bundle Adjustment

細心的你可能已經發現前面提到了三種BA方式，分別是full BA、motion-only BA、local BA。這么多花式的BA，究竟有什么不一樣？

Full BA主要是在初始化時用的，它把初始化生成的所有地圖點和對應的圖像幀都納入優化變量，好好調優一番。畢竟開局小地圖，后面全靠補，一開始必須要穩！

Motion-only BA就是只對位姿進行優化，在跟蹤進程中每次估計相機位姿或者重定位時計算出相機位姿后都會對位姿進行優化，而不考慮地圖點。

而Local BA則是優化局部的位姿和地圖點，在局部建圖進程中把當前關鍵幀和他的“同伙們”以及他們對應的地圖點當作優化變量，其余的關鍵幀則保持不變。

網上有大佬把他們畫成了簡單的圖，更直觀地解釋了其中關系。圖中紅色的代表待優化變量，灰色的則表示保持不變。

3.2.4 優缺點

ORBSLAM 在工程上是非常完整的 SLAM 系統，里面涉及的很多參數都是通過計算得出，后續有大量的學者在其基礎上改進。

隨后 Raul 在前面的基礎上利用寬基線做了更加精密的半稠密地圖構建的工作， 2017 年又將 IMU 融入到ORBSLAM 中，由此可見ORBSLAM 的可擴展性很好。

當然與許多其他的基於特征點的 SLAM 系統一樣，有很多自身的缺陷：因為特征點的原因，只能得到稀疏點雲地圖，這對機器人下一步的導航應用會造成很大困難，而且它不易作為環境地圖的描述，也很難構建高層次地圖（語義地圖等），給環境的語義構建帶來了諸多不便。

結語

特征點法中的特征一般為人類根據圖像的一些特性，自己設計的算法，可能失去了大自然的本質和意義；在特征點法中絕大多數時間都耗費在特征的提取和匹配上，特征點法的瓶頸在於如何設計和提取更好的特征點；相比之下，直接法不依賴特征的提取和匹配，直接通過兩幀之間的像素灰度值構建光度誤差來求解相機運動。因此直接法可以在特征缺失的場合下使用。

該篇作為SLAM系列的第三篇，主要介紹了特征點法開源算法——PTAM和SLAM。接下來，SLAM系列文章會持續更新，會繼續和大家介紹視覺SLAM的主流框架，VIO多傳感器融合以及激光SLAM等內容，歡迎大家持續關注~

獲取文中ORB-SLAM2論文閱讀完整版，請關注公眾號留言或掃描下方個人微信。

參考：

1. 《ORB-SLAM: A Versatile and Accurate Monocular SLAM System》R. Mur-Artal等人

2. https://github.com/raulmur/ORB_SLAM2

3. 《ORB: an efficient alternative to SIFT or SURF》E. Rublee等人

4. 《視覺SLAM十四講：從理論到實踐第2版》高翔等人

5. 《ORB-SLAM中的ORB特征（提取）》小葡萄知乎文章

6. 《ORB-SLAM（五）優化》路游俠博客園文章