[論文學習] ORB-SLAM: a Versatile and Accurate Monocular SLAM System

轉自：https://blog.csdn.net/darlingqiang/article/details/78840627

ORB-SLAM: a Versatile and Accurate Monocular SLAM System

原文發表于：IEEE Transactions on Robotics (Impact Factor: 2.43). 10/2015

摘要：

本文主要講了ORB-SLAM，一個基于特征識别的單目slam系統，可以實時運作，适用于各種場合，室内的或者室外的，大場景或小場景。系統具有很強的魯棒性，可以很好地處理劇烈運動圖像、可以有比較大的餘地自由處理閉環控制、重定位、甚至全自動位置初始化。基于近年來的優秀算法，我們對系統做了精簡，采用了所有SLAM相同功能：追蹤，地圖建構，重定位和閉環控制。選用了比較适合的政策，地圖重構的方法采用雲點和關鍵幀技術，具有很好的魯棒性，生成了精簡的、可追蹤的地圖，當場景的内容改變時，地圖建構可持續工作。我們用最流行的圖像資料集測試了27個圖像序列。相比最新的單目SLAM，ORB SLAM性能優勢明顯。我們在網站上公布了源代碼。

一  簡介

由于比較強的比對網絡和初始化位置估計，BA廣泛應用于相機位置的準确估計和離散幾何重構。在一段比較長的時間裡，這種方法被認為不适合實時圖像系統，比如vSLAM。vSLAM系統在建構環境的同時需要估計相機的軌迹。基于現有的硬體裝置，現在可以獲得比較好的計算結果，将BA應用于實時SLAM系統中：

• 在候選圖像幀子集中（關鍵幀）比對觀測的場景特征（地圖雲點）。
• 由于關鍵幀數量的增長，需要做篩選避免備援。
• 關鍵幀和雲點的網絡配置可以産生精确的結果，也就是，分布良好的關鍵幀集合和有明顯視差、大量閉環比對的觀測雲點。
• 關鍵幀和雲點位置的初始估計，采用非線性優化的方法。
• 在建構局部地圖的過程中，優化的關鍵是獲得良好的穩定性。
• 本系統可以實時執行快速全局優化（比如位置地圖）閉環回路。

B A出現于PTAM中，第一次實時應用是視覺裡程。盡管受制于小場景的應用，這個算法對關鍵幀的選擇，特征比對，雲點三角化，每幀相機位置估計，追蹤失敗後的重定位非常有效。不幸的是幾個關鍵因素限制了它的應用：缺少閉環控制和足夠的阻塞處理，較差的視圖不變特性和在形成地圖過程中需要人工幹預。

為了完成這些工作，我們采用的技術來源于，PTAM、place recognization、scale-aware loop closing和大場景的視圖關聯資訊。

單目ORB SLAM系統包含：

• 對所有的任務采用相同的特征，追蹤、地圖建構、重定位和閉環控制。這使得我們的系統更有效率、簡單可靠。ORB特征，在沒有GPU的情況下可以應用于實時圖像系統中，具有很好的旋轉不變特性。
• 可應用于實時戶外環境操作。由于其視圖内容關聯的特性，追蹤和地圖建構可在局部視圖關聯中處理，這可以獨立于全局視圖進行工作。
• 基于位置優化的實時閉環控制，我們稱作Essential Graph。它通過生成樹建構，生成樹由系統、閉環控制連結和視圖内容關聯強邊緣進行維護。
• 實時相機重定位具有明顯的旋轉不變特性。這就使得跟蹤丢失可以重做，地圖也可以重複使用。
• 選擇不同的模型可以建立不同的平面或者非平面的初始化地圖；自動的、具有良好魯棒性的初始化過程也是基于模型而選擇。
• 大量地圖雲點和關鍵幀，需要經過嚴格的挑選，必須找到一個最合适的辦法。好的挑選方法可以增強追蹤的魯棒性，同時去除備援的關鍵幀以增強程式的可操作性。

我們在公共資料集上對程式在室内和室外環境進行了評估，包括手持裝置、汽車和機器人。相機的位置比現在最新的方法更精确，它通過像素擴充集進行優化、而不是特征的重映射。我們還讨論了提高基于特征的方法的準确性的原因。

閉環控制和重定位的方法是基于我們之前的工作論文11。系統最初的版本是論文12。本文中我們添加了初始化的方法，Essential graph 和其他方法。我們詳細了描述了系統的各個闆塊，并且做了實驗進行驗證。就我們所知，這是目前最完整最可靠的單目SLAM系統。視訊示範和源代碼放在我們的項目網站上。

二  相關工作

A.     位置識别

論文13比較幾種基于圖像處理技術的位置識别的方法，其中有圖像到圖像的比對，在大環境下比地圖到地圖或圖像到地圖方法尺度特性更好。圖像方法中，詞袋模型的效率更高，比如基于機率論的FAB-MAP。由BRIEF特征描述子和FAST特征檢測産生的二進制詞袋可以用DBoW2獲得。與SURF和SIFT相比，它的特征提取運算時間減小一個數量級。盡管系統運作效率高、魯棒性好，采用BRIEF不具有旋轉不變性和尺度不變性，系統隻能運作在平面軌迹中，閉環檢測也隻能從相似的視角中獲得。在我們之前的工作中，我們用DBoW2生成了基于ORB的詞袋模型位置識别器。ORB是具有旋轉不變和尺度不變特性的二進制特征，它是一種高效的具有良好針對視圖不變的識别器。我們在4組不同的資料集上示範了位置識别功能，複用性好，魯棒性強，從10K圖像資料庫中提取一個候選閉合回路的運算時間少于39毫秒。在我們的工作中，我們提出了一種改進版本的位置識别方法，采用内容相關的視圖，檢索資料庫時傳回幾個前提而不是最好的比對。

B.     地圖初始化

單目SLAM通過圖像序列生成初始化地圖，單一圖像并不能重建深度圖。解決這個問題的一種方法是一開始跟蹤一個已知的圖像結構。在濾波方法中，用機率論方法從逆深度參數方法得到深度圖中，初始化地圖雲點，與真實的位置資訊融合。論文10中，采用類似的方法初始化像素的深度資訊得到随機數值。

通過兩個局部平面場景視圖進行初始化的方法，從兩個相關相機（視圖）位姿進行3D重構，相機的位姿關系用單映射表示，或者計算一個基本矩陣，通過5點算法建構平面模型或者一般場景模型。兩種方法都不會受到低視差的限制，平面上的所有的點也不需要靠近相機中心。另外，非平面場景可以通過線性8點算法來計算基本矩陣，相機的位姿也可以重構。

第四章詳細講述了一個全新的自動方法，它基于平面單映射或非平面的基本矩陣。模型選擇的統計方法如論文28較長的描述。基于相似變換理論，我們開發了初始化算法，選擇退化二次曲線例子中基本矩陣，或單映射矩陣。在平面例子中，為了程式穩定性，如果選擇情況模糊不清，我們盡量避免做初始化，否則方案可能崩潰。我們會延遲初始化過程，直到所選的方案産生明顯的視差。

C.     Monocular SLAM （單目SLAM）

