博主github：https://github.com/MichaelBeechan
博主CSDN：https://blog.csdn.net/u011344545
ORB-SLAM：一種通用的(全能的)精確的單目SLAM系統
ORB-SLAM: A Versatile and Accurate Monocular SLAM System
原作者：Raul Mur-Artal, J. M. M. Montiel, Member, IEEE, and Juan D. Tardos, Member, IEEE
翻譯時間：2017.8.31
譯者：Michael Beechan（陳兵） 重慶理工大學
引用：Mur-Artal R, Montiel J M M, Tardós J D. ORB-SLAM: A Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.
ORB-SLAM2：https://github.com/MichaelBeechan/ORB_SLAM2
ORB-SLAM: http://webdiis.unizar.es/~raulmur/orbslam/
摘要
本文提出了ORB-SLAM，在大小場景、室內室外環境下都可以實時操作的一種基於特徵的單目SLAM系統。系統對複雜的劇烈運動具有魯棒性，允許寬基線的閉環和重定位，且包含完整的自動初始化。基於最近幾年的優秀算法之上，我們從頭開始設計了一種新穎的系統，它對所有SLAM任務使用相同的特徵：追蹤、建圖、重定位和閉環。合適策略的存在使得選擇的重建點和關鍵幀具有很好的魯棒性，並能夠生成緊湊的可追蹤的地圖，只有當場景內容發生變化地圖才改變，從而允許長時間操作。本文從最受歡迎的數據集中提供了27個序列的詳盡評估。相對於其他最先進的單目SLAM方法，ORB-SLAM實現了前所未有的性能。爲了社會的利益，我們將源代碼公開。
關鍵字：Lifelong mapping, localization, monocular vision, recognition, simultaneous localization and mapping (SLAM).
1 引言
捆綁調整（BA）提供相機定位的精確估計以及稀疏幾何重建[1,2]，並且提供了強烈的匹配網絡和良好的初始猜測。一段長的時間，這種方法被認爲不符合實時性的應用，如在視覺SLAM上（Visual SLAM）。視覺SLAM估計相機軌跡的目標是重建環境。現在，我們知道爲了不以過高的計算成本獲得準確的結果，實時SLAM算法必須向BA提供以下信息。
1） 對場景特徵(地圖點)的相應觀察是在候選幀(關鍵幀)的子集中。(匹配)
2） 隨着關鍵幀複雜性的增大，它們的選擇應該避免不必要的冗餘。(篩選)
3） 關鍵幀和點的強大網絡配置產生精確結果，那就是說，一系列關鍵幀的觀察點，具有明顯的視差和大量的閉環匹配。
4） 採用非線性優化初始化估計關鍵幀位姿和點位置。
5） 構建局部地圖過程中，其優化集中在實現良好的穩定性。
6） 執行快速全局優化（例如，位姿圖）和實時閉合循環的能力。
Mouragon等人[3]在視覺里程計上完成了BA的第一個實時應用，隨後是Klein和Murray
[4]實現了里程碑性的工作，提出了PTAM(Parallel tracking and mapping)。這個算法雖然是有限的小尺度的操作，提供簡單而且有效的對每一幀關鍵幀的選擇，特徵匹配，點三角測量，相機定位的方法，以及跟蹤失敗後重定位。遺憾的是，有不少因素限制了它的應用：缺少閉環和適當的處理遮擋，重定位視點的low不變性和需要人爲干預地圖引導。
在本研究中，我們依賴PTAM的主要觀點，Galvez-Lopez和Tardos [5]的位置識別工作，Strasdat等人[6]的閉環尺度意識和[4,5]中使用共視信息進行大尺度操作，設計了ORB-SLAM。一個新穎的單目SLAM系統主要有以下貢獻。
1） 對所有任務使用相同的特性：追蹤、建圖、重定位和閉環。這使得我們的系統更有效率，更簡單，更可靠。使用ORB特徵[9]，允許沒有GPUs的情況下可以應用於實時系統中，在視角和光照變化時提供良好的不變性。
2） 在大環境中實時操作。由於共視圖（covisibility graph）的使用，跟蹤和建圖集中在一個與全局地圖大小獨立的局部共視區域。
3） 實時閉環基於位姿圖的優化，我們稱爲本質圖（Essential Graph）。它由系統維護的生成樹構建，閉環鏈接和strong edges(強邊緣)來自共視圖。
4） 實時相機重定位，對視角和光照具有顯着的不變性。這允許從跟蹤失敗中恢復，並且還增強了地圖重用。
5） 一個基於模型選擇的新的自動的魯棒的初始化過程允許創建初始化一個平面和非平面的場景地圖。
6） 一個優勝略汰的方法選擇地圖點和關鍵幀，generous in the spawning but very restrictive in the culling.這種策略提升了跟蹤的魯棒性，增強了永久操作性，因爲冗餘的關鍵幀被丟棄了。
我們對室內和室外環境的公共數據集進行了廣泛的評估，包括手持式，汽車和機器人序
列。特別地，我們在直接方法[10]中實現了比現有技術更好的相機定位精度，其直接優於像素強度而不是特徵重投影誤差。我們在第IX-B節中討論基於特徵的方法比直接方法更準確的可能原因。
這裏介紹的閉環和重定位方法是基於我們以前的工作[11]。在[12]中提出了系統的初步版本。在本文中，我們添加了初始化方法，本質圖，並完善了所有涉及的方法。我們還詳細描述了所有構建塊，並進行了詳盡的實驗驗證。
據我們所知，這是單目SLAM最完整可靠的解決方案，爲了社區的利益，我們將源代碼公開。示範視頻和代碼可以在我們的項目網頁中找到。
2 相關工作
A . 位置識別
Williams等人[13]的綜述比較了位置識別的一些方法並且得出了技術基於外觀的結論，也就是說，在大環境中圖像到圖像的匹配比地圖到地圖或圖像到地圖的方法尺度更好。在基於外觀(圖像)的方法中，詞袋技術[14]，如概率方法FAB-MAP[15]，由於他很高的效率是最好的選擇。DBoW2 [5]首次使用從BRIEF描述符[16]獲得的二進制單詞包以及使用非常有效的FAST特徵檢測器[17]。與目前爲止用在詞袋方法的SURF [18]和SIFT [19]特徵相比，特徵提取所需的時間大大超過一個數量級。雖然系統證明是非常有效和魯棒的，但使用的BRIEF，既沒有旋轉不變性也沒有尺度不變性，從類似的角度將系統限制在平面軌跡和循環檢測中。在我們以前的工作[11]中，我們提出了一個用ORB建立在DBoW2上的一個詞袋識別器[9]。ORB是旋轉和縮放（在一定範圍內）不變的二進制特徵，導致非常快的識別對場景具有良好的不變性。我們在四個不同的數據集中展現了識別器的高回調和魯棒性，需要少於39 ms（包括特徵提取）從10 K圖像數據庫中檢索循環候選。在這項研究中，我們使用改進版本的識別器，使用共視信息，並在查詢數據庫時返回幾個假設，而不僅僅是最佳匹配。
B．地圖初始化
由於深度不能從單一的圖像中恢復，單目SLAM需要一個創建初始地圖的過程。一個解決此問題的方法是初始化跟蹤一個知道的結構[20]。在過濾方法的上下文中，點可以使用反深度參數化[21]，將點的深度不確定度初始化爲很高，希望之後能夠收斂到其實際位置。Engel等人[10]最近的半稠密工作遵循類似的方法將像素的深度初始化爲具有高方差的隨機值。
從兩個視圖的初始化方法可以假設局部場景平面度[4]，[22]，並使用Faugeras和Lustman [23]的方法從單應性上恢復相機相對姿態，或使用Nister [26]的五點法在[25] 模型平面和一般場景下計算一個本質矩陣[24]，需要處理多種解決方案。這兩種重建方法在低視差下都沒有很好的約束，如果平面場景的所有點都更靠近其中一個攝像機的中心，則會遭受雙重歧義解決方案[27]。另一方面，如果使用視差觀察非平面場景，則可以使用八點算法[2]計算唯一的基本矩陣，並且可以無歧義地恢復相對相機姿態。
在第四節中，我們提出了一種基於平面場景的單應性和非平面場景的基本矩陣之間新的自動模型選擇方法。Torr等人提出了模型選擇的統計方法[28]。在類似的原理下，我們已經開發了一種啓發式初始化算法，其考慮到接近退化情況（即，平面，近平面和低視差）選擇基本矩陣的風險，有利於選擇單應性。在平面情況下，爲了安全起見，如果解決方案有二重歧義，我們不會初始化，因爲可以選擇損壞的解決方案。我們延遲初始化，直到該方法產生具有顯着視差的唯一解。
C. 單目SLAM
單目SLAM最初是通過濾波解決的[20,21,29,30]。在該方法中，每個幀由濾波器處理以共同估計地圖特徵位置和相機姿態。它具有在處理連續幀中浪費計算的缺點，具有很少的新信息和線性化誤差的累積。另一方面，基於關鍵幀的方法[3]，[4]僅使用選擇的幀（關鍵幀）估計地圖，允許執行成本更高的但準確的BA優化，因爲映射不與幀速率相關聯。Strasdat等人[31]表明，基於關鍵幀的技術比同樣計算成本的濾波更爲準確。
最具代表性的基於關鍵幀的SLAM系統可能是Klein和Murray的PTAM [4]。這是在並行線程引入分割相機跟蹤和地圖創建想法的第一個工作，並證明在小環境中成功應用於增強現實。原始版本後來改進了邊緣特徵，跟蹤期間的旋轉估計步驟和更好的重定位方法[32]。PTAM的地圖點與FAST角點通過補丁(patch)相關性匹配。這使得點僅對跟蹤有用，但不適用於位置識別。實際上，PTAM沒有檢測到大的循環，重定位是基於低分辨率關鍵幀縮略圖的相關性，產生了對視點的低不變性。
Strasdat等人[6]提出了一種在GPU上實現的基於光流的前端大尺度單目SLAM系統，其次是FAST特徵匹配和僅運動的BA，以及基於滑動窗口BA的後端。通過具有相似約束（7自由度（DoF））的姿態圖優化來解決閉環檢測，其能夠校正出現在單目SLAM中的尺度漂移。從這項工作，我們採取7自由度姿態圖優化的閉環的想法，並將其應用於第III-D節中基本圖的定義。
Strasdat等人[7]使用PTAM的前端，但僅在從共視圖中檢索到的局部地圖中執行跟蹤。 他們提出了一種雙窗口優化後端，在內部窗口中持續執行BA，並在有限大小的外部窗口中構造圖形。但是，只有在外部窗口的大小足夠大可以包括整個環時，閉環纔有效。在我們的系統中，我們利用了基於共視圖使用局部地圖的優秀想法，並從共視圖中構建位姿圖，但將其應用於完全重新設計的前端和後端。另一個區別是，我們不使用特定功能檢測環路（SURF），而是在相同的跟蹤和映射功能上進行位置識別，獲得魯棒的幀率重定位和環路檢測。
Pirker等人[33]提出了CD-SLAM，即一個非常完整的系統，包括閉環，重定位，大尺度操作以及努力處理動態環境。但是，沒有提到地圖初始化。缺乏公共實施不允許我們對準確性，魯棒性或大尺度能力進行比較。
宋等人的視覺里程計[34]使用ORB特徵進行跟蹤和一個時間滑動窗口BA後端。相比之下，我們的系統更全面，因爲它們沒有全局重定位，閉環，並且不重複使用地圖。他們還使用從相機到地面的已知距離來限制單目尺度漂移。
Lim等人[25]，在我們提交了這項工作的初步版本[12]後發表的工作，也使用相同的功能進行跟蹤，映射和循環檢測。然而，BRIEF的選擇使得系統限制在平面軌跡上。他們的系統只跟蹤最後一個關鍵幀的點數; 因此，如果重新訪問（類似於視覺測距），並且無限制增長的問題，地圖是不會重複使用的。我們在第VIII-E部分將我們的結果與這種方法進行了定性比較。
Engel等人最近的工作 [10]，被稱爲LSD-SLAM，能夠使用直接方法（即直接優於圖像像素強度優化）而不是BA對特徵構建大尺度的半稠密地圖。它們的結果非常令人印象深刻，因爲系統能夠實時操作，無需GPU加速，構建半稠密地圖，相比於產生稀疏輸出的基於特徵的SLAM，LSD-SLAM具有更多潛在的機器人應用。然而，它們仍然需要環路檢測的特徵，並且它們的相機定位精度顯著低於我們的系統和PTAM，如我們在第VIII-B節中實驗所示。這個令人驚訝的結果在第IX-B節中討論。
在直接和基於特徵的方法之間是Forster等人[22]的半直接視覺里程計SVO。無需在每個幀中提取特徵，它們能夠在四旋翼飛機上以高速率幀進行操作，從而獲得令人印象深刻的結果。然而，不執行迴環檢測，目前的實現主要是向下看相機。
最後，我們要討論關鍵幀選擇。文獻中所有視覺SLAM的作品都認爲運行BA在所有點和所有框架是不可行的。Strasdat等人的工作 [31]表明，最具成本效益的方法是保持儘可能多的點，同時只保留非冗餘關鍵幀。PTAM方法是非常謹慎地插入關鍵幀，以避免計算複雜度的過度增長。這種限制性關鍵幀插入策略使得跟蹤在艱難的探索條件下失效。我們的測試策略的生存通過儘可能快地插入關鍵幀，在困難的情況下實現前所未有的魯棒性，並且刪除更多冗餘的框架，以避免額外的成本。
3.系統概況
A．特徵選擇
我們系統的主要設計思想之一是使用映射和跟蹤所使用的相同特徵進行位置識別，以執行幀速率重定位和循環檢測。這使得我們的系統有效，並避免了像以前的作品一樣插入SLAM附近的深度特徵識別[6]，[7]。我們要求每張圖像提取特徵的時間少於33 ms，不包括流行的SIFT（〜300 ms）[19]，SURF（〜300 ms）[18]或最近的A-KAZE（〜100 ms）[35]。爲了獲得一般的位置識別能力，我們要求旋轉不變性，不包括BRIEF [16]和LDB [36]。
我們選擇了ORB [9]，它們是一個帶有一個256位描述符關聯的多尺度FAST角。他們非常快速的計算和匹配，而他們有很好的視角不變性。這使我們能夠用比較寬的基線匹配，提高了BA的準確性。我們已經在[11]中展示了ORB在位置識別中的良好性能。雖然我們目前的實現使用ORB，但是所提出的技術不限於這些特徵。
B．三線程：跟蹤，局部建圖和迴環
我們的系統，見圖1的概述，包含並行運行的三個線程：跟蹤，局部建圖（局部映射）和迴環。跟蹤負責每個幀相機的定位，並決定何時插入新的關鍵幀。我們首先執行與前一幀的初始特徵匹配，並使用僅運動的（motion-only）BA優化位姿。如果跟蹤丟失（例如，由於遮擋或突然移動），則使用位置識別模塊來執行全局重定位。一旦有了相機姿態和特徵匹配的初步估計，使用由系統維護的關鍵幀的共視圖來檢索局部可視圖（參見圖2（a）和（b））。然後，通過重新投影搜索與局部地圖點的匹配，並且使用所有匹配再次優化相機姿態。最後，跟蹤線程決定是否插入新的關鍵幀。所有跟蹤步驟將在第五部分中詳細介紹。創建初始映射的新穎過程見第四部分。
局部映射處理新的關鍵幀並執行局部BA以在相機姿態的周圍實現最佳重建。新的對應策略就是，對於在新的關鍵幀中不匹配的ORB尋找共視圖中關鍵幀到三角分解的新點的聯繫。創建後的一段時間，根據跟蹤期間收集到的信息，應用緊急點選擇策略，以保留高質量點。局部映射還負責剔除冗餘關鍵幀。我們在第六節中詳細說明所有局部映射步驟。
閉環搜索每個新關鍵幀的循環。如果檢測到一個循環，我們計算一個相似變換，報告循環中的漂移積累。然後，循環的兩側對齊，並融合重複點。最後，對相似約束[6]進行位姿圖優化，以實現全局一致性。主要的新穎之處在於我們對本質圖進行優化，即第三部分D中解釋的共視圖的稀疏子圖。循環檢測和校正步驟在第七節中有詳細的說明。
我們使用g2o [37]中實現的Levenberg-Marquardt算法進行所有優化。在附錄中，我們描述了每個優化中涉及的錯誤項，成本函數和變量。
Fig. 1. ORB-SLAM system overview, showing all the steps performed by the tracking, local mapping, and loop closing threads. The main components of the place recognition module and the map are also shown.
C．地圖點和關鍵幀的選擇
每個地圖點pi存儲以下內容：
1)世界座標系下的3D位置Xw,I；
2)視角方向ni，所有視角方向的平均單位矢量（通過點和關鍵幀光學中心的連線觀察它）；
3)一個代表性的ORB描述子Di，是 ORB相關描述符，其漢明距離相對於觀察點的關鍵幀中的所有其他相關描述符最小;
4)根據ORB特徵的尺度不變性約束，可以觀測地圖點的最大距離dmax和最小距離dmin。
每個關鍵幀Ki存儲以下內容：
1)相機位姿Tiw，一種從世界座標系到相機座標系的剛體變換。
2)相機內參，包括焦距和主焦點。
3)所有被提取的ORB特徵，和地圖點關聯或者沒關聯的，如果提供了一個失真模型，其座標是不失真的。
地圖點和關鍵幀用一種generous policy生成，後來非常緊急的剔除機制負責檢測冗餘關鍵幀和錯誤匹配或不可追蹤的地圖點。這允許在勘探期間靈活的地圖擴展，其在硬條件（例如，旋轉，快速移動）下提高跟蹤魯棒性，同時其大小被限制在連續重新訪問相同的環境，即終身操作。此外，與PTAM相比，我們的地圖包含非常少的離羣值，而不包含較少的積分。 地圖點和關鍵幀的剔除程序分別在第VI-B和VI-E節中進行了說明。
D. 共視圖(Covisibility Graph)和本質圖
關鍵幀之間的共視信息對於我們系統的的一些任務的非常有用的，被表示爲無向加權圖，如[7]。每個節點是一個關鍵幀，如果兩個關鍵幀之間的邊緣共享相同的地圖點（至少15個）的觀測值，那麼它們之間存在邊緣，即邊緣的權重θ普通地圖點的數量。
爲了糾正一個循環，我們執行一個位姿圖優化[6]，它沿着圖形分佈循環閉合誤差。爲了不包括由共視圖提供的所有邊緣，可以非常密集，我們建議構建一個保留所有節點（關鍵幀）但是邊緣較少的基本圖，仍然保留強大的網絡，從而產生準確的結果。該系統從初始關鍵幀增量生成生成樹，其提供了具有最少邊數的共視圖的連接子圖。當插入新的關鍵幀時，它被包含在鏈接到共享大多數觀察點的關鍵幀的樹中，並且當關鍵幀被剔除策略擦除時，系統更新受該關鍵幀影響的鏈接。本質圖包含生成樹，具有高共視性（θm in = 100）的共視圖的邊緣子集，以及循環閉合邊緣，導致強大的相機網絡。圖2示出了共視圖，生成樹和相關聯的基本圖的示例。如第VIII-E節的實驗所示，當執行位姿圖優化時，解決方案非常精確，以至於附加的全BA優化幾乎不能改善解決方案。第VIII-E部分的結尾顯示了本質圖的效率和θm的影響。
Fig. 2. Reconstruction and graphs in the sequence f r3 long of f ice household from the TUM RGB-D Benchmark [38]. (a) Keyframes (blue), current camera (green), map points (black, red), current local map points (red). (b) Covisibility graph. (c) Spanning tree (green) and loop closure (red). (d) Essential graph.
E. 詞袋位置識別
基於DBoW2[5]，系統嵌入一個詞袋位置識別模型，來執行閉環檢測和重定位。視覺單詞僅僅是描述子空間的離散化，稱爲視覺詞典。詞典是從一大堆圖像的ORB描述子特徵中線下產生的。前期工作顯示，如果圖像足夠多，相同的詞典可以在不同的環境中得到好的性能[11]。系統逐漸建立一個包含反轉索引的數據庫，該索引存儲詞彙表中的每個視覺詞，其中看到了關鍵幀，從而可以非常有效地完成查詢數據庫。當關鍵幀被剔除程序刪除時，數據庫也會被更新。
因爲在關鍵幀之間存在視覺重疊，所以在查詢數據庫時，不存在高分的唯一關鍵幀。原來的DBoW2考慮到了這個重疊，加上了時間接近的圖像得分。這具有不包括查看相同位置但在不同時間插入的關鍵幀的限制。相反，我們將在共視圖中連接的關鍵幀進行分組。此外，我們的數據庫返回所有關鍵幀匹配，其分數高於最佳分數的75％。
文獻[5]報道了功能匹配的詞表示的另外的好處。當我們想要計算兩組ORB特徵之間的對應關係時，我們可以將暴力匹配僅限於屬於詞彙樹中同一個節點的某些特徵（我們選擇六分之二），加快搜索。當搜索匹配以對三分法新點進行循環檢測和重新定位時，我們使用這個技巧。我們還通過定向一致性測試（參見[11]的細節）來改進對應關係，拋棄異常值，確保所有對應關係的連貫輪換。
4. 自動地圖初始化
地圖初始化的目的是計算兩個幀之間的相對姿態，以對初始的一組地圖點進行三角測量。該方法應與場景（平面或一般）無關，並且不需要人爲干預來選擇良好的雙視圖配置，即具有顯着視差的配置。我們建議並行計算兩個幾何模型：假設平面場景的單應性和假設非平面場景的基本矩陣。然後，我們使用啓發式選擇一個模型，並嘗試使用所選模型的特定方法恢復相對位姿。我們的方法只有在確定雙視圖配置安全，檢測低視差情況和衆所周知的雙重平面模糊度時才初始化[27]，避免初始化損壞地圖。我們的算法步驟如下。
1）找初始對應(匹配)：提取當前幀Fc的ORB特徵(僅在最好的尺度下)，並且尋找與參考幀Fr的匹配Xc<—>Xr。如果沒能找到足夠的匹配，重置參考幀。
2）兩個模型的平行計算：計算平行線程單應矩陣Hcr和基本矩陣Fcr
用歸一化的DLT和八點算法，分別在文獻[2]中的一個RANSAC方案中被解釋。爲了使兩個模型的過程均勻，迭代次數以及每次迭代使用的點數被置於兩個模型的前面：八個基本矩陣，四個單應矩陣。在每次迭代中，我們計算每個模型M的一個評分 (H代表單應矩陣，F代表基本矩陣)
其中和表示從一幀到另一幀對稱轉移誤差[2]。表示基於95 %的分佈的異常值剔除閾值(TH=5.99，TF=3.84，假設測量誤差的標準差爲1像素點)。被定義等於TH，以至於在他們的內點區域中相同的d具有相等的模型得分，再一次使得過程齊次。
我們保持單應矩陣和基本矩陣具有最高的得分。如果沒有模型能得到高分(沒有足夠的內點)，重新第一步過程。
3）模型選擇：如果場景是平面，幾乎是平面或者又很低的視差，可以通過單應矩陣表示。然而，基本矩陣也能處理，但問題是沒有好的約束[2]，且任何從基本矩陣試圖恢復的運動會產生錯誤的結果。我們應該選擇單應性，因爲重建方法可以從平面正確初始化，否則會檢測到低視差情況，拒絕初始化。另一方面，具有足夠視差的非平面場景只能由基本矩陣來解釋，但如果它們位於平面上或者它們具有較低的視差（它們很遠）來說明匹配的子集，也可以發現單應性。在這種情況下，我們應該選擇基本矩陣。我們發現一個魯棒的啓發式來計算
如果，選擇單應矩陣，可以適當的獲得平面和低視差。否則選擇基本矩陣。
4）運動和從運動中恢復結構：一旦一個模型被選擇，我們檢索相關的運動假設。單應矩陣的情況，我們用Faugeras and Lustman的方法檢索八個運動假設[23]。該方法提出了方便性測試來選擇有效的解決方案。然而，如果存在低視差，這種測試將失敗，由於點容易在相機前後跑，這也可能會選擇一種錯誤的解決方案。我們建議直接對八個解決方案進行三角測量，並檢查是否有一個解決方案，具有視差的大多數點，在兩個相機前面，並且具有低重投影錯誤。如果沒有明確的解決方案，我們不會從步驟1初始化和繼續。這種解決方案的方法使我們的初始化在低視差和雙重歧義配置下是穩定的，並且可以被認爲是我們方法魯棒性的關鍵。
基本矩陣的情況，我們用標定矩陣K轉換爲一個本質矩陣：
然後用奇異值分解法檢索四個運動假設[2]。我們對四種解決方案進行三角測量，並選擇重建作爲單應性。
5）BA(Bundle Adjustment)：最後，我們執行完整的BA（詳見附錄），以改進初始重建。
戶外New College機器人序列[39]中的一個具有挑戰性的初始化的例子如圖3所示。可以看出，PTAM和LSD-SLAM如何在平面上初始化所有點，而我們的方法已經等到有足夠的視差，從基本矩陣正確初始化。
5. 跟蹤
在本節中，我們將描述跟蹤線程在相機每幀執行的步驟。在幾個步驟中提到的相機姿態優化包括僅在運動中的BA，其在附錄中描述。
A．ORB提取
我們在8層金字塔上提取FAST角點，尺度因子爲1.2。對於從512×384到752×480像素的圖像分辨率，我們發現適合提取1000個角，對於更高的分辨率，如KITTI數據集中的1241×376 [40]，我們提取2000個角。爲了確保均勻分佈，我們用網格劃分每一個層級，試圖提取每個單元格至少五個角點。然後，我們檢測每個單元格中的角點，如果沒有找到足夠的角點，則調整檢測器閾值。如果某些單元格不含角點（無紋理或低對比度），則每個單元格保留的角點數也會適應。接着在保留的FAST角點上計算方向和ORB描述符。在所有特徵匹配中使用ORB描述符，與PTAM中的補丁相關搜索相反。
B．從最後一幀初始化位姿估計
如果跟蹤在最後一幀成功，我們使用勻速運動模型來預測相機姿態，並對最後一幀中觀察到的地圖點進行引導搜索。如果沒有找到足夠的匹配（即明顯違反了運動模型），則我們在最後一幀中圍繞其位置使用更廣泛的地圖點搜索。然後利用找到的對應關係優化位姿。
C．通過全局重定位初始化位姿估計
如果跟蹤丟失，我們將幀轉換成詞袋，並將查詢的識別數據庫用於全局重定位的關鍵幀候選。我們在每個關鍵幀中計算與地圖點相關聯的ORB，如第III-E節所述。然後，我們對每個關鍵幀執行所選的RANSAC迭代，並嘗試使用PnP算法找到相機姿態[41]。 如果我們找到具有足夠內聯的相機位姿，我們可以優化位姿，並對候選關鍵幀的地圖點進行更多匹配的引導搜索。最後，再次優化相機姿態，如果支持足夠的內聯，跟蹤過程將繼續進行。
D．跟蹤局部地圖
一旦我們估計了攝像機姿態和一組初始的特徵匹配，我們可以將地圖投影到幀中，並搜索更多的地圖對應點。爲了限制大型地圖的複雜性，我們只投影一個局部地圖。該局部地圖包含與當前幀共享地圖點的關鍵幀K1和與共視圖中的關鍵幀K1的鄰近的集合K2。局部地圖也有一個參考關鍵幀Kref∈K1，其與當前幀共享大多數地圖點。現在，如下在K1和K2中搜索當前幀看到的每個地圖點。
1）計算當前幀中地圖點投影x，如果投影在圖像外面則丟棄。
2）計算當前視線方向向量v與地圖點雲平均視線方向向量n的夾角，捨棄v*n<cos(60)的點雲。
3）計算地圖點到相機中心的距離d，如果不在地圖點的尺度不變區域內則捨棄，即
4）計算幀的尺度因子，爲。
5）將地圖點的特徵描述符D與還未匹配的ORB特徵進行比較，找到在預測尺度範圍內和x附近，與最佳匹配相關聯的地圖點。
相機姿態最終用幀中的所有映射點進行優化。
E．新關鍵幀的選擇
最後一步是確定當前幀是否被生成爲一個新的關鍵幀。由於局部建圖(局部映射)中存在一種機制來剔除冗餘關鍵幀，因此我們將嘗試儘可能快地插入關鍵幀，因爲這樣可以使追蹤更具魯棒性，以適應挑戰性的相機移動（通常是旋轉）。要插入新的關鍵幀，必須滿足以下所有條件。
1）從上一次全局重定位已經超過20幀。
2）局部建圖(局部映射)是空閒的，或者從最後一個關鍵幀插入已經超過20幀。
3）當前幀至少跟蹤50點。
4）當前幀跟蹤Kref小於90％的點。
我們不是將其他關鍵幀的距離標準用作PTAM，而是施加最小視覺變化（條件4）。條件1確保良好的重新定位和條件3良好的跟蹤。如果在局部建圖(局部映射)繁忙（條件2的第二部分）中插入了一個關鍵幀，則發送一個信號來停止局部BA，以便它可以儘快處理新的關鍵幀。
參考文獻：
[1] B. Triggs, P. F. Mc Lauchlan, R. I. Hartley, and A. W. Fitzgibbon, “Bundle adjustment a modern synthesis,” in Vision Algorithms: Theory and Practice. New York, NY, USA: Springer, 2000, pp. 298–372.
[2] R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, 2nd ed., Cambridge, U.K.: Cambridge Univ. Press, 2004.
[3] E. Mouragnon, M. Lhuillier, M. Dhome, F. Dekeyser, and P. Sayd, “Real time localization and 3D reconstruction,” in Proc. IEEE Comput. Soc. Conf. Comput. Vision Pattern Recog., 2006, vol. 1, pp. 363–370.
[4] G. Klein and D. Murray, “Parallel tracking and mapping for small AR workspaces,” in Proc. IEEE ACM Int. Symp. Mixed Augmented Reality, Nara, Japan, Nov. 2007, pp. 225–234.
[5] D. G´alvez-L ´opez and J. D. Tard ´os, “Bags of binary words for fast place recognition in image sequences,” IEEE Trans. Robot., vol. 28, no. 5, pp. 1188–1197, Oct. 2012.
[6] H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Scale drift-aware large scale monocular SLAM,” presented at the Proc. Robot.: Sci. Syst., Zaragoza, Spain, Jun. 2010.
[7] H. Strasdat, A. J. Davison, J. M. M. Montiel, and K. Konolige, “Double window optimisation for constant time visual SLAM,” in Proc. IEEE Int. Conf. Comput. Vision, Barcelona, Spain, Nov. 2011, pp. 2352–2359.
[8] C. Mei, G. Sibley, and P. Newman, “Closing loops without places,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Taipei, Taiwan, Oct. 2010, pp. 3738–3744.
[9] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “ORB: An efficient alternative to SIFT or SURF,” in Proc. IEEE Int. Conf. Comput. Vision, Barcelona, Spain, Nov. 2011, pp. 2564–2571.
[10] J. Engel, T. Sch ¨ops, and D. Cremers, “LSD-SLAM: Large-scale direct monocular SLAM,” in Proc. Eur. Conf. Comput. Vision, Zurich, Switzerland, Sep. 2014, pp. 834–849.
[11] R. Mur-Artal and J. D. Tard ´os, “Fast relocalisation and loop closing in keyframe-based SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., Hong Kong, Jun. 2014, pp. 846–853.
[12] R. Mur-Artal and J. D. Tard ´os, “ORB-SLAM: Tracking and mapping recognizable features,” presented at the MVIGRO Workshop Robot. Sci. Syst., Berkeley, CA, USA, Jul. 2014.
[13] B. Williams, M. Cummins, J. Neira, P. Newman, I. Reid, and J. D. Tard ´os,“A comparison of loop closing techniques in monocular SLAM,” Robot. Auton. Syst., vol. 57, no. 12, pp. 1188–1197, 2009.
[14] D. Nister and H. Stewenius, “Scalable recognition with a vocabulary tree,” in Proc. IEEE Comput. Soc. Conf. Comput. Vision Pattern Recog., New York, NY, USA, Jun. 2006, vol. 2, pp. 2161–2168.
[15] M. Cummins and P. Newman, “Appearance-only SLAM at large scale with FAB-MAP 2.0,” Int. J. Robot. Res., vol. 30, no. 9, pp. 1100–1123,2011.
[16] M. Calonder, V. Lepetit, C. Strecha, and P. Fua, “BRIEF: Binary robust independent elementary features,” in Proc. Eur. Conf. Comput. Vision, Hersonissos, Greece, Sep. 2010, pp. 778–792.
[17] E. Rosten and T. Drummond, “Machine learning for high-speed corner detection,” in Proc. Eur. Conf. Comput. Vision, Graz, Austria, May 2006, pp. 430–443.
[18] H. Bay, T. Tuytelaars, and L. Van Gool, “SURF: Speeded up robust features,” in Proc. Eur. Conf. Comput. Vision, Graz, Austria, May 2006, pp. 404–417.
[19] D. G. Lowe, “Distinctive image features from scale-invariant keypoints,” Int. J. Comput. Vision, vol. 60, no. 2, pp. 91–110, 2004.
[20] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “Mono SLAM: Real-time single camera SLAM,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 29, no. 6, pp. 1052–1067, Jun. 2007.
[21] J. Civera, A. J. Davison, and J. M. M. Montiel, “Inverse depth parametrization for monocular SLAM,” IEEE Trans. Robot., vol. 24, no. 5, pp. 932–945, Oct. 2008.
[22] C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Autom., Hong Kong, Jun. 2014, pp. 15–22.
[23] O. D. Faugeras and F. Lustman, “Motion and structure from motion in a piecewise planar environment,” Int. J. Pattern Recog. Artif. Intell., vol. 2, no. 03, pp. 485–508, 1988.
[24] W. Tan, H. Liu, Z. Dong, G. Zhang, and H. Bao, “Robust monocular SLAM in dynamic environments,” in Proc. IEEE Int. Symp. Mixed Augmented Reality, Adelaide, Australia, Oct. 2013, pp. 209–218.
[25] H. Lim, J. Lim, and H. J. Kim, “Real-time 6-DOF monocular visual SLAM in a large-scale environment,” in Proc. IEEE Int. Conf. Robot. Autom., Hong Kong, Jun. 2014, pp. 1532–1539.
[26] D. Nist´er, “An efficient solution to the five-point relative pose problem,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 26, no. 6, pp. 756–770, Jun. 2004.
[27] H. Longuet-Higgins, “The reconstruction of a plane surface from two perspective projections,” Proc. Royal Soc. London Ser. B, Biol, Sci., vol. 227, no. 1249, pp. 399–410, 1986.
[28] P. H. Torr, A. W. Fitzgibbon, and A. Zisserman, “The problem of degeneracy in structure and motion recovery from uncalibrated image sequences,” Int. J. Comput. Vision, vol. 32, no. 1, pp. 27–44, 1999.
[29] A. Chiuso, P. Favaro, H. Jin, and S. Soatto, “Structure from motion causally integrated over time,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 24, no. 4, pp. 523–535, Apr. 2002.
[30] E. Eade and T. Drummond, “Scalable monocular SLAM,” in Proc. IEEE Comput. Soc. Conf. Comput. Vision Pattern Recog., New York, NY, USA, Jun. 2006, vol. 1, pp. 469–476.
[31] H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Visual SLAM: Why filter?” Image Vision Comput., vol. 30, no. 2, pp. 65–77, 2012.
[32] G. Klein and D. Murray, “Improving the agility of keyframe-based SLAM,” in Proc. Eur. Conf. Comput. Vision, Marseille, France, Oct. 2008, pp. 802–815.
[33] K. Pirker, M. Ruther, and H. Bischof, “CD SLAM-continuous localization and mapping in a dynamic world,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., San Francisco, CA, USA, Sep. 2011, pp. 3990–3997.
[34] S. Song, M. Chandraker, and C. C. Guest, “Parallel, real-time monocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Autom., 2013, pp. 4698–4705.
[35] P. F. Alcantarilla, J. Nuevo, and A. Bartoli, “Fast explicit diffusion for accelerated features in nonlinear scale spaces,” presented at the Brit. Mach. Vision Conf., Bristol, U.K., 2013.
[36] X. Yang and K.-T. Cheng, “LDB: An ultra-fast feature for scalable augmented reality on mobile devices,” in Proc. IEEE Int. Symp. Mixed Augmented Reality, 2012, pp. 49–57.
[37] R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A general framework for graph optimization,” in Proc. IEEE Int. Conf. Robot. Autom., Shanghai, China, May 2011, pp. 3607–3613.
[38] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A benchmark for the evaluation of RGB-D SLAM systems,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Vilamoura, Portugal, Oct. 2012, pp. 573–580.
[39] M. Smith, I. Baldwin, W. Churchill, R. Paul, and P. Newman, “The new college vision and laser data set,” Int. J. Robot. Res., vol. 28, no. 5, pp. 595–599, 2009.
[40] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics: The KITTI dataset,” Int. J. Robot. Res., vol. 32, no. 11, pp. 1231–1237, 2013.
[41] V. Lepetit, F. Moreno-Noguer, and P. Fua, “EPn P: An accurate O(n) solution to the Pn P problem,” Int. J. Comput. Vision, vol. 81, no. 2, pp. 155–166, 2009.
[42] B. K. P. Horn, “Closed-form solution of absolute orientation using unit quaternions,” J. Opt. Soc. Amer. A, vol. 4, no. 4, pp. 629–642, 1987.
[43] F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard, “3-D mapping with an RGB-D camera,” IEEE Trans. Robot., vol. 30, no. 1, pp. 177–187, Feb. 2014.
[44] R. A. Newcombe, S. J. Lovegrove, and A. J. Davison, “DTAM: Dense tracking and mapping in real-time,” in Proc. IEEE Int. Conf. Comput. Vision, Barcelona, Spain, Nov. 2011, pp. 2320–2327.
[45] S. Lovegrove, A. J. Davison, and J. Ibanez-Guzm´an, “Accurate visual odometry from a rear parking camera,” in Proc. IEEE Intell. Vehicles Symp., 2011, pp. 788–793.
[46] P. H. Torr and A. Zisserman, “Feature based methods for structure and motion estimation,” in Vision Algorithms: Theory and Practice. New York, NY, USA: Springer, 2000, pp. 278–294.
[47] R. Mur-Artal and J. D. Tardos, “Probabilistic semi-dense mapping from highly accurate feature-based monocular SLAM,” presented at the Proc. Robot.: Sci. Syst., Rome, Italy, Jul. 2015.
[48] H. Strasdat, “Local accuracy and global consistency for efficient visual SLAM,” Ph.D. dissertation, Imperial College London, London, U.K., Oct. 2012.