個人博客：http://www.chenjianqu.com/
原文鏈接：http://www.chenjianqu.com/show-105.html
論文：Raul Mur-Artal, Juan D. Tard ´ os´.ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras.2017
ORB-SLAM2：一個開源的支持單目、雙目和RGB-D相機的SLAM系統
摘要——我們提出了一個完整的SLAM系統，支持單目相機、雙目相機、RGB-D相機，擁有地圖重用、迴環檢測、重定位等特性。本系統可以在標準的CPU上實時運行，應用範圍從室內小型手持設備的序列，工業環境無人機的序列，再到城市裏汽車的序列。我們的後端基於單目視覺和立體視覺的BA，可以使用度量尺度進行精確的軌跡估計。我們的系統包含一個輕量級的定位模式，這個定位模式利用未建圖區域的視覺里程計軌跡，匹配到地圖點，可以實現無漂移定位。我們在29個流行的公共序列上進行評估，結果顯示我們的方法達到了SOTA精度，是最精確的SLAM解決方案。我們公開了源代碼，不僅爲了SLAM領域的發展，也是爲了能提供給其他領域的研究者一個開箱即用的SLAM解決方案。
I.引言
同步定位與地圖構建（SLAM）是CV和機器人領域這20年來一個研究熱點，也吸引了高科技公司的注意力。SLAM技術構建一個未知環境的地圖並在地圖中定位。SLAM的一個基本要求是實時運行。在各種各樣的傳感器中，相機是比較便宜的而且可以提供環境豐富的信息，這可以提高位置識別的精度和魯棒性。因此基於相機的視覺SLAM，是當前的主流研究。位置識別是SLAM系統的一個關鍵模塊，它用於迴環檢測（檢測什麼時候傳感器返回到已經建圖的區域，然後修正迴環的誤差）以及在SLAM跟蹤丟失或重新初始化時後進行重定位。跟蹤丟失的原因可能是遮擋、劇烈運動。
VSLAM可以僅僅使用單目相機實現，這是最便宜、最輕量的傳感器配置形式。然而單個相機無法觀測到深度信息，因此地圖和估計軌跡的尺度未知。此外，系統初始化(system bootstrapping)需要多視圖或濾波技術來生成初始地圖，因爲它無法在第一幀進行三角化。最重要的一點是，單目SLAM會遇到尺度漂移問題，而且在純旋轉環境中可能無法工作。使用立體相機或RGB-D相機可以解決上述的問題，可以構建一個更加可靠的VSLAM系統。
本文我們在單目的ORB-SLAM[1]的基礎上提出了ORB-SLAM2，主要由以下貢獻：
*這是第一個同時支持單目、雙目、RGB-D相機的開源的SLAM系統，包含迴環檢測、重定位和地圖重用等功能。
*我們的RGB-D結果表明，與基於ICP或光度和深度誤差最小化的SOTA方法相比，使用BA可以獲得更高的精度。
*通過使用近、遠立體點(stereo points)和單目觀測，我們的立體測量結果比SOTA的直接立體SLAM更精確。
*一種輕量級的定位方式，可以在無法建圖的情況下高效的重用地圖。
下圖1展示了基於立體視覺和RGB-D的ORB-SLAM2的輸出實例：
立體視覺的例子展示了KITTI數據集[2]00序列的最終的軌跡和離散重建。這是一個擁有多個迴環的城市序列，但是ORB-SLAM2能成功的檢測。RGB-D的例子展示了TUM RGB-D[3]數據集中fr1_room序列的關鍵幀位姿估計,以及一個稠密點雲，通過估計的位姿反向投影(backprojecting)傳感器深度圖進行渲染。注意我們的SLAM不執行任何類似KinectFusion[4]那樣的融合(fusion)，但是這個良好的結果表明我們系統估計的關鍵幀位姿的精度很高。更多實例我們將在相關視頻進行展示。
在這篇論文的剩餘部分，我們討論Section II中的相關工作，在Section III中描述我們的系統，在Section IV展示評估結果，最後在Section V進行總結。
II.相關工作
本節我們討論雙目SLAM和RGBD-SLAM的相關工作。
A.雙目SLAM
早期雙目SLAM中一個出色的工作來自Paz et al.[5]。基於條件獨立的分治(Conditionally Independent Divide and Conquer) EKF-SLAM算法，它比當時其它的方法能在更大的環境中運行。最重要的是，這是第一次利用近點和遠點（即由於立體相機的視差很小而無法可靠估計其深度的點）的雙目SLAM，對後者使用了深度逆參數化[6]。他們經驗性表明，如果點的深度小於雙目基線的40倍，可以可靠地進行三角化。這裏我們遵從這個策略，以不同的方式處理近點和遠點，這將在Section III-A進行解釋。
大多數現代雙目SLAM系統是基於關鍵幀的，並在局部區域執行BA，實現可伸縮性。Strasdat et al.[8]的工作是對關鍵幀的內窗口中的BA(點-位姿約束)和外窗口中的位姿圖(位姿-位姿約束)進行聯合優化。通過限制這些窗口的大小，實現恆定的時間複雜度，但是不能保證全局一致性。Mei et al.[9]的RSLAM使用路標和位姿的相對錶示，在活動區域執行相對BA(relative BA)，這樣可以限制爲恆定時間。RSLAM能夠檢測迴環，這允許拓展迴環兩邊的活動區域，但不強制實現全局一致性。最近Pire et al.[10]的S-PTAM執行局部BA，然而它缺乏大型的迴環檢測。跟這些方法類似，我們在一組局部關鍵幀中執行BA，這樣複雜性就與地圖大小無關，我們可以在大環境中運行。然而我們的目標是構建一個全局一致的地圖。當閉合一個迴環時，我們的系統就像RSLAM一樣，先將兩邊進行對齊，這樣跟蹤就能繼續使用舊地圖進行定位，然後執行一個位姿圖優化，將回路中的累積漂移減至最小，接下來執行全局BA。
最近Engel et al.[11]的LSD-SLAM是一個基於半稠密、直接法的SLAM，最小化圖片中高梯度區域的光度誤差。不依賴於特徵點，這個方法預計對運動模糊或紋理較差的環境更加魯棒。然而作爲一種直接法，它的性能會因爲未建模的影響比如rolling shutter或non-lambertian reflectance而嚴重降低。
B.RGB-D SLAM
最早也是最著名的RGB-D SLAM系統是Newcombe et al.[4]的KinectFusion。該方法將來自RGB-D相機的所有深度數據融合到一個體積密度模型中，使用ICP計算相機位姿。該系統的被限制在一個很小的工作空間中，由於它的體積表示和缺乏迴環檢測。Whelan et al.[12]的Kintinuous能夠運行在大型環境中，因爲它使用了rolling cyclical buffer，以及包含了基於位置識別和位姿圖優化的迴環檢測。
Endres et al.[13]的RGB-D SLAM可能是第一個流行的開源系統。這是一個基於特徵的系統，整個前端通過特徵匹配和ICP算法計算幀到幀運動。後端使用啓發式搜索的迴環約束執行位姿圖優化。類似的，Kerl et al.[14]的DVO-SLAM的後端優化位姿圖，其中關鍵幀到關鍵幀的約束是從視覺里程計計算出來的，該視覺里程計最小化光度誤差和深度誤差。DVO-SLAM還以啓發式搜索之前所有的幀，以獲得閉環候選，而不是依賴於位置識別。
Whelan et al.[15]最近的工作ElasticFusion構建了一個基於面片(surfel)的環境地圖。這是一個以建圖爲中心的方法，捨棄位姿，執行迴環檢測，將non-rigid deformation應用到地圖，而不是標準的位姿圖優化。該系統的詳細的重建和定位精度令人印象深刻，但是目前的實現僅限於房子大小的地圖，因爲它的複雜程度與地圖中surfel的數量相關。
正如Strasdat et al.[8]所說的那樣，我們的ORB-SLAM2使用深度信息合成立體座標，用於提取圖片上的特徵。這樣我們的系統就不需要知道輸入是雙目還是RGB-D了。不同於上面所有的方法，我們的後端基於BA，而且構建一個全局一致的離散重建。因此我們的方法是輕量級的，可以工作在標準CPUs上。我們的目標是系統能長期運行和全局一致的定位，而不是構建細節最豐富的稠密重建。However from the highly accurate keyframe poses one could fuse depth maps and get accurate reconstruction on-the-fly in a local area or post-process the depth maps from all keyframes after a full BA and get an accurate 3D model of the whole scene.
III.ORB-SLAM2
用於雙目和RGB-D相機的ORB-SLAM2基於單目的、基於特徵點的ORB-SLAM[1]。Fig.2展示了系統的框架：
系統有三個線程：1)跟蹤線程：通過與局部地圖進行特徵匹配，應用motion-only BA最小化重投影誤差，從而實現每一幀的定位。2)局部建圖線程：管理局部地圖並優化它，執行局部BA。3)迴環線程：檢測大回環並通過執行位姿圖優化修正累積漂移。在位姿圖優化後，該線程會啓動第四個線程用於執行全局BA，計算最佳結構和運動解。
本系統嵌入了一個基於DBoW2[16]的位置識別模塊，用於重定位。在跟蹤丟失(比如被遮擋)、在已經建圖的場景重新初始化、迴環檢測的時候都會用到位置識別模塊。本系統維持一個共視圖[8]和關鍵幀的最小生成樹，共視圖中節點是關鍵幀，當兩個關鍵幀存在共視點時，就會存在一條邊將兩個節點連接。這些圖結構可以檢索某個關鍵幀的局部窗口，因此跟蹤和局部建圖可以在局部運行，這樣使得系統可以工作在大型環境中，並且當閉合迴環時執行位姿圖優化執行的結構。
本系統的跟蹤、建圖和位置識別任務中都使用相同的ORB特徵[17]。這些特徵對旋轉和尺度都很魯棒，而且對相機的自動增益、自動曝光、光照變化都展示了很好的不變性。而且，ORB特徵的抽取和匹配速度很快，這允許系統實時運行。此外，ORB特徵在詞袋位置識別[18]中展示了很好的精度和回召率。
在後面的章節我們會展示立體/深度信息如何被利用以及哪些部件會受到影響。關鍵每個系統每個部分更詳細的信息，讀者可以參考單目版本[1]。
A.單目、近立體和遠立體點
ORB-SLAM2作爲一種基於特徵的方法，預處理輸入，在顯著關鍵點位置抽取特徵，如Fig.2b所示：
然後就可以捨棄輸入圖像了，整個系統的操作基於這些抽取的特徵，因此係統獨立於所用的傳感器，無論是雙目還是RGB-D。我們的系統處理單目關鍵點(monocular)和立體關鍵點(stereo)，立體關鍵點被進一步分爲近關鍵點和遠關鍵點。
立體關鍵點(stereo keypoints)定義爲x_s=(uL,vL,uR)，其中(uL,vL)是雙目相機中左圖像座標系的座標，uR是右圖像座標的水平軸的值。對於雙目相機，我們在左右兩個圖像抽取ORB特徵，然後對左圖每個ORB特徵都在右圖尋找一個匹配的點。可以高效的設計立體矯正圖像， 使得極線是水平的。然後用左圖ORB特徵點的座標和右圖匹配的水平座標來生成立體關鍵點，並通過patch correlation進行亞像素細化（subpixel refined）。對於RGB-D相機，我們從RGB圖像中抽取ORB特徵，然後如Strasdat et al.[8]提出的方法，對每個座標爲(uL,vL)的特徵點，我們將它的深度值d轉換到一個虛擬的右相機座標：uR=uL - fx*b/d，fx是水平焦距，b是structured light projector和紅外相機的基線長度，對於Kinect和Asus Xtion大概爲8cm。深度傳感器的不確定程度由虛擬的右相機座標表示。通過這種方式，我們雙目相機和RGB-D相機就可以等價的被系統處理。
如果立體點的深度小於雙目相機/RGB-D相機的40倍，則將該點分類爲近點，如[5]所示，否則將其分類爲遠點。近點可以被安全的三角化，因爲此時深度估計是準確的，並提供尺度、平移和旋轉信息。而遠點可以提供精確的旋轉信息，但是尺度和平移信息比較弱。當遠點被其它視圖支持時，我們可以對它進行三角化。
單目關鍵點(Monocular keypoints)定義爲左相機座標x_m=(uL,vL)，對應到該ORB特徵點雙目匹配失敗或RGB-D相機深度值無效的座標。這些點只能被多個視圖三角化，不提供尺度信息，但是可以提供旋轉和平移估計。
B.系統引導
使用雙目相機或RGB-D相機的主要優點之一是可以僅從一次圖像採集中獲得深度信息，而不需要像單目相機那樣運動初始化中獲得特定結構。在系統初始化時，我們使用第一幀作爲關鍵幀，設置它的位姿爲原點，使用所有的立體點創建初始地圖。
C.Monocular和Stereo約束的BA
我們的系統在跟蹤線程中執行motion-only BA去優化相機位姿，在局部建圖線程中執行local BA優化關鍵幀和地圖點的局部窗口，在迴環線程中執行full BA優化所有的關鍵幀和關鍵點。我們使用在g2o中實現的Levenberg-Marquardt方法[19]。
Motion-only BA：通過最小化匹配點之間的重投影誤差優化該幀相機位姿的旋轉R∈SO(3)和平移t∈R^3，這裏的匹配點是世界座標3D點Xi∈R^3和立體關鍵點/單目關鍵點xi，i表示該幀所有的匹配對索引。公式如下：
其中ρ是robust Huber代價函數，Σ是與關鍵點的尺度有關的協方差矩陣。Π(.)是投影函數，Πm是單目投影函數，Πs是矯正後的立體函數，定義如下：
其中(fx,fy)是焦距，(cx,cy)是相機主點，b是基線長度。
Local BA優化一組共視的關鍵幀K_L以及被這些關鍵幀看到的所有地圖點PL2。其它的不在K_L裏的關鍵幀K_F，它們觀察到的點保持不變。定義Xk是一組在P_L中的點與關鍵幀k中的點的匹配，其優化問題可以表示如下：
Full BA是local BA的特殊情況，所有的關鍵幀和地圖點都被優化，除了用來消除gauge freedom的初始關鍵幀之外。
D.迴環檢測和全局BA
迴環檢測的執行有兩步，首先是檢測並驗證迴環，第二是通過位姿圖優化對迴環進行修正。與可能會出現尺度漂移[20]的單目ORB-SLAM相比，立體/深度信息使得尺度可觀測，幾何驗證和位姿圖優化不再需要處理尺度漂移，因此不是通過相似性而是通過剛體變換實現。
在ORB-SLAM2中，我們在位姿圖優化後加入了一個全局BA優化以獲得最優解。這個優化可能很費計算資源，因此我們另外開一個線程執行它。允許系統繼續創建地圖和檢測迴環。然而，這可能會存在BA輸出和當前地圖狀態如何合併的問題。當全局BA正在運行而新的迴環右檢測到的時候，我們將中止優化過程，而是繼續閉合迴環，然後重新啓動全局BA。當全局BA完成的時候，我們需要將由全局BA更新的關鍵幀子集、地圖點，與後來插入的關鍵幀、地圖點進行合併。這是通過生成樹將更新的關鍵幀的校正(比如從非最佳位姿到最佳位姿的轉換)傳播到未更新的關鍵幀來完成的。未更新的點通過應用於參考關鍵幀的矯正進行變換。
E.關鍵幀的插入
ORB-SLAM2根據單目ORB-SLAM引入的策略插入關鍵幀，這個策略插入和刪除關鍵幀很頻繁。近立體點和遠立體點之間的差別，使得我們可以引入新的關鍵幀插入的條件，這在惡劣環境中非常關鍵，因爲場景大部分都是遠立體點，如Fig.3所示：
在這鐘環境中，我們需要足夠的近點來精確的估計相機的平移，因此如果系統此時跟蹤的近立體點的數量小於τ_t，並且該幀可以創建至少 τ_c個新的近立體點，那麼系統將插入新的關鍵幀。我們經驗性的發現τ_t=100，τ_c=70時，系統工作的很好。
F.定位模式
我們包含了一個定位模式(Localization Mode)，在場景沒有發生大的變換的情況下，可以在已經建圖的區域進行長期的、輕量的定位。在這個模式中，局部建圖線程和迴環檢測線程並不工作，相機通過跟蹤線程或重定位連續的定位。在此模式中，跟蹤線程利用視覺里程計匹配和地圖點匹配。視覺里程計匹配是當前幀中的ORB特徵和之前的幀創建的3D點之間的匹配。這些匹配使得在未建圖區域的定位更加魯棒，但是漂移會累積。地圖點匹配確保對現有地圖的無漂移定位。可以在相關視頻上看到這個模式的演示。
IV.評估
我們在三個流行的數據集上評估ORB-SLAM2，並將結果與其它SOTA的SLAM相比較，使用原始作者發佈的結果，使用標準評估指標。我們在16Gb RAM的Intel Core i7-4790桌面電腦上運行。考慮到多線程系統的不確定性，我們將每個數據序列運行5次，展示中位數結果，以估計軌跡的準確性。我們的開源實現包括校準和在這些數據集上運行的說明。
A.KITTI數據集
KITTI數據集[2]包含從一個汽車在市區和高速路上記錄的雙目序列。這個雙目傳感器的基線長大約54cm，錄製頻率爲10Hz，矯正後的分辨率爲1240x376。序列00,02,05,06,07,09包含迴環。我們的ORB-SLAM2檢測到所有的迴環，而且能夠在之後重用地圖，除了序列09，因爲它只在最後的幾幀出現迴環。Table I展示了11個訓練序列的結果，還對比了ground truth，並與SOTA的SLAM：雙目LSD-SLAM[11]進行對比:
我們使用了兩個不同的指標，[3]提出的絕對平移均方根誤差(absolute translation RMSE) t_abs，以及[2]提出的平均相對平移(average relative translation) t_rel和旋轉 r_rel誤差。我們的系統在大多數序列都勝過Stereo LSD-SLAM，多數序列的相對誤差低於1%。序列01,如Fig3，是訓練集中唯一的高速路序列，其平移誤差稍差。在這個序列中，平移誤差更難去估計，因爲車輛高速行駛而低幀率採集導致能跟蹤的點很少。然而方向可以被精確的估計，達到每100m 0.21度的誤差，因爲有很多點可以被跟蹤。Fig.4展示一些軌跡估計的實例：
與[1]提出的單目ORB-SLAM相比，這裏的立體版本可以處理序列01，該序列使得單目ORB-SLAM失敗了。在這個高速公路序列中，看Fig.3，近立體點僅在很少的幀可以被看到。雙目相機/RGB-D相機可以僅從一個關鍵幀創建點，而不需要像單目相機那樣延遲初始化(需要尋找兩個關鍵幀之間的匹配)，這是在該序列中不跟蹤丟失的關鍵。此外，立體版本的系統用度量的尺度估計地圖和軌跡，不受尺度漂移的影響，如Fig.5：
B.EuRoC數據集
最近的EuRoC數據集[21]包含了11個立體序列，這些序列通過微型飛行器(MAV)在兩個不同的房間和大型工業環境飛行錄製得到。這個雙目傳感器的基線大約是11cm，提供20Hz錄製的WVGA圖像。這些序列根據MAAV的速度、光照、場景紋理被分類爲easy、medium、difficult。所有的序列中MAV都會重訪問環境，ORB-SLAM2能夠重用地圖和迴環檢測。Table II展示了ORB-SLAM2在所有序列的絕對平移均方根誤差，同時與Stereo LSD-SLAM[11]進行比較：
ORB-SLAM2實現定位精度在幾釐米，比Stereo LSD-SLAM更加精確。我們的跟蹤線程在V2_03_difficult序列丟失了，由於嚴重的運動模糊。正如[22]展示的那樣，這個序列可以通過IMU信息處理。Fig.6展示了計算的軌跡與ground truth軌跡比較的實例：
C.TUM RGB-D數據集
TUM RGB-D數據集[3]包含來自RGB-D傳感器的室內序列，這些序列被分爲幾個類別，用於評估目標重建和SLAM/里程計算法在不同紋理、光照和結構條件下的表現。我們展示了在最常用的序列上的結果。Table III我們比較了我們的精度與當前SOTA方法：ElasticFusion[15]，Kintinuous[12]，DVO-SLAM[14]和RGB-D SLAM[13]進行的比較。
我們的方法是唯一一個基於BA的系統，而且在大多數序列中超過了其它方法。正如[1]中RGB-D SLAM的結果，freiburg2序列的深度圖大約由4%的尺度誤差，這可能是由於誤矯正，我們已經在運行時進行了補償，並且可以部分解釋我們的明顯有更好結果。Fig.7展示了4個序列中從計算的關鍵幀位姿反向投影傳感器深度圖產生的點雲：
良好的清晰度和書桌、海報的直線輪廓證明了我們這個方法的高精度定位。
D.時間消耗結果
爲了全面的評估系統，我們在Table IV展示了時間消耗的結果，在3個不同圖像分辨率和傳感器的序列上進行測試：
顯示了每個線程時間消耗的平均值和標準差。這些序列包含一個單一回環，因此全局BA和迴環閉合的一些任務只執行一次，僅報告一次時間測量。每幀的平均跟蹤時間低於每個相機幀率的倒數，意味着我們的系統可以實時工作。因爲ORB特徵抽取在雙目相機中是並行化的，在V2_02的立體WVGA圖像中抽取1000個ORB特徵類似於在fr3_office的單個VGA圖像中抽取同樣數量的特徵。
迴環中關鍵幀的數量與迴環閉合的時候存在關係。當KITTI 07序列中的迴環包含更多的關鍵幀時，室內fr3_office構建的共視圖更加稠密，因此迴環融合、位姿圖優化、全局BA任務更加昂貴。共視圖越稠密，使得局部地圖包含更多的關鍵幀和地圖點，因此局部建圖線程和局部BA也更加昂貴。
V.總結
我們提出了一個基於單目、雙目、RGB-D相機的完整SLAM系統，能夠在標準CPU上實時執行重定位、迴環閉合、地圖重用。如實驗所示，我們專注於構建全局一致的地圖，以在各種環境中進行長期定位。所提出的具有定位模式可以爲已知環境提供一種非常健壯，零漂移和輕量級的定位方法。此模式對某些應用可能很有用，比如在良好建圖的空間的AR：跟蹤用戶的視角。
與其它SOTA系統的比較展示了ORB-SLAM2達到了最高的精度。在KITTI視覺里程計benchmark中ORB-SLAM2是當前最好的立體SLAM解決方案。關鍵的是，與其它立體視覺里程計方法相比，ORB-SLAM2在已建圖區域實現了零漂移定位。
出乎意料的是，我們的RGB-D結果表明，如果想要最精確的相機定位，這BA方法優於直接法或ICP，並且BA的計算成本更低，不需要GPU也可以實時運行。
我們已經開源了，並給出了例程和文檔，以便於被其它開發者使用。ORB-SLAM2據我們所知，是第一個可以通過支持單目、雙目、RGB-D的VSLAM系統。此外，我們的源代碼包含了一個使用單目相機實現AR的示例，以證明我們這個系統的潛力。
舉例來說，將來的擴展可能包括不重疊的多相機，魚眼鏡頭或全向相機支持，大規模密集融合，協作映射或增強的運動模糊魯棒性。