Mono-SLAM可以通過濾波方案初始化。在這種方案，每一幀都通過濾波器估計地圖特征位置和相機位姿，将其關聯。處理連續的圖像幀需要進行大量運算，線性誤差會累積。由于地圖建構并不依賴于幀率，基于關鍵幀的方法，用篩選的關鍵幀估計地圖，采用精确的BA優化。論文31示範了基于關鍵幀的地圖方法比濾波器方法在相同的運算代價上更精确。

基于關鍵幀技術最具代表性的SLAM系統可能是PTAM。它第一次将相機追蹤和地圖建構分開，并行計算，在小型場合，如增強現實領域非常成功。PTAM中的地圖雲點通過圖像區塊與FAST角點比對。雲點适合追蹤但不适合位置識别。實際上，PTAM并不适合檢測大的閉合回路，重定位基于低分辨率的關鍵幀小圖像塊，對視圖不變性較差。

論文6展示了大場景的單目SLAM系統，前端用GPU光流，用FAST特征比對和運動BA；後端用滑動視窗BA。閉環檢測通過7自由度限制的相似變換位姿圖優化，能夠校正單目系統中的尺度偏移。我們采用這種7自由度的位姿圖優化方法，應用到我們的Essential Graph方法中，如第三章D節裡面較長的描述。

論文7用，PTAM的前端，通過内容相關的視圖提取局部地圖執行追蹤。他們使用兩個視窗優化後端，在内部視窗中采用BA，在外部一個限制大小的視窗做位姿圖。隻有在外部視窗尺寸足夠大可以包含整個閉環回路的情況下，閉環控制才能起作用。我們采用了基于内容相關的視圖建構局部地圖的方法，并且通過内容相關的視圖建構位姿圖，同時用它們設計前端和後端。另一個差別是，我們并沒有用特别的特征提取方法做閉合回路檢測（比如SURF方法），而是在相同的追蹤和建圖的特征上進行位置識别，獲得具有魯棒性的重定位和閉環檢測。

論文33 提出了CD-SLAM方法，一個非常複雜的系統，包括閉環控制，重定位，動态環境、大場景下運作。但它的地圖初始化并沒有講。是以沒法做精确性、魯棒性和大場景下的測試對比。

論文34的視覺裡程計方法使用了ORB特征做追蹤，處理BA後端滑動視窗。相比之下，我們的方法更具一般性，他們沒有全局重定位，閉環回路控制，地圖也不能重用。他們使用了相機到地面的真實距離限制單目尺度漂移。

論文25與我們之前的工作論文12一樣，也采用的相同的特征做追蹤，地圖建構和閉環檢測。由于選擇BRIEF，系統受限于平面軌迹。從上一幀追蹤雲點，通路過的地圖不能重用，與視覺裡程計很像，是以系統不能擴充。我們在第三章E小節裡面定性地做了比較。

論文10裡的LSD-SLAM，可以建構大場景的半稠密地圖，特征提取并沒有采用BA方法，而是直接方法（優化也是直接通過圖像像素）。沒有用GPU加速，建構了半稠密地圖，可以運作在實時應用中，非常适合機器人應用，比其他基于特征的稀疏地圖SLAM效果好。但它們仍然需要特征做閉環檢測，相機定位的精度也明顯比PTAM和我們的系統慢，我們将在第8章B小節示範實驗結果。在IX章B小節進行讨論。

論文22提出了介于直接方式和基于特征的方法之間的半直接視覺裡程SVO方法。不需要每幀都提取特征點，可以運作在較高的幀率下，在四軸飛行器上效果很好。然而，沒有閉環檢測，而且隻使用了視角向下的攝像頭。

最後，我們想讨論關鍵幀的選擇。所有的的視覺SLAM用所有的雲點和圖像幀運作BA是不可行的。論文31在保留了盡可能多地非備援關鍵幀和雲點，成本效益較高。PTAM方法非常謹慎插入關鍵幀避免運算量增長過大。這種嚴格限制關鍵幀插入政策可能在未知地圖追蹤使失敗。比較好的政策是在不同的場景下快速插入關鍵幀，然後再移除那些備援的圖像幀，避免額外的運算成本。

三  系統架構

A.     特征選擇

我們系統設計的中心思想是對這些功能采用相同的特征，建構地圖、追蹤、位置識别、基于圖像幀率的重定位和閉環回路檢測。這使得我們的系統更有效率，沒有必要極化特征識别的深度圖，如論文6,7裡讨論的。我們每張圖像的特征提取遠少于33毫秒，SIFT（~300ms）,SURF(~300ms)，A-KAZE（~100ms）。為了獲得一般性的位置識别方法，我們需要特征提取的旋轉不變性，而BRIEF和LDB不具備這樣的特性。

我們選擇了我們選擇了ORB，它是旋轉多尺度FAST角點檢測，具有256位特征描述子。他們計算和比對的速度非常快，同時對視角具有旋轉不變的特性。這樣可以在更寬的基準線上比對他們，增強了BA的精度。我們已經在論文11中示範了ORB位置識别的性能。本文的方案中也采用ORB。

B.     三個線程：追蹤、局部地圖建構和閉環控制

我們的系統如圖一所示，整合了三個并行的線程：追蹤、局部地圖建構和閉環回路控制。追蹤線程通過每幀圖像定位相機位置，決定什麼時候插入一個新的關鍵幀。我們先通過前一幀圖像幀初始化特征比對，采用運動B A優化位姿。如果追蹤丢失，位置識别子產品執行全局重定位。一旦獲得最初的相機位姿估計和特征比對，通過内容相似視圖的關鍵幀提取一個局部可視化地圖，如圖2a,b所示。然後進行映射搜尋局部地圖雲點的比對，根據比對優化相機位姿。最後，追蹤線程決定是否插入新的關鍵幀。所有的追蹤步驟将在第5章詳細解釋。建立初始化地圖将在第4章進行說明。

局部地圖建構處理新的關鍵幀，在相機位姿的環境中執行局部BA優化重構。根據互動視圖中已經連接配接的關鍵幀，搜尋新關鍵幀中未比對的ORB特征的對應關系，來三角化新的雲點。有時盡管已經建立了新的雲點，基于追蹤線程過程中新收集的資訊，為了獲得高品質的雲點，根據雲點篩選政策可能會臨時删除一些點。局部地圖建構也負責删除備援關鍵幀。我們将在第6章詳細說明局部地圖建構的步驟。

對每個新的關鍵幀都要進行閉環搜尋，以确認是否形成閉環。如果閉環被偵測到，我們就計算相似變換來檢視閉環的累積誤差。這樣閉環的兩端就可以對齊，重複的雲點就可以被融合。最後，相似限制的位姿圖優化確定全局地圖一緻性。本文主要通過關鍵圖像（Essential Graph）進行優化，它是一個互動視圖中離散的子圖像，第三章D小節較長的描述。閉環檢測和校驗步驟将在第7章較長的描述。

我們使用g2o庫中的Levenverg-Marquardt算法執行全局優化。我們在附錄中描述了每個優化的誤差，計算成本和變量。

A.     地圖雲點、關鍵幀和選擇标準

每個地圖雲點pi儲存：

