論文：ORB-SLAM：a Versatile and accurate Monocular SLAM System
前言
一、介紹
PTAM（parallel tracking and Mapping）系統局限於小尺度場景中，提供簡單但是有效的關鍵幀選擇、特征匹配、三角化、重定位技術，但是具有以下缺點：（1）缺少閉環優化；（2）重定位的不穩定性；（3）需要人工初始化地圖。ORB-SLAM在PTAM系統基礎上構建， ORB-SLAM系統具有以下優點：
（1）在追蹤、建圖、重定位和回環過程中采用同一種特征點-ORB，使得該系統在沒有GPU加速下實時運行，並且對光照和視角變化具有良好的魯棒性。
（2）在追蹤和建圖過程中使用局部共視地圖，而不是全局地圖，使得該系統能夠在大尺度場景下實時運行。
（3）在共視圖上通過最小生成樹生成本質圖，簡化地圖結構，使得系統能夠實時的閉環優化
（4）在特征跟蹤失敗情況下，通過詞典模型匹配，使得系統能夠實時的重定位。
（5）采用適者生存的方法來篩選地圖點和關鍵幀，從而提高系統追蹤的魯棒性和系統的可持續運行。
二、相關工作
A、地點識別（place recognition）
B、地圖初始化（map initialization）
C、單目SLAM（monocular SLAM）
三、系統概觀
A、特征點選擇（feature choice）
ORB-SLAM系統的一個重要特點就是在追蹤和建圖線程的地點識別中使用同一種特征，從而保證了實時的重定位和回環檢測。首先為了滿足系統實時性要求，每幀圖像提取特征的時間應該少於33ms，從而排除主流的SIFT特征點（約300ms）、SURF特征點（約300ms）和最近的A-KAZE特征點（約100ms）。然後為了實現地點識別的性能，該特征點需要旋轉不變性，從而排除BRIEF描述子和LDB描述子。我們選擇ORB（Oriented FAST and Rotated BRIEF）特征點，它是由改進後的FAST角點（具有方向和尺度）和256位二進制的BRIEF描述子組成，使得ORB算法不僅保持旋轉和尺度不變性，而且能滿足實時性要求。
B、三個線程：追蹤、局部地圖和閉環優化（tracking、local mapping and loop closing）
如上圖所示，ORB-SLAM系統主要運行三個並行線程：特征追蹤線程（主線程）、局部建圖線程和回環優化線程。
特征追蹤線程主要負責估計每一幀相機的位姿和選擇新的關鍵幀。首先初始化地圖，然後基於前一幀和勻速運動模型來初始化當前幀的位姿，如果當前幀丟失，則通過全局重定位來初始化當前幀的位姿。一旦獲取當前幀的初始化位姿和一組匹配點對，構建當前幀的局部地圖，通過將局部地圖中的地圖點投影到當前幀中尋找更多的3D-2D匹配約束，使用PnP算法估計當前幀的位姿。最後根據準則判斷當前幀是否可作為新的關鍵幀插入。
局部建圖線程處理新的關鍵幀，主要包括管理局部地圖和局部BA優化。首先根據當前關鍵幀更新共視圖和本質圖信息。然後在相鄰幀中搜索當前幀未匹配的ORB特征點，三角化創建新的地圖點。根據一些準則剔除冗余的地圖點和關鍵幀。最後對當前幀局部地圖中的關鍵幀和地圖點進行局部BA優化。
回環優化線程處理局部建圖線程後的關鍵幀，主要包括回環檢測和全局優化。首先查找當前關鍵幀是否存在回環候選幀，如果存在，則計算當前幀與回環候選幀的相似變換sim3，從而確定是否為回環幀和估計回環處的累積漂移。然後根據相似變換信息，融合地圖點和更新共視圖中的邊。最後在本質圖上進行位姿圖優化，從而實現全局位姿的一致性。
C、地圖點、關鍵幀和選擇標準（MapPoint and KeyFrame）
每個地圖點Pi都包含以下信息：
（1）該地圖點在世界坐標系下的3D坐標Xw,i；
（2）該地圖點的視角方向ni, 為所有視角方向（該地圖點與所有觀測到它的關鍵幀的相機中心的射線）的平均單位向量；
（3）該地圖點的ORB描述子，選取該地圖點對應的ORB特征點與其他關鍵幀的對應ORB特征點的漢明距離最小的ORB描述子。
（4）該地圖點被觀測到的最大距離dmax和最小距離dmin，根據ORB特征點的尺度不變性確定。
每個關鍵幀Ki都包含以下信息：
（1）該關鍵幀的相機位姿Tiw，是一種剛體運動變換。
（2）相機的內參K，包括相機在x，y軸上的焦距fx、fy，相機的光圈中心cx、cy和深度圖的縮放因子s。
（3）該關鍵幀上提取的ORB特征點，它的坐標是經過畸變矯正後的坐標。
地圖點和關鍵幀的創建是在一個相對寬松的條件下，為了保證特征追蹤線程在惡劣條件下（旋轉、快速移動等）下具有魯棒性；但是想要保留下來需要滿足嚴格的條件，為了剔除冗余的關鍵幀和錯誤匹配的地圖點，這樣有利於系統的可持續運行。
D、共視圖和本質圖（Covisibility graph and Essential graph）
在共視圖中將每個關鍵幀作為一個節點，若兩個關鍵幀存在一定數量（至少15個）共視地圖點則這兩個這兩個關鍵幀之間存在一條邊，邊的權重為共視地圖點的數量。
我們使用位姿圖來校正閉環處的累積誤差，由於共視圖中邊的數量較多，我們創建共視圖的子圖-本質圖，保留全部的節點，但是選擇更少的邊。ORB-SLAM系統在第一個關鍵幀開始就維護一個生成樹，每個關鍵幀只與共視點最多的相鄰幀產生邊。本質圖包含生成樹、共視地圖點數量超過100的邊和回環的邊。
E、基於詞袋的地點識別（bags of words）
ORB-SLAM系統中嵌入了基於DBoW2的地點識別模塊，用於回環檢測和重定位。視覺詞典是一種描述子空間的離散化表示，可由大量圖像中提取ORB描述子來離線創建。如果訓練的圖像足夠充足，那麽訓練的視覺詞典可用於不同的環境下工作。本ORB-SLAM系統增量式地維護一個關鍵幀數據庫，該數據庫存儲了每個關鍵幀中的視覺單詞，同時在關鍵幀刪除後該數據庫也會進行更新。
由於關鍵幀之間存在視覺重疊，所以在查詢關鍵幀數據庫時不存在獨一無二的高分關鍵幀。原始的DBoW2考慮到重疊問題，提高相近時間內關鍵幀的最小相似度分數。本系統中在查詢關鍵幀數據庫時會首先剔除共視圖中相鄰的關鍵幀，然後選取相似度分數高於最高分數75%的關鍵幀。
四、地圖自動初始化（Automatic Map Initialization）
地圖初始化的目的就是計算兩幀的相對位姿和初始化一些地圖點。初始化的方法應該適用於平面和非平面場景，並且不需要人工選擇好的圖像幀來幹預。我們提出了一種魯棒性的方法-首先並行計算兩個幾何模型：適用於平面場景的單應矩陣H和適用於非平面場景的基礎矩陣F，然後根據一些具體的方法來選擇一個模型，最後恢復運動與結構。該算法步驟如下：
1）找到對應點
在當前幀Fc中提取ORB特征點Xc，然後在參考幀Fr中尋找匹配Xr，如果匹配點數量不夠，則重置參考幀。
2）並行計算兩個模型
根據下列公式並行計算單應矩陣Hcr和基礎矩陣Fcr：
在RANSAC叠代中分別使用DLT和八點法來計算單應矩陣H和基礎矩陣F。叠代的次數都是固定且相同的，每次叠代中計算基礎矩陣需要8個點，計算單應矩陣需要4個點，而且每次叠代都要計算每個模型的分數S，公式如下：
其中dcr2和drc2為當前幀到參考幀的對稱轉換誤差；TM為基於X2測試為95%的外點閾值（TH為5.99，TF為3.84）；τ的值等於TH。
3）模型選擇
在平面場景或者低視差場景中，選擇單應矩陣；在高視差場景中，選擇基礎矩陣。ORB-SLAM系統根據以下方法來選擇模型：
如果RH>0.45,選擇單應矩陣模型，否則選擇基礎矩陣模型。
4）運動與結構恢復
對於單應矩陣模型，Faugeras et .al提出恢復8種運動假設的方法，該方法容易在低視差的情況下失效。ORB-SLAM系統提出了直接三角化出8種運動假設，然後根據是否滿足低視差、低重投影誤差和景深是否為正等條件選擇其中的一種運動假設。
對於基礎矩陣，首先根據公式計算出本質矩陣E，然後根據SVD的方法恢復出4種運動假設，最後根據景深是否為正條件選擇其中的一種運動假設。
5）BA優化
最後進行一個全局BA優化。
由上圖對比實驗可知：PTAM系統和LSD-SLAM系統初始化平面中的所有地圖點，但是ORB-SLAM系統僅當擁有足夠的視差下才會初始化地圖點。
五、特征點追蹤（Tracking）
特征點追蹤線程處理相機的每一幀，主要做兩件事：（1）估計每一幀相機的位姿，（2）選擇新的關鍵幀。
A、ORB特征提取（ORB Extraction）
我們在尺度因子為1.2的8個尺度圖像下提取FAST角點，對於分辨率為512X384至752X480的圖像提取1000個角點，對於圖像分辨率為1241X376的KITTI數據集中提取2000個角點。為了確保提取的角點分布均勻，將圖像劃分若幹個網格（cell），並保證在每個網格上至少提取5個角點。如果提取的角點不夠則自適應調整角點的檢測閾值，從而保證總角點數量的一致。最後計算每個FAST角點的方向及相應的ORB描述子，用於特征匹配。
B、基於前一幀的位姿初始化（Initial Pose Estimation from Previous Frame）
如果上一幀被成功追蹤到，使用勻速運動模型來估計當前幀的位姿，並且將上一幀觀測到的地圖點投影到當前幀確定匹配搜索範圍。如果沒有搜索到足夠的匹配點，則擴大搜索的範圍。通過計算匹配到的3D-2D點來初始化當前幀相機的位姿。
C、基於全局重定位的位姿初始化（Initial Pose Estimation via Global Relocalization）
如果當前幀追蹤失敗，則將當前幀轉化為詞袋向量並通過查詢關鍵幀數據庫來實現全局重定位。對每一個關鍵幀，通過使用RANSAC算法和PnP算法來估計當前幀相機的位姿，如果足夠多的內點，則確定候選關鍵幀，通過確定搜索範圍搜索更多的3D-2D匹配點，進一步優化當前幀相機的位姿。
D、基於局部地圖的追蹤（Track Local Map）
基於前一幀的位姿初始化，是建立當前幀與上一幀之間的聯系；基於全局重定位的位姿初始化，是建立當前幀與某一關鍵幀的聯系；基於局部地圖的位姿估計，是建立當前幀與局部關鍵幀地圖的聯系，也就是通過搜索更多的3D-2D的匹配點來對更精確的估計當前幀的位姿。
一旦我們獲取了相機的初始化位姿和一組3D-2D的匹配點，我們可以將地圖和當前幀聯系起來從而得到更多的3D-2D匹配對應關系。為了減少大地圖的復雜性，我們只映射局部地圖。局部地圖包括一個幀集合K1（與當前幀有超過一定數量相同地圖點的關鍵幀），和一個幀集合K2（與K1有超過一定數量相同的地圖點但是與當前幀無相同的地圖點的關鍵幀）組成。局部地圖中定義一個參考關鍵幀Kf，它是與當前幀共有最多地圖點的幀。
針對局部地圖中的每一個地圖點在當前幀中搜索：
（1）計算地圖點X在當前幀中的投影，如果該投影超出圖像邊界則刪除該地圖點；
（2）計算當前幀的視角射線v與地圖點平均視角方向n的夾角，如果vn < cos(60°)則刪除該地圖點；
（3）計算地圖點到相機中心的距離d，如果該距離不在地圖點的尺度不變區域內（d∉ [dmin, dmax]）則剔除該地圖點。
（4）計算當前幀的尺度因子，比值為d/dmin。
（5）比較地圖點的描述子D和當前幀中還未匹配的ORB特征點，選擇尺度因子和距離最近的作為最優匹配對。
相機的位姿最終通過所有的3D-2D匹配點進行優化。
E、選擇新關鍵幀（New Keyframe Decision）
跟蹤線程除了計算每幀的位姿，還有一個很重要的目的是確定當前幀是否可作為關鍵幀。由於在局部建圖線程中會剔除冗余的關鍵幀，所以在關鍵幀插入時應該盡可能的迅速，從而使得追蹤線程對相機的運動（尤其是旋轉運動）更具有魯棒性。我們根據下列的要求插入新的關鍵幀：
（1）距離上一次全局重定位超過20幀；
（2）局部建圖線程處於空閑，或者距離上一次關鍵幀插入超過20幀；
（3）當前幀至少能追蹤50個地圖點；
（4）當前幀能追蹤到的地圖點要少於參考關鍵幀Kf中地圖點的90%。
其中條件1確保一個良好的重定位，條件3保證一個良好的追蹤，條件4保證了一個最小的視角變化。
六、局部建圖（Local Mapping）
局部建圖線程處理每個關鍵幀，主要做以下兩件事：（1）管理地圖點和關鍵幀，（2）局部BA優化
A、插入新的關鍵幀（KeyFrame Insertion）
每插入一個關鍵幀，首先更新共視圖，包括增加一個新的節點Ki和更新關鍵幀中擁有共視點的邊信息。然後更新本質圖，即更新關鍵幀中擁有最多共視點的邊信息。最後計算當前關鍵幀的詞袋模型表示。
B、創建新的地圖點（New MapPoint Creation）
通過三角化共視圖中相連關鍵幀的ORB特征點來創建新的地圖點。對於關鍵幀Ki中的未匹配ORB特征點，在其他關鍵幀中查找匹配點。刪除不滿足對極約束的匹配點對，然後三角化匹配點對生成新的地圖點，檢查是否滿足景深為正，視差和重投影誤差、尺度一致性。最後將該地圖點投影到其它關鍵幀中尋找匹配。
C、地圖點剔除（Recent MapPoint Culling）
一個地圖點想要保留在地圖中必須通過嚴格的檢驗，即它能夠在被創建後的連續三個關鍵幀被追蹤到，這樣是為了剔除錯誤三角化的地圖點。一個良好的地圖點需要滿足以下兩個條件：
（1）超過25%的幀數在理論上可以觀察到該地圖點；
（2）該地圖點創建後能夠被至少三個連續關鍵幀追蹤到。
當然即使地圖點滿足以上條件，還是有可能會被刪除，比如某個關鍵幀被刪除或者當做局部BA的外點被剔除等。
D、局部地圖的BA優化（Local Bundle Adjustment）
局部BA優化的對象有當前關鍵幀Ki、共視圖中的相鄰關鍵幀Kc和這些關鍵幀觀測到的地圖點，另外可以觀測到這些地圖點但是不與當前幀相鄰的關鍵幀僅作為約束條件而不作為優化的對象。
E、關鍵幀剔除（Local Keyframe Culling）
隨著新關鍵幀的插入，為了不增加BA優化的壓力，和維持系統的可持續運行，需要刪除冗余的關鍵幀。如果某一關鍵幀觀測到的90%地圖點的能夠被至少三個其他關鍵幀觀測到，那麽剔除該關鍵幀。
七、閉環優化（Loop Closing）
閉環優化線程處理局部建圖後的關鍵幀，主要做以下兩件事：（1）閉環檢測；（2）全局優化。
A、檢測閉環候選幀（Loop Candidates Detection）
首先我們計算當前關鍵幀Ki與共視圖中相鄰關鍵幀Kc詞袋向量的相似度，並保留最小相似度分數Smin。然後查詢關鍵幀數據庫，剔除相似度分數低於最小分數的關鍵幀和直接相鄰的關鍵幀，將其他關鍵幀作為閉環候選幀。最後通過檢測連續三個閉環候選幀的一致性來選擇閉環幀。
B、計算Sim3相似變換（Computer the Similarity Transformation）
在單目SLAM系統中，地圖（關鍵幀和地圖點）有七個自由度可以漂移，包括三個平移自由度、三個旋轉自由度和一個尺度自由度。通過計算當前幀Ki與關鍵幀Kl的相似變換，不僅可以計算出閉環處的累計誤差，而且可以判斷該閉環的有效性。
首先計算當前幀和閉環候選幀中ORB特征點對應的地圖點，得到當前幀與每個閉環候選幀的3D-3D對應關系。然後通過對每個閉環候選幀采用RANSAC算法叠代，試圖計算出相似變換。如果某閉環候選幀的相似變換具有足夠多的內點，通過尋找更多匹配點優化，如果仍具有足夠多的內點，則該閉環候選幀為閉環幀。
C、閉環融合（Loop Fusion）
從上一步可以得到當前幀的閉環幀和相似變換，回環優化的第一步就是在當前幀融合地圖點和在共視圖中插入新的邊信息。首先當前關鍵幀Ki的位姿Tiw通過相似變換校正，並校正當前關鍵幀的相鄰幀Kc的位姿Tcw。回環幀Kl和它相鄰幀Klc的地圖點投影到當前幀Ki和它的相鄰幀Kc中尋找匹配點對，所有匹配的3D-3D地圖點和相似變換的內點進行融合。所有參與融合的關鍵幀都更新他們在共視圖中的邊信息，並創建與回環幀相鄰的邊。
D、基於本質圖的優化（Essential Graph Optimization）
為了有效的閉合回環，我們在本質圖上進行位姿圖優化。
八、試驗
我們在大場景機器人序列-NewCollege數據集上評估ORB-SLAM系統的整體性能，在16組手持室內RGBD序列-TUM數據集上評估系統的定位精度、重定位和可持續性能。在10組汽車室外序列-KITTI數據集中評估系統在大場景下的實時性能，定位精度和位姿圖優化的效率。實驗電腦配置為：Intel Core i7-4700MQ（4 [email protected]）和8Gb內存。
A、基於NewCollege數據集的系統性能
NewCollege數據集是通過移動機器人采集校園而形成的2.2km圖像序列，該序列由雙目相機以20fps的速率采集，圖像的分辨率為512X382。該數據集包含了幾個大回環和快速旋轉的情況，使得此前無單目SLAM可以跑完整個數據集。
從圖4可知，ORB-SLAM系統檢測到出NewCollege數據集中的閉環，並且具有局夠多的內點來計算出相似變換矩陣。
圖5顯示了回環優化前後的全局地圖，藍色表示回環檢測，紅色表示局部地圖。
圖6表示ORB-SLAM處理完整個數據集序列後的全局地圖，由於右邊大的循環是反方向測得數據，不能夠檢測到閉環，所以不能進行閉環優化。
特征追蹤和局部建圖所消耗的時間如表1所示，我們可以看出，特征追蹤線程能夠能夠以幀率為25-30Hz的速度處理圖像，其中追蹤局部地圖最為消耗時間。在局部建圖線程中，局部BA優化最為消耗時間。
表2表示ORB-SLAM系統處理數據集中6個回環時時間。我們可知回環檢測的時間與關鍵幀數量呈現亞線性關系，這是因為查詢關鍵幀數據庫只需要比較圖像中公共的視覺單詞，從而視覺詞典可用於地點識別。另外本質圖中邊的數量僅為節點數量的5倍左右，因此是一個相當稀疏的地圖。
B、基於TUM RGBD數據集的定位精度
由於TUM RGBD數據集提供相機的真實運動軌跡，所以能夠測量ORB-SLAM系統的定位精度。將ORB-SLAM系統與PTAM系統、LSD-SLAM系統、RGBD-SLAM系統進行比較，運行TUM數據集中的的16組序列，比較他們的均方根誤差（RMSE）。
從表3可知，ORB-SLAM系統能夠處理所有的序列除了fr3_nostructure_texture_far(fr3_nstr_tex_far)序列，該序列為平面場景，相機位姿在平面場景中由兩種可能的解。ORB-SLAM系統初始化時檢測到了歧義後拒絕初始化地圖，然而PTAM系統初始化後有時效果好有時差，LSD-SLAM系統初始化後誤差非常大。
在開環序列中，ORB-SLAM系統和PTAM系統的定位精度相差不大，但是在大場景閉環序列-fr3_nostructure_texture_near _withloop (fr3 nstr tex near)中，ORB-SLAM的定位精度明顯優於PTAM。令人驚訝的是，PTAM和ORB-SLAM系統的定位精度都要優於LSD-SLAM和RGBD-SLAM系統。一個可能的原因是，前者使用了位姿圖優化，而且沒有使用傳感器測量的深度數據。另一個有趣的結果是，從fr2_desk_with_person and fr3_walking_xyz序列運行結果可以看出，LSD-SLAM系統在動態環境下具有較差的魯棒性。
C、基於TUM RGBD數據集的重定位試驗
D、基於TUM RGBD數據集的可持續實驗
E、基於KITTI數據集的大尺度和大閉環的實驗
九、結論和討論
A、結論
B、稀疏特征法VS稠密直接法
C、未來展望
論文：ORB-SLAM：a Versatile and accurate Monocular SLAM System