• 它在世界坐标系中的3D位置Xw,i
• 視圖方向ni，也就是所有視圖方向的平均機關向量（這個方向是以觀察關鍵幀的光學中心到雲點的方向）
• ORB特征描述子Di，已經觀測到雲點的關鍵幀與它關聯的所有其他特征描述子相比，它漢明距離最小
• 根據ORB特征尺度不變性限制，可觀測的雲點的最大距離dmax和最小距離dmin

每個關鍵幀Ki儲存：

• 相機位姿Tiw，從世界坐标轉換成相機坐标
• 相機内參，包括主點和焦點距離
• 所有從圖像幀提取的ORB特征，不管是否已經關聯了地圖雲點，雲點的坐标經過校準模型矯正過

地圖雲點和關鍵幀通過多重政策建立，在稍後挑選機制上将檢測備援的關鍵幀和錯誤比對的雲點或不可追蹤的地圖雲點。系統在運作的過程中，地圖擴充的彈性就比較好，在比較惡劣的環境下（比如視圖旋轉，快速移動）還是需要增強魯棒性，在同一環境的重複通路情況下地圖的大小是有限的，可以控制的。另外，與PTAM相比，由于包含的雲點比較少，我們的地圖包含的無效資料也更少一些。地圖雲點和關鍵幀的篩選過程将在第6章B節和E節分别解釋。

B.     内容關聯視圖和重要視圖

關鍵幀之間的視圖内容相關資訊在系統的幾個功能上都非常有用，如論文7所述，它表示了一個間接的權重圖像。每個節點都是一個關鍵幀，關鍵幀之間存在一個邊緣，幀上面可以觀察到相同的地圖雲點（至少有15個），對于邊緣上相同的雲點的數量我們賦予權重θ。

為了矯正閉環回路，我們像論文6那樣做位姿圖優化，優化方法将閉環回路的誤差分散到圖像裡。為了排除内容相關視圖的邊緣，可能非常密集，我們建構了關鍵圖像（Essential Graph）保留所有的節點（關鍵幀），但是邊緣更少，這可以保持一個比較強的網絡以獲得精确的結果。系統增量式地建構一個生成樹，從第一個關鍵幀開始，它連接配接了邊緣數量最少的内容相關視圖的子圖像。當新的關鍵幀插入時，它加入樹中連接配接到老的關鍵幀上，新舊關鍵幀具有最多的相同的雲點，但一個關鍵幀通過篩選政策删除時，系統會根據關鍵幀所在的位置更新連結。關鍵圖像（Essential Graph）包含了一個生成樹，具有高視圖相關性（θmin=100）的相關視圖的邊緣子集，閉環回路邊緣産生一個相機的強網絡。圖2是一個相關視圖的例子，生成樹和關聯的關鍵圖像。第8章E節的實驗裡，運作位姿圖優化時，方案效果精确BA優化幾乎沒有增強系統效果。關鍵圖像的效果和θmin的效果如第8章E節所示。

C.     基于圖像詞袋模型的位置識别

采用DBoW2，系統嵌入了圖像詞袋位置識别子產品，執行閉環檢測和重定位。視覺單詞離散分布于特征描述子空間，視覺單詞組成視覺字典。視覺字典是離線建立的，用ORB特征描述子從大量圖像中提取。如果圖像都比較多，相同的視覺字典在不同的環境下也能獲得很好的性能，如論文11那樣。系統增量式地建構一個資料庫，包括一個逆序指針，存儲每個視覺字典裡的視覺單詞，關鍵幀可以通過視覺字典檢視，是以檢索資料庫效率比較高。當關鍵幀通過篩選程式删除時，資料庫也會更新。

關鍵幀在視圖上可能會存在重疊，檢索資料庫時，可能不止一個高分值的關鍵幀。DBoW2認為是圖像重疊的問題，就将時間上接近的所有圖像的分值相加。但這并沒有包括同一地點不同時間的關鍵幀。我們将這些與内容相關視圖的關鍵幀進行分類。另外，我們的資料庫傳回的是分值高于最好分值75%的所有關鍵幀。

詞袋模型表示的特征比對的另外一個優勢在論文5裡詳細介紹。如果我們想計算兩個ORB特征的對應關系，我們可以通過暴力比對視覺字典樹上某一層（6層裡面選第2層）的相同節點（關鍵幀）裡的特征，這可以加快搜尋。在閉環回路檢測和重定位中，我們通過這個方法搜尋比對用作三角化新的雲點。我們還通過方向一緻性測試改進對應關系，具體如論文11，這可以去掉無效資料，保證所有對應關系的内在方向性。

四全自動地圖初始化

地圖初始化的目的是計算兩幀之間的相關位姿來三角化一組初始地圖雲點。這個方法與場景圖像不相關（平面的或一般的）而且不需要人工幹預去選擇一個好的兩個視圖對應關系，比如具有明顯視差。我們建議并行計算兩個幾何模型，平面視圖的單映射和非平面視圖的基本矩陣。我們用啟發式的方法選擇模型，并從相關位姿中進行重構。當個視圖之間的關系比較确定時，我們才發揮作用，檢測低視差的情況或已知兩部分平面模糊的情況（如論文27所示），避免生成一個有缺陷的地圖。這個算法的步驟是：

1. 查找最初的對應關系：

從目前幀Fc提取ORB特征（隻在最好的尺度上），與參考幀Fr搜尋比對Xc<-->Xr。如果找不到足夠的比對，就重置參考幀。

1. 并行計算兩個模型：

在兩個線程上并行計算單映射Hcr和基本矩陣Fcr：

Xc=HcrXr                                 XcTFcrXr=0                             （1）

在《多視圖幾何》裡詳細解釋了分别使用歸一化直接線性變換DLT和8點算法計算原理，通過RANSAC計算。為了使兩個模型的計算流程盡量一樣，将兩個模型的疊代循環次數預先設定成一樣，每次疊代的雲點也一樣，8個基本矩陣，4個單映射。每次疊代我們給每一個模型M（H表示單映射，F表示基本矩陣）計算一個數值SM：

其中，dcr2和drc2是幀和幀之間對稱的傳遞誤差。TM是無效資料的排除門檻值，它的依據是X2的95%（TH=5.99, TF=3.84，假設在測量誤差上有1個像素的标準偏差）。τ等于TH，兩個模型在有效資料上對于同一誤差d的分值相同，同樣使得運算流程保持一緻。

單映射和基本矩陣的分值最高。如果沒有足夠的有效資料，模型沒有找到，算法流程重新開機，從第一步開始重新尋找。

1. 模型的選擇：

如果場景是平面的，靠近平面有一個低視差，可以通過單映射來描述。同樣地，我們也可以找到一個基本矩陣，但問題不能很好地限制表示，從基本矩陣重構運動場景可能會導緻錯誤的結果。我們應該選擇單映射作為重構的方法，它可以從二維圖像正确初始化或者檢測到的低視差的情況而不進行初始化工作。另外一方面，非平面場景有足夠的視差可以通過基本矩陣來表示，但單映射可以用來表示二維平面或者低視差的比對子集。在這種情況下我們應該選擇基本矩陣。我們用如下公式進行計算：

如果RH>0.45 , 這表示二維平面和低視差的情況。其他的情況，我們選擇基本矩陣。

1. 運動和運動結構重構

一旦選擇好模型，我們就可以獲得運動狀态。在單映射的例子中，我們通過論文23的方法，提取8種運動假設。這個方法通過測試選擇有效的方案。如果在低視差的情況下，雲點跑到相機的前面或後面，測試就會出現錯誤進而選擇一個錯的方案。我們建議直接三角化8種方案，檢查兩個相機前面具有較少的重投影誤差情況下，在視圖低視差情況下是否大部分雲點都可以看到。如果沒有一個明确的方案勝出，我們就不執行初始化，重新從第一步開始。這種方法就提供了一個清晰的方案，在低視差和兩個交叉的視圖情況下，初始化程式更具魯棒性，這是我們方案魯棒性的關鍵所在。在基本矩陣的情況下，我們用校準矩陣和本證矩陣進行轉換：

我們通過論文2中的單值分解方法提取四種運動假設。針對單映射，我們三角化四種方案進行重構。

1. 捆集調整

最後我們執行一個全局捆集調整，如附錄所示，用來優化初始化重構。

圖3所示的論文39，室外環境下初始化工作具有很大挑戰。PTAM和LSD-SLAM初始化二維平面所有雲點，我們的方法是有足夠視差才進行初始化，從基本矩陣正确地初始化。

五追蹤

這章我們詳細介紹追蹤線程通過相機圖像每一幀執行的具體步驟。相機的位置優化，如前幾步提到的，由局部捆集調整構成，如附錄中較長的描述。

A.     ORB特征提取

我們在8層圖像金字塔，提取FAST角點，尺度因子為1.2。如果圖像分辨率從512*384到752*480，我們發現提取1000個角點比較合适，如果分辨率提高，如KITTI資料集，論文40，我們提取2000個角點。為了確定單映射分布，我們将每層分成網格，每格提取至少5個角點。檢測每格角點的時候，如果角點數量不夠，就提高門檻值。角點的數量根據單元格變化，即使單元格檢測不出角點（沒有紋理或對比度低的情況）。再根據保留FAST的角點計算方向和ORB特征描述子。ORB特征描述子用于所有的特征比對，而不是像P T A M那樣根據圖像區塊關聯性進行搜尋。

B.     前一圖像幀作初始位姿估計

如果上一幀的追蹤成功，我們就用同樣的速率運動模型計算相機位置，搜尋上一幀觀測到的地圖雲點。如果沒有找到足夠的比對（比如，運動模型失效），我們就加大搜尋範圍搜尋上一幀地圖雲點附近的點。通過尋找到的對應關系優化相機位姿。

C.     初始化位姿估計與全局重定位

如果跟蹤丢失，我們把目前幀轉換成圖像詞袋，檢索圖像資料庫，為全局重定位查找關鍵幀。我們計算ORB特征和每個關鍵幀的地圖雲點的對應關系，如第三章E節描述。接着，我們對每個關鍵幀執行RANSAC疊代計算，用PnP算法找出相機位置。如果通過足夠的有效資料找到相機位姿，我們優化它的位姿，搜尋候選關鍵幀的地圖雲點的更多的比對。最後，相機位置被進一步優化，如果有足夠的有效資料，跟蹤程式将持續執行。

D.    局部地圖追蹤

一旦我們獲得了相機位姿和一組初始特征比對的估計，我們可以将地圖和圖像幀對應起來，搜尋更多地圖雲點的對應關系。為了減少大地圖的複雜性，我們隻映射局部地圖。局部地圖包含一組關鍵幀K1，它們和目前關鍵幀有相同的地圖雲點，與相鄰的關鍵幀組K2圖像内容相關。局部地圖有一個參考關鍵幀Kref∈K1，它與目前幀具有最多相同的地圖雲點。針對K1, K2可見的每個地圖雲點，我們通過如下步驟，在目前幀中進行搜尋：

1. 計算目前幀中的地圖雲點映射集x。如果位于圖像邊緣外面的點，就丢掉。
2. 計算目前視圖射線v和地圖雲點平均視圖方向n的夾角。如果n< cos(60o)，就丢掉。
3. 計算地圖雲點到相機中心的距離d。如果它不在地圖雲點的尺度不變區間，即d∉[dmin,dmax]，就丢掉。
4. 計算每幀圖像的尺度因子，比值為d/dmin。
5. 對比地圖雲點的特征描述子D和目前幀中還未比對的ORB特征，在尺度因子，和靠近x的雲點作最優比對。

相機位姿最後通過目前幀中獲得所有的地圖雲點進行優化。

E.      新關鍵幀的判斷标準

最後一步是決定目前幀是否可以作為關鍵幀。局部地圖建構的過程中有一個機制去篩選備援的關鍵幀，我們盡可能快地插入關鍵幀，這可以使跟蹤線程對相機的運動更具魯棒性，尤其是旋轉。我們根據以下要求插入新的關鍵幀：

1. 每次全局重定位過程需要超過20個圖像幀。
2. 局部地圖建構處于空閑狀态，或者上一個關鍵幀插入時，已經有超過20個關鍵幀。
3. 目前幀跟蹤至少50個地圖雲點。
4. 目前幀跟蹤少于參考關鍵幀雲點的90%。

與PTAM中用關鍵幀的距離作為判斷标準不同，我們加入一個最小的視圖變換，如條件4要求。條件1 確定一個好的重定位，條件3保證好的跟蹤。當局部地圖建構處于忙狀态（條件2的後半部分）的同時插入關鍵幀的時候，就會發信号去暫停局部捆集調整，這樣就可以盡可能快地去處理新的關鍵幀。

六局部地圖建構

這章我們将描述根據每個新的關鍵幀Ki建構局部地圖的步驟。

A.關鍵幀插入

我們先更新内容相關的視圖，添加節點Ki，更新關鍵幀間具有相同地圖雲點産生的邊緣。我們還要更新生成樹上Ki和其他關鍵幀的連結。然後，計算表示關鍵幀的詞袋，用于資料關聯來三角化新的雲點。

B.地圖雲點篩選

保留在地圖裡的地圖雲點，在最初建立的3個關鍵幀上，需要經過嚴格的檢測，保證它們可以被跟蹤，不會由于錯誤的資料被錯誤地被三角化。一個雲點必須滿足如下條件：

1. 跟蹤線程必須要找到超過25%的關鍵幀。
2. 如果超過一個關鍵幀完成地圖雲點建立過程，它必須至少是能夠被其他3個關鍵幀可被觀測到。

一旦一個地圖雲點通過測試，它隻能在被少于3個關鍵幀觀測到的情況下移除。在局部捆集調整删除無效觀測資料的情況下，關鍵幀才能被篩除掉。這個政策使得我們的地圖包含很少的無效資料。

C.新地圖雲點标準

在内容相關的視圖Kc之間三角化ORB特征向量，可以建立新的地圖雲點。對Ki中每個未比對的ORB特征，和其他關鍵幀中沒有比對的雲點，我們查找一個比對。這個比對過程在第三章E節詳細解釋，丢掉那些不滿足極對限制的比對。ORB特征對三角化後，将要獲得新的雲點，這時要檢查兩個相機視圖的景深，視差，重映射誤差，和尺度一緻性。起初，一個地圖雲點通過2個關鍵幀進行觀測，但它卻是和其他關鍵幀比對，是以它可以映射到其他相連的關鍵幀，按照第5章D節的方法搜尋對應關系。

A.     局部捆集調整

局部捆集調整優化目前處理的關鍵幀Ki, 在互動視圖集Kc中所有連接配接到它的關鍵幀，和所有被這些關鍵幀觀測到的地圖雲點。所有其他能夠觀測到這些雲點的關鍵幀但沒有連接配接到目前處理的關鍵幀的這些關鍵幀會被保留在優化線程中，但會被修複。被标記為無效資料的觀測将在優化中間階段和最後階段被丢棄。附錄有詳細的優化細節。

B.     局部關鍵幀篩選

為了使重構保持簡潔，局部地圖建構盡量檢測備援的關鍵幀，删除它們。這會大有幫助，随着關鍵幀數量的增加，捆集調整的複雜度增加，但關鍵幀的數量也不會無限制地增加，因為它要在同一環境下在整個運作執行操作，除非場景内容發生變化。我們會删除Kc中所有90%的地圖雲點可以至少被其他3個關鍵幀在同一或更好的尺度上觀測到的關鍵幀。

七閉環控制

閉環控制線程擷取Ki，上一個局部地圖建構的關鍵幀，用于檢測和閉合回環。具體步驟如下所示：

A.        候選回環檢測

我們先計算Ki的詞袋向量和它在視圖内容相關的所有鄰近圖像（θmin=30）中計算相似度，保留最低分值Smin。然後，我們檢索圖像識别資料庫，丢掉那些分值低于Smin的關鍵幀。這和DBoW2中均值化分值的操作類似，可以獲得好的魯棒性，它計算的是前一幀圖像，而我們是使用的内容相關資訊。另外，所有連接配接到Ki的關鍵幀都會從結果中删除。為了獲得候選回環，我們必須檢測3個一緻的候選回環（内容相關圖像中的關鍵幀）。如果對Ki來說環境樣子都差不多，就可能有幾個候選回環。

B.        計算相似變換

單目SLAM系統有7個自由度，3個平移，3個旋轉，1個尺度因子，如論文6。是以，閉合回環，我們需要計算從目前關鍵幀Ki到回環關鍵幀Kl的相似變換，以獲得回環的累積誤差。計算相似變換也可以作為回環的幾何驗證。

我們先計算ORB特征關聯的目前關鍵幀的地圖雲點和回環候選關鍵幀的對應關系，具體步驟如第3章E節所示。此時，對每個候選回環，我們有了一個3D到3D的對應關系。我們對每個候選回環執行RANSAC疊代，通過Horn方法（如論文42）找到相似變換。如果我們用足夠的有效資料找到相似變換Sil，我們就可以優化它，并搜尋更多的對應關系。如果Sil有足夠的有效資料，我們再優化它，直到Kl回環被接受。

C.       回環融合

回環矯正的第一步是融合重複的地圖雲點，插入與回環閉合相關的相似視圖的新邊緣。先通過相似變換Sil矯正目前關鍵幀位姿Tiw，這種矯正方法應用于所有Ki相鄰的關鍵幀，執行相似變換，這樣回環兩端就可以對齊。回環關鍵幀所有的地圖雲點和它的近鄰映射到Ki，通過映射在較小的區域内搜尋它的近鄰和比對，如第5章D節所述。所有比對的地圖雲點和計算Sil過程中的有效資料進行融合。融合過程中所有的關鍵幀将會更新它們的邊緣，這些視圖内容相關的圖像建立的邊緣用于回環控制。

D.      本征圖像優化

為了有效地閉合回環，我們通過本征矩陣優化位姿圖，如第三章D節所示，這樣可以将回環閉合的誤差分散到圖像中去。優化程式通過相似變換校正尺度偏移，如論文6。誤差和成本計算如附錄所示。優化過後，每一個地圖雲點都根據關鍵幀的校正進行變換。

八實驗

我們對實驗進行了系統的評估，用Newcollege大場景機器圖像序列，評估整個系統性能；用TUM RGB-D的16個手持式室内圖像序列，評估定位精度，重定位和程式運作能力；用KITTI的10個汽車戶外圖像資料集，評估實時大場景操作，定位精度和位姿圖優化效率。

我們的電腦組態為Intel Core i7-4700MQ （4核@2.40GHz）和8GB RAM，用于實時處理圖像。ORB-SLAM有3個主線程，它和其他ROS線程并行處理。

A.     Newcollege

Newcollege是一個2.2公裡的校園和相鄰的公園的機器人圖像序列。它是雙目相機，幀率20fps，分辨率512x38。它包含幾個回環和快速的旋轉，這對單目視覺非常具有挑戰性。據我們所知，沒有單目系統可以處理整個圖像序列。例如論文7，可以形成回環，也可以應用于大場景環境，但對單目結果隻能顯示序列的一小部分。

圖4顯示的回環閉合線程通過有效資料支援相似變換。圖5對比了回環閉合前後的重構狀況。紅色是局部地圖，回環閉合延伸回環的兩端後的狀況。圖6是實時幀率狀态下處理圖像序列的整個地圖。後邊的大回環并沒有完全閉合，它從另外一個方向穿過，位置識别程式沒能發現閉合回環。

我們統計了每個線程所用時間。表1顯示了追蹤和局部地圖建構所用的時間。跟蹤的幀率大概在25-30Hz，在做局部地圖跟蹤時的要求最高。如有必要，這個時間可以減少，需要限制局部地圖關鍵幀的數量。局部地圖建構線程中要求最高的是局部捆集調整。局部捆集調整的時間根據機器人的狀态變動，向未知環境運動或在一個已經建好地圖的區域運動是不同的，因為在未知環境中如果跟蹤線程插入一個新的關鍵幀，捆集調整會被中斷，如第5章E節所示。如果不需要插入新的關鍵幀，局部捆集調整會執行大量已經設定的疊代程式。

表2顯示了6個閉合回路的結果。可以看到回環檢測是如何亞線性地增加關鍵幀的數量。主要是由于檢索資料庫的效率高，它值比較了具有相同圖像單詞的圖像子集，由此可見用于位置識别的詞袋模型的作用。我們的本征圖像包含的邊緣是關鍵幀數量的5倍，它是一個稀疏圖。

B.     TUM RGB-D定位精度對比

論文38裡，評測的TUM RGB-D資料集，用于評估相機定位精度，它通過外部運動捕捉系統提供了具有精确基準的幾個圖像序列。我們去掉那些不适合純單目SLAM系統的圖像序列，這些序列包含強烈的旋轉，沒有紋理或沒有運動。

為了友善比較，我們還運作了直接的半稠密LSD-SLAM（論文10）和PTAM（論文4）作為對比。我們還比較了RGBD-SLAM（論文43）軌迹作為對比。為了在相同的基準下比較ORB-SLAM，LSD-SLAM和PTAM，我們用相似變換對齊關鍵幀軌迹，在尺度未知的情況下，檢測軌迹的絕對誤差（論文38）。對RGBD-SLAM我們通過相機坐标變換對齊軌迹，同樣的方法檢測尺度是否覆寫良好。LSD-SLAM從随機深度值進行初始化，執行聚類，是以與基準對比的時候，我們會丢掉前10個關鍵幀。對于PTAM，我們從一個好的初始化中，手動選擇兩個關鍵幀。表3 是對16個圖像序列運作5次的中等結果。

ORB-SLAM可以運作所有的圖像序列，除了fr3 nostructure texture far (fr3 nstr tex far)以外。這是一個平面的場景，相機的軌迹可能有兩種解釋，比如論文20中的描述。我們的初始化方法檢測到模糊的視圖，為了安全而不進行初始化。PTAM初始化有時會選擇對的方案，有些可能會選擇錯的方案，但導緻的錯誤可能不能接受。我們沒有注意到LSD-SLAM的2種不同的重構方案，但在這個圖像序列出現的錯誤非常多。其他的圖像序列，PTAM和LSD-SLAM的魯棒性比我們的方法差，容易跟蹤丢失。

關于精度問題，ORB-SLAM和PTAM非常相似，ORB-SLAM在圖像序列fr3 nostructure texture near withloop (fr3 nstr tex near)中檢測大的閉環時，可以獲得更高精度。非常意外的一個結果是PTAM和ORB-SLAM都非常明顯地表現出精度高于LSD-SLAM和RGBD-SLAM。一個可能的原因是它們沒有用傳感器的測量優化位姿圖進而減少了對地圖的優化，但我們采用捆集調整，同時通過傳感器測量優化相機和地圖，通過運動結構的經典算法來解決傳感器測量，如論文2所示。我們在第9章B節進一步讨論了這個結果。另一個有趣的結果是在圖像序列fr2 desk with person 和 fr3 walking xyz中，LSD-SLAM對動态物體的魯棒性相比ORB-SLAM差一些。

我們注意到RGBD-SLAM在圖像序列fr2上尺度上有偏差，可以用7個自由度對齊軌迹明顯減少誤差。最後我們注意到論文10提到在f2_xyz上PTAM的精度比LSD-SLAM低，RMSE是24.28cm。然而，論文沒有給出足夠的細節說明如何獲得這些結果的，我們沒有辦法複現它。

C.     TUM RGB-D重定位性能比較

我們在TUM RGB-D資料集上進行了重定位性能的對比實驗。在第一個實驗中，我們通過fr2_xyz的前30秒建構了一個地圖，執行了全局重定位，評估了位姿精度。對PTAM進行了相同的實驗。圖7是建立初始地圖的關鍵幀，重定位的圖像幀位姿和這些幀的基準。可以看出PTAM由于重定位方法較少的不變性而隻重定位關鍵幀附件的圖像幀。表4顯示了相對基準的調用和誤差。ORB-SLAM比PTAM可以更精準地多定位2倍的圖像幀。在第2個實驗中，我們采用fr3_sitting_xyz初始化地圖，從fr3_walking_xyz重定位所有的圖像幀。這是一個頗具挑戰性的實驗，由于圖像中有人移動造成了阻塞。這裡，PTAM并沒有重定位，而ORB-SLAM重定位78%的圖像幀，如表4所示。圖8顯示了ORB-SLAM重定位的一些挑戰。

D.     TUM RGB-D系統運作實驗對比

之前的重定位實驗表明我們的系統可以從不同的視角定位地圖，在中等動态環境中的魯棒性也較好。這個特性和關鍵幀篩選程式可以在不同的視角和局部動态環境中一直運作到程式結束。

在全靜态場景情況下，即使相機從不同視角觀測場景，ORB-SLAM也可以使關鍵幀數量保持在一個有限的水準内。我們示範了一個圖像序列，相機93秒以内都在拍攝同一張桌子，但視角一直變換，形成一個軌迹。我們對比了我們地圖的關鍵幀數量和PTAM生成的關鍵幀，如圖9所示。可以看到PTAM一直插入關鍵幀，ORB-SLAM會删除備援的關鍵幀。

當然，在整個程式運作過程中，靜态環境下的正常操作是任何SLAM系統的一個基本要求，更引人注意的是動态環境下的狀況。我們在幾個fr3的圖像序列中分析了ORB-SLAM系統的狀況，圖像序列有：sitting xyz, sitting halfsphere, sitting rpy, walking xyz, walking halfspehere 和walking rpy。所有的視訊中，相機都對着桌子，但運動軌迹不同，有人在移動，椅子也被移動了。圖10（a）是地圖中所有關鍵幀的總數量，圖10（b）顯示從圖像幀中建立或删除關鍵幀，從關鍵幀到地圖建構需要多久時間。可以看到前2個圖像序列中新看到（增加）場景時地圖的大小一直在增加。圖10（b）是前2個視訊中建立的關鍵幀。在視訊sitting_rpy和walking_xyz中，地圖沒有增加，地圖是通過已有場景建立。相反，在最後兩個視訊中，有更多的關鍵幀插入但沒有在場景中表示出來，可能由于場景的動态變化。圖10（C）關鍵幀的柱狀圖，它們是從視訊中挑選出來的。大部分的關鍵幀被篩選程式删除了，隻有一小部分留下來了。ORB-SLAM有大量關鍵幀的生成政策，在未知環境下非常有用；後面系統會生成一個小的子集來代表這些關鍵幀。

在整個實驗中，我們系統的地圖根據場景上内容來增加，而不是根據時間，它可以存儲場景的動态變化，對場景的了解非常有用。

E.     KITTI大場景大回環行性能對比

裡程計通過KITTI資料集的11個視訊對比，它是在住宅區駕駛汽車，基準精度非常高，有一個GPS和一個Velodyne Laser Scanner。對單目系統非常有調整性，它裡面有快速旋轉，區域内有大量樹葉，這使資料關聯變得更困難，車速比較快，視訊的幀率隻有10fps。除了視訊01外，ORB-SLAM可以處理其他所有的視訊，01是高速路上的視訊，可追蹤的物體非常少。視訊00,02,05,06,07,09，有閉環回路，系統可以檢測到，并使它閉合。視訊09的閉環隻能在視訊的最後幾個圖像幀裡檢測到，并不是每次都能成功檢測到。

對于軌迹和基準的定性比較如圖11和12所示。在TUMRGB-D對比中，我們可以通過相似變換對齊軌迹的關鍵幀和基準。圖11是定性比較的結果，圖12是論文25中的最新單目SLAM在視訊00,05,06,07和08上執行的結果。除了08有一些偏移以外，ORB-SLAM在這些視訊上的軌迹都很精準。

表5顯示了每個視訊的關鍵幀軌迹的中等RMSE誤差。我們基于地圖尺寸提供了軌迹的誤差。結果表明我們的軌迹誤差吃地圖尺寸的1%左右。大緻範圍低的是視訊03的0.3%高的是視訊08的5%。視訊08中沒有閉環，漂移也沒辦法糾正，因為閉環控制需要獲得更精确的重構。

我們還确認了全局捆集調整的20層疊代如何增強地圖重構，在每個視訊的最後，細節如附錄所示。我們還注意到全局捆集調整可以稍微增加閉環軌迹的精度，但這對開環軌迹有負面影響，這意味着我們的系統已經非常精确了。在有些應用中，如果需要非常精确的結果我們的算法會提供一組比對，需要定義一個比較強的相機網絡，一個初始估計，這樣全局捆集調整疊代次數就會變少。

最後講一下閉環控制和用于本征圖像的邊緣的θmin的效率。視訊09是個閉環，時間長。我們評估的不同的閉環政策。表6是關鍵幀軌迹RMSE和不同情況下沒有閉環控制優化所用的時間，如果直接采用全局捆集調整（20層或100層疊代），如果隻用位姿圖優化（10層疊代不同數量的邊緣），如果先用位姿圖優化再執行全局捆集調整。結果表明，在閉環控制之前，方案就比較糟糕，以至于捆集調整會有融合問題。100層疊代後誤差就非常大。本征圖像優化融合速度更快，結果也更精确。θmin對精度影響并不大，減少邊緣的數量會明顯降低精度。位姿圖優化後再執行一個捆集調整可以增加精度，但時間也增加了。

九結論和讨論

A.     結論

通過實驗，我們的系統可以應用于室内和室外場景，用在汽車、機器人和手持裝置上。室内小場景的精度在1厘米，室外大場景的應用是幾個厘米。

PTAM被認為是目前最精準的單目SLAM。根據離線運動結構問題，PTAM後端捆集調整不能同時加載。PTAM的主要貢獻是給機器人SLAM的研究帶來了新的技術，具有良好的實時性。我們的主要貢獻是擴張了PTAM對于應用環境的多樣性和對系統的互動性。是以，我們挑選了新的方法和算法，整合了前人優秀的地方，如論文5的閉環檢測，論文6,7的閉環控制程式和内容關聯視圖，論文37的g2o優化架構，論文9的ORB特征。目前所知，我們的精度最高，是最可靠最完整的單目SLAM系統。我們的生成和挑選關鍵幀政策可以建立較少的關鍵幀。彈性的地圖探測在條件較差的軌迹上非常有用，比如旋轉和快速移動。在相同場景的情況下，地圖在隻有新内容的情況下才會增長，儲存了不同的視圖外觀。

最後，ORB特征可識别劇烈的視角變換的位置。也可以快速提取特征和比對特征，使得實時追蹤和地圖建構更精确。

B.     基于離散/特征方法與稠密/直接方法對比

論文44的DTAM和論文10的LSD-SLAM可以進行環境的稠密或半稠密重構，相機位姿可以通過圖像像素的亮度直接優化。直接方法不需要特征提取，可以避免人工比對。他們對圖像模糊，低紋理環境和論文45的高頻紋理的魯棒性更好。稠密的重構，對比稀疏的地圖雲點，比如ORB-SLAM或PTAM，對相機定位更有效。

然而，直接方法有他們自己的局限。首先，這些方法采用一個表面回報模型。光電描記法的一緻性要求限制了比對的基準，這要比其他特征窄得多。這對重構的精度影響很大，需要更寬的觀測減少深度的不确定。直接方法，如果準确的模組化，可能會受到快門，自動增益和自動曝光的影響（如TUM RGB-D 的對比測試）。最後，由于直接方法計算要求較高，像DTAM中地圖增量式地擴張，或像LSD-SLAM丢掉傳感器測量，根據位姿圖優化地圖。

相反，基于特征的方法可以在更寬的基準上比對特征，主要得益于其較好地視圖不變特性。捆集調整和相機位姿優化，地圖雲點通過傳感器測量進行融合。在運動結構估計中，論文46已經指出了基于特征的方法相比直接方法的優勢。在我們的實驗中，直接提供了證據，第8章B節，表明精度更高。未來單目SLAM應該會整合兩種最好的方法。

C.     後續工作

系統精度可以進一步增強。這些雲點，如果沒有足夠的視差是不放入地圖中的，但對相機的旋轉非常有用。

另外一種方法是将稀疏地圖更新到稠密地圖，重構更有作用。由于我們關鍵幀的選擇機制，關鍵幀有高精度位姿和更多的内容相關視圖資訊。是以，ORB-SLAM稀疏地圖是一個非常優秀的初始估計架構，比稠密地圖更好。論文47有較長的描述。

附錄： 非線性優化

捆集調整

地圖雲點3D位置 Xw,j∈R3，關鍵幀位姿Tiw∈SE(3)

W表示世界坐标，通過比對的關鍵點Xi,j∈R2減少重投影誤差。

地圖雲點j在關鍵幀i中的誤差是：

其中πi是影射函數：

其中，Riw∈SO(3)，tiw∈R3，分别表示Tiw的旋轉和平移部分

（fi,u , fi,v），（ci,u , ci,v）分别是相機i的焦點距離和主點。

代價函數：

ρh是Huber魯棒代價函數，Ωi,j=δi,j2I2x2是協方差矩陣，與檢測關鍵點的尺度有關。在全局捆集調整中（在初始化地圖中），我們優化了所有雲點和關鍵幀。

鼓勵譯者

你的鼓勵是我寫作的動力，歡迎光臨，謝謝鼓勵。：）

參考文獻

[1] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon,“Bundle adjustment a modern synthesis,” in Vision algorithms: theoryand practice, 2000, pp. 298–372.

[2] R. Hartley and A. Zisserman, Multiple View Geometry in ComputerVision, 2nd ed. Cambridge University Press, 2004.

[3] E. Mouragnon, M. Lhuillier, M. Dhome, F. Dekeyser, and P. Sayd, “Realtime localization and 3d reconstruction,” in Computer Vision and PatternRecognition, 2006 IEEE Computer Society Conference on, vol. 1, 2006,pp. 363–370.

[4] G. Klein and D. Murray, “Parallel tracking and mapping for small ARworkspaces,” in IEEE and ACM International Symposium on Mixed andAugmented Reality (ISMAR), Nara, Japan, November 2007, pp. 225–234.

[5] D. G´alvez-L´opez and J. D. Tard´os, “Bags of binary words for fastplace recognition in image sequences,” IEEE Transactions on Robotics,vol. 28, no. 5, pp. 1188–1197, 2012.

[6] H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Scale drift-awarelarge scale monocular SLAM.” in Robotics: Science and Systems (RSS),Zaragoza, Spain, June 2010.

[7] H. Strasdat, A. J. Davison, J. M. M. Montiel, and K. Konolige,“Double window optimisation for constant time visual SLAM,” in IEEEInternational Conference on Computer Vision (ICCV), Barcelona, Spain,November 2011, pp. 2352–2359.

[8] C. Mei, G. Sibley, and P. Newman, “Closing loops without places,” inIEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), Taipei, Taiwan, October 2010, pp. 3738–3744.

[9] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “ORB: an efficientalternative to SIFT or SURF,” in IEEE International Conference onComputer Vision (ICCV), Barcelona, Spain, November 2011, pp. 2564–2571.

[10] J. Engel, T. Sch¨ops, and D. Cremers, “LSD-SLAM: Large-scale directmonocular SLAM,” in European Conference on Computer Vision(ECCV), Zurich, Switzerland, September 2014, pp. 834–849.

[11] R. Mur-Artal and J. D. Tard´os, “Fast relocalisation and loop closing inkeyframe-based SLAM,” in IEEE International Conference on Roboticsand Automation (ICRA), Hong Kong, China, June 2014, pp. 846–853.

[12] ——, “ORB-SLAM: Tracking and mapping recognizable features,” inMVIGRO Workshop at Robotics Science and Systems (RSS), Berkeley,USA, July 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,”Robotics and Autonomous Systems, vol. 57, no. 12, pp. 1188–1197, 2009.

[14] D. Nister and H. Stewenius, “Scalable recognition with a vocabularytree,” in IEEE Computer Society Conference on Computer Vision andPattern Recognition (CVPR), vol. 2, New York City, USA, June 2006,pp. 2161–2168.

[15] M. Cummins and P. Newman, “Appearance-only SLAM at large scalewith FAB-MAP 2.0,” The International Journal of Robotics Research,vol. 30, no. 9, pp. 1100–1123, 2011.

[16] M. Calonder, V. Lepetit, C. Strecha, and P. Fua, “BRIEF: BinaryRobust Independent Elementary Features,” in European Conference onComputer Vision (ECCV), Hersonissos, Greece, September 2010, pp.778–792.

[17] E. Rosten and T. Drummond, “Machine learning for high-speed cornerdetection,” in European Conference on Computer Vision (ECCV), Graz,Austria, May 2006, pp. 430–443.

[18] H. Bay, T. Tuytelaars, and L. Van Gool, “SURF: Speeded Up RobustFeatures,” in European Conference on Computer Vision (ECCV), Graz,Austria, May 2006, pp. 404–417.

[19] D. G. Lowe, “Distinctive image features from scale-invariant keypoints,”International Journal of Computer Vision, vol. 60, no. 2, pp. 91–110,2004.

[20] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “MonoSLAM:Real-time single camera SLAM,” IEEE Transactions on Pattern Analysisand Machine Intelligence, vol. 29, no. 6, pp. 1052–1067, 2007.

[21] J. Civera, A. J. Davison, and J. M. M. Montiel, “Inverse depthparametrization for monocular SLAM,” IEEE Transactions on Robotics,vol. 24, no. 5, pp. 932–945, 2008.

[22] C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-directmonocular visual odometry,” in Proc. IEEE Intl. Conf. on Robotics andAutomation, Hong Kong, China, June 2014, pp. 15–22.

[23] O. D. Faugeras and F. Lustman, “Motion and structure from motionin a piecewise planar environment,” International Journal of PatternRecognition and Artificial Intelligence, vol. 2, no. 03, pp. 485–508, 1988.

[24] W. Tan, H. Liu, Z. Dong, G. Zhang, and H. Bao, “Robust monocularSLAM in dynamic environments,” in IEEE International Symposium onMixed and Augmented Reality (ISMAR), Adelaide, Australia, October2013, pp. 209–218.

[25] H. Lim, J. Lim, and H. J. Kim, “Real-time 6-DOF monocular visualSLAM in a large-scale environment,” in IEEE International Conferenceon Robotics and Automation (ICRA), Hong Kong, China, June 2014, pp.1532–1539.

[26] D. Nist´er, “An efficient solution to the five-point relative pose problem,”IEEE Transactions on Pattern Analysis and Machine Intelligence,vol. 26, no. 6, pp. 756–770, 2004.

[27] H. Longuet-Higgins, “The reconstruction of a plane surface from twoperspective projections,” Proceedings of the Royal Society of London.Series B. Biological Sciences, vol. 227, no. 1249, pp. 399–410, 1986.

[28] P. H. Torr, A. W. Fitzgibbon, and A. Zisserman, “The problem ofdegeneracy in structure and motion recovery from uncalibrated imagesequences,” International Journal of Computer Vision, vol. 32, no. 1,pp. 27–44, 1999.

[29] A. Chiuso, P. Favaro, H. Jin, and S. Soatto, “Structure from motioncausally integrated over time,” IEEE Transactions on Pattern Analysisand Machine Intelligence, vol. 24, no. 4, pp. 523–535, 2002.

[30] E. Eade and T. Drummond, “Scalable monocular SLAM,” in IEEE ComputerSociety Conference on Computer Vision and Pattern Recognition(CVPR), vol. 1, New York City, USA, June 2006, pp. 469–476.

[31] H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Visual SLAM: Whyfilter?” Image and Vision Computing, vol. 30, no. 2, pp. 65–77, 2012.

[32] G. Klein and D. Murray, “Improving the agility of keyframe-basedslam,” in European Conference on Computer Vision (ECCV), Marseille,France, October 2008, pp. 802–815.

[33] K. Pirker, M. Ruther, and H. Bischof, “CD SLAM-continuous localizationand mapping in a dynamic world,” in IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS), San Francisco,USA, September 2011, pp. 3990–3997.

[34] S. Song, M. Chandraker, and C. C. Guest, “Parallel, real-time monocularvisual odometry,” in IEEE International Conference on Robotics andAutomation (ICRA), 2013, pp. 4698–4705.

[35] P. F. Alcantarilla, J. Nuevo, and A. Bartoli, “Fast explicit diffusion foraccelerated features in nonlinear scale spaces,” in British Machine VisionConference (BMVC), Bristol, UK, 2013.

[36] X. Yang and K.-T. Cheng, “LDB: An ultra-fast feature for scalableaugmented reality on mobile devices,” in IEEE International Symposiumon Mixed and Augmented Reality (ISMAR), 2012, pp. 49–57.

[37] R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard,“g2o: A general framework for graph optimization,” in IEEE InternationalConference on Robotics and Automation (ICRA), Shanghai, China,May 2011, pp. 3607–3613.

[38] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “Abenchmark for the evaluation of RGB-D SLAM systems,” in IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS),Vilamoura, Portugal, October 2012, pp. 573–580.

[39] M. Smith, I. Baldwin, W. Churchill, R. Paul, and P. Newman, “The newcollege vision and laser data set,” The International Journal of RoboticsResearch, vol. 28, no. 5, pp. 595–599, 2009.[40] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics:The KITTI dataset,” The International Journal of Robotics Research,vol. 32, no. 11, pp. 1231–1237, 2013.

[41] V. Lepetit, F. Moreno-Noguer, and P. Fua, “EPnP: An accurate O(n)solution to the PnP problem,” International Journal of Computer Vision,vol. 81, no. 2, pp. 155–166, 2009.

[42] B. K. P. Horn, “Closed-form solution of absolute orientation using unitquaternions,” Journal of the Optical Society of America A, vol. 4, no. 4,pp. 629–642, 1987.

[43] F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard, “3-d mappingwith an rgb-d camera,” IEEE Transactions on Robotics, vol. 30, no. 1,pp. 177–187, 2014.

[44] R. A. Newcombe, S. J. Lovegrove, and A. J. Davison, “DTAM: Densetracking and mapping in real-time,” in IEEE International Conference onComputer Vision (ICCV), Barcelona, Spain, November 2011, pp. 2320–2327.

[45] S. Lovegrove, A. J. Davison, and J. Ibanez-Guzm´an, “Accurate visualodometry from a rear parking camera,” in IEEE Intelligent VehiclesSymposium (IV), 2011, pp. 788–793.

[46] P. H. Torr and A. Zisserman, “Feature based methods for structureand motion estimation,” in Vision Algorithms: Theory and Practice.Springer, 2000, pp. 278–294.

[47] R. Mur-Artal and J. D. Tardos, “Probabilistic semi-dense mapping fromhighly accurate feature-based monocular SLAM,” in Robotics: Scienceand Systems (RSS), Rome, Italy, July 2015.

[48] H. Strasdat, “Local Accuracy and Global Consistency for EfficientVisual SLAM,” Ph.D. dissertation, Imperial College, London, October2012.