作者：Liam
來源：公眾號@3D視覺工坊
摘要
ORB-SLAM3是一個支持視覺、視覺加慣導、混合地圖的SLAM系統，可以在單目，雙目和RGB-D相機上利用針孔或者魚眼模型運行。他是第一個基於特征的緊耦合的VIO系統，僅依賴於最大后驗估計(包括IMU在初始化時)。這樣一個系統的效果就是：不管是在大場景還是小場景，室內還是室外都能魯棒實時的運行，在精度上相比於上一版提升了2到5倍。本文的第二個創新點是根據改進recall的新的重定位模塊來構建的混合地圖，因為這個模塊他可以讓ORB-SLAM3在特征不是很好的場景中長期運行：當里程計失敗的時候，系統會重新構建地圖並將這個地圖和原來構建的地圖對齊。和那些僅利用最新的幾幀數據的里程計相比，ORB-SLAM3是第一個能夠在所有算法階段重用所有先前信息的系統。這樣的機制就可以在BA的時候用有共視關系的關鍵幀，即使兩幀在時間相差很遠，或者來自原來的建圖過程。這個系統在EuRoC數據集上達到了平均3.6cm的精度，在TUM-VI這種利用手持設備快速移動的數據集(AR/VR場景)上達到了9mm的精度。作者已經開源了代碼：https://github.com/UZ-SLAMLab/ORB_SLAM3
一、介紹
SLAM建圖的最大優勢在於，它允許在BA中匹配並使用執行三種數據關聯的先前觀測值：
- 短期的數據關聯：在最新的幾秒中匹配地圖元素。就像是VO中做的一樣，丟掉那些已經看不到的幀，這會導致有累計的漂移。
- 中期的數據關聯：匹配相機累計誤差小的地圖，這也可以用在BA中，當系統在已經建好的地圖中運行的時候可以達到零漂移。
- 長期的數據關聯：利用場景重識別來匹配當前的觀測和先前的觀測，不用管累計誤差而且即使跟蹤失敗也可以實現,長期的匹配可以利用位姿圖優化重新設置漂移，為了更准確也可以利用BA。這是SLAM在大場景中精度保證的關鍵。
這是第一個可能對短期、中期、長期數據進行數據關聯的視和視覺慣導的系統。在已知地圖的環境中可以沒有漂移的運行，其中混合地圖數據關聯－這個可以保證我們進行地圖匹配和進行BA優化，這也達到了一個目的：構建一個地圖，然后可以在地圖中進行精確的定位。
主要貢獻：
- 一個單目和雙目的視覺慣導SLAM系統：全部依賴於MAP(最后后驗概率估計)，即使是在IMU初始化的時候。
- 高召回率的場景重識別算法:DBoW2需要匹配三個連續的關鍵幀，太慢了。作者的方法是：候選的關鍵幀第一次就進行幾何一致性檢測，然后利用三個共視的關鍵幀進行局部的一致性檢驗，這種策略提升了召回率，並簡化了數據關聯，從而提高了地圖准確性，但計算成本變高。
- 第一個可以解決純視覺或者視覺慣導的完整的混合地圖的SLAM系統。在單目或者雙目的系統中,Atlas代表的是一系列不連續的地圖，而且可以把他們應用到所有的建圖過程中：場景重識別、相機重定位、閉環檢測和精確的地圖融合。這就允許地圖是在不同的時間構建的(增量的SLAM系統)，純視覺的Atlas是參考的2019年IROS的一篇文章：ORBSLAM-atlas: a robust and accurate multi-map system，本文又添加了視覺慣導的混合地圖系統來實現場景重識別。
- 抽象的相機表示：使SLAM系統與所使用的相機模型無關。並允許通過提供其投影，非投影和Jacobian函數來添加新模型我們提供了針孔和魚眼模型的實現。
二、相關工作
表一中展示位姿估計和數據關聯的工作。
視覺SLAM
- MonoSLAM:第一個基於EKF和ShiTomasi特征的SLAM系統
- PTAM:分離tracking和mapping、基於關鍵幀
- LSD-SLAM:構建大場景的半稠密地圖，但是沒有對地圖進行優化，精度低
- SVO檢測FAST特征，利用直接法跟蹤特征，利用重投影誤差模型來優化位姿，但是只有短期的數據關聯，限制了他的精度。
- DSO在檢測不到特征點的場景也可以得到精准的相機定位，利用7個關鍵幀的局部光度誤差的BA來優化位姿，利用逆深度來存儲點。
視覺慣導系統
- MSCKF:基於EKF的利用特征邊緣化來簡化計算(狀態向量中沒有地圖點)
- OKVIS:第一個緊耦合的基於關鍵幀優化的VIO系統
- ROVIO利用EKF的光度誤差
- 快速的IMU初始化方法：Closed-form solution of visual-inertial structure from motionSimultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation
- VINS->VINS-Fusion->Kimera
- VI-DSO:初始化需要20-30s
三、系統概述
ORB-SLAM3是基於ORB-SLAM2和ORB-SLAM-VI構建的系統，他可以在純視覺或者視覺慣導的系統中魯棒的運行（單目、雙目和RGB-D利用針孔或者魚眼模型，你也可以自己定義模型）。
- Atlas是一個由一系列離散的地圖組成的混合地圖。這里會維護一個active map來定位來的新的關鍵幀，local mapping線程不斷的優化更新這個地圖。Atlas里面還有non-active地圖。這個系統基於詞袋模型給系統編碼用於重定位、閉環檢測和地圖融合。
- 跟蹤線程處理傳感器信息、實時的計算當前幀和active map的位姿，最小化匹配特征點的重投影誤差，這個線程還有一個關鍵幀篩選的過程。在VIO的模式下，機體的速度和IMU的bias利用慣導的參差來=進行優化。當跟蹤線程跟丟的時候，跟蹤線程嘗試在所有的Alats中完成當前幀的重定位，如股票重定位成功，跟蹤就被拉回來了，否則過一段時間activate就會重新存儲為non-active，一個新的active map就會被初始化。
- 局部建圖線程添加關鍵幀和點到active map中，刪除多余的幀並使用視覺或者視覺慣導的BA來優化地圖，這些都是在一個局部的滑窗中做的。除此之外，在有慣導的情況下，IMU的參數是利用MAP估計來初始化和優化。
- 閉環檢測和地圖融合線程在active map和整個Atlas中檢測相同的區域。如果相同的區域是在active中，就會執行閉環的過程，如果屬於不同的map，他們就會被融合為一個地圖。在閉環糾正后，一個全局的BA在一個線程中被觸發來優化地圖。
四、相機模型
提供了針孔模型和魚眼模型。系統中把相機模型單獨的抽象出來(重投影和反投影方程，Jacobian方程等)。這就允許我們在系統中使用自己的相機模型。在抽象的過程中也遇到了一些困難，在下邊討論。
A.重定位
ORB-SLAM解決重定位是用的Epnp；但是他是基於一個標定好的針孔相機模型的，為了繼續我們的工作，我們采用獨立於相機的ML-pnp算法可以完全從相機模型中解耦，因為他利用投影光線作為輸入。相機模型只需要提供一個從像素傳遞到投影光線的反投影函數，以便能夠使用重定位。
B.未矯正的雙目SLAM
很多的SLAM系統都假設雙目相機已經矯正了(兩幅圖像都轉換為使用相同焦距的針孔投影，圖像平面共面，並與水平極線對齊，這樣，通過觀察另一幅圖像中的同一行，可以很容易地匹配其中一幅圖像中的特征)。然而，對立體圖像進行校正的假設有很大的局限性，在許多應用中不適合也不可行。例如矯正一個魚眼相機需要需要很嚴重的裁剪。這就失去了廣角相機大視角，快速建圖、處理遮擋魯棒的優點。因此本文的系統不依賴於圖像矯正，而是把雙目相機看成兩個單目相機：
- 有一個恆定的SE(3)變換
- 兩幅圖像有部分相同的觀測
這些約束在我們三角化新的路標點和進行BA優化的時候很有效率。SLAM估計6自由度剛體位姿，其參考系可以位於其中一個攝像機中，也可以位於IMU傳感器中，根據剛體位姿表示攝像機。我們可以利用雙目初始化第一次看到的路標點。雙目還有很多重疊的區域，我們把他看成單目相機來使用。
視覺慣導SLAM
ORB-SLAM-VI是第一個有能力地圖重用的視覺慣導的系統，但是他只能基於單目的針孔模型，初始化很慢，在本文，系統中使用快速精准的IMU初始化，通過了一個開源的SLAM庫利用針孔或者魚眼模型快速的完成單目慣導或者雙目慣導的初始化。
B.IMU初始化
初始化的目的是為了給慣導的變量提供良好的初始值：機體速度、重力方向還有IMU的偏置。VI-DSO嘗試不進行初始化，直接利用BA來進行優化導致初始化長達30s。在本文的工作中我們提出了基於三個關鍵insights的快速精准的初始化方法：
- 純單目SLAM可以提供非常精確的初始地圖，但是不知道尺度。所以可以加入Imu獲得尺度信息。
- Scale drift-aware large scale monocular SLAM證明當尺度被明確地表示為一個優化變量，而不是使用BA的隱式表示時，它收斂得更快。
- 在IMU初始化過程中忽略傳感器的不確定性會產生較大的不可預測的誤差(Fast and robust initialization for visual-inertial SLAM_2019ICRA)
所以我們需要考慮初始化的過程中傳感器的不確定性，把IMU初始化看成一個MAP的問題，分為三個步驟：純視覺的MAP估計：在ORB-SLAM中初始化純單目相機僅用了2s，以4HZ的速度插入關鍵幀。初始化后我們有按一定尺度比的由十個相機位姿和數百個3D點組成的地圖。利用圖2中純視覺的模型進行BA優化。這些位姿被轉換到機體坐標系下獲得軌跡
。純慣導的MAP估計：慣導的狀態向量為：
一旦慣性優化完成，框架的姿態和速度和3D地圖點將與估計的比例進行縮放，並旋轉使z軸與估計的重力方向對齊。對偏差進行更新，並重復IMU預積分，以減少未來的線性化誤差。視覺慣導的MAP估計：一旦我們有了對視覺和慣導好的參數，我們可以執行一個聯合的視覺慣導優化老進一步優化參數。這個圖在2a中但是所有關鍵幀的bias都相同，而且先驗信息也相同。我們在EuRoC數據集上進行的詳盡的初始化實驗表明，這種初始化非常有效，在2秒軌跡的情況下實現了5%的尺度誤差。為了改進初始估計，在初始化后5秒和15秒進行視慣性BA，收斂到1%尺度誤差，如第七節所示。在這些BAs之后，我們說地圖已經成熟了，也就是說尺度、IMU參數和重力方向已經被准確地估計出來了，這種初始化方法比ORB-SLAM-VI和VI-DSO都好的多。通過將尺度因子固定為1，並將其從慣性優化變量中提取出來，我們可以很容易地將單目慣性初始化擴展到立體慣性，從而增強其收斂性。
C.跟蹤和建圖
跟蹤和建圖采用的是Visual-inertial monocular SLAM with map reuse中的方案。
- 跟蹤線程只優化最新兩幀的狀態而地圖點位置保持不變。
- 建圖使用關鍵幀及其點的滑動窗口作為可優化變量，包括可共視的關鍵幀，但保持其固定。
在某些情況下，當慢速運動不能提供良好的慣性參數觀測能力時，初始化可能無法在15秒內收斂到精確解。為了使系統更魯棒，本文提出一個新的尺度優化的方法，這種方法基於改進的單慣導的優化方法，其中插入所有關鍵幀，但尺度和重力方向是唯一的估計參數(圖2d)。在這種情況下，biases不變是不對的假設。可以使用每一幀的估計值來修正biases。這種優化的計算效率非常高，每10秒在局部建圖線程中執行一次，直到建圖超過100個關鍵幀，或者初始化超過75秒。
D.跟蹤丟失的魯棒提升
本文的VIO系統在系統跟蹤少於15個點的時候就進入視覺跟蹤失敗的狀態，然后執行：
- 短期的失敗：利用IMU的讀數估計位姿，把地圖點投影到估計的相機位姿上，然后在一個大的image窗口中做匹配，匹配的結果包含在VIO優化中。在大多說情況下可以恢復視覺跟蹤，但是如果超過5s還沒有恢復。進入下一個狀態。
- 長期的失敗：重新進行視覺慣導的初始化構建一個地圖，這個地圖成為active地圖。
地圖融合和閉環檢測
在圖像幀和active地圖間建立的短期和中期的數據關聯是在跟蹤和建圖線程利用地圖點投影到估計的位姿上，然后在小的窗口中匹配得到匹配關系。為了長期的數據關聯來進行重定位和閉環檢測，ORB-SLAM是用的詞袋模型。與跟蹤不同，位置識別是利用DBoW2使用其詞袋矢量構建關鍵幀的數據庫，並且給定查詢圖像能夠根據其詞袋有效地提供最相似的關鍵幀。僅使用第一個候選幀，原始DBoW2查詢就可以達到50-80％的精度和召回率。為了防止假陽性的觀測，DBoW2實施時間和幾何一致性檢查，將工作點的精度提高到100％，召回率達到30-40％。至關重要的是，時間一致性檢查至少在3個關鍵幀期間延遲了位置識別。當嘗試在我們的Atlas系統中使用它時，我們發現這種延遲和較低的召回率經常是在相同或不同地圖的重復區域中造成的。在本文的工作中我們提出了一個在長期和混合地圖數據關聯的時候有改進召回率的新的場景重識別的算法。當建圖線程篩選出一個關鍵幀，場景重識別就嘗試檢測在Atlas中的關鍵幀進行匹配。如果匹配的關鍵幀在active地圖中，這就確定了一個閉環；否則就是混合地圖的數據關聯，執行匹配的地圖和當前的active地圖的融合。這個方法的第二個特點是一旦當前幀和匹配的地圖幀的位姿估計出來了，我們就在匹配幀和其在共視圖中的相鄰幀構建一個局部的窗口。在此窗口中，我們集中搜索中期數據關聯，從而提高了閉環檢測和地圖融合的准確性。
A.場景重識別
為了達到高的召回率，每個新來的關鍵幀都會利用DBoW2數據庫在Altas中檢測幾個相似的關鍵幀。為了達到百分之百的准確度。每個候選幀都要進行幾何驗證。幾何檢驗包括檢驗圖像窗口中是否有和地圖點描述子匹配的上的ORB特征點(漢明距離)。如果有幾個匹配候選幀，首先去除不正確的匹配，還需要檢驗和排第二的候選幀比較距離比。場景重識別的過程如下：
- DBoW2候選關鍵幀：利用active關鍵幀在Altas的DBoW2數據庫中檢索三個候選的相似幀，包括Ka的共視幀，我們把匹配幀稱為Km。
- 局部窗口：對與每一個Km我們定義一個局部的窗口包括Km和他最好的共視幀，以及他們觀測到的所有的地圖點。DBoW2直接索引提供了Ka和局部窗口關鍵幀的特征點匹配，我們可以得到2D-2D和3D-3D的匹配關系。
- 3D對齊變換：利用RANSAC->Tam來來更好的對齊局部窗口中的Km和Ka的地圖點。在單目或者單目慣導的系統中，如果地圖還沒初始化成功，我們計算Sim3變換，如果初始化成功我們計算SE3變換。在計算兩種變換的時候我們都利用3點的Horn算法來找到Tam。如果計算出來的Tam把Ka中的點變換到Km中重投影誤差小於某個閾值，就把這個匹配當成正確的。擁有最多選票的假設被選擇（前提是該假設滿足一定的閾值）。
- 匹配優化：將局部窗口中的所有地圖點通過Tam進行轉換，以找到更多與Ka中的關鍵點匹配的地圖點。同時也要把Ka轉換到局部窗口中找到與之的匹配點。利用所有的匹配點來計算Tam，利用雙向帶有魯棒核函數的重投影誤差的非線性優化來優化Tam，如果優化后inliers數量超過一定的閾值，就會在一個更小的圖像窗口中進行第二段的匹配和非線性優化。
- 在三個共視關鍵幀中驗證：為了避免假陽性的結果，DBoW2在連續三個關鍵幀中決定是否觸發、延時或丟失位置重識別。這個方法的關鍵是：在大多數情況下我們需要驗證的信息已經都在地圖中了，為了驗證位置重識別，我們在active地圖中和Ka共視的兩個關鍵幀（共視幀中共視的地圖點超過一定的閾值。如果沒有找到這樣的共視關鍵幀，這種驗證就會在新來的關鍵幀中進行，但是不需要再次啟動詞袋。驗證一直持續，直到有三個關鍵幀驗證Tam，或者連續兩個關鍵幀驗證失敗。
- 重力方向驗證：在有IMU的情況下，如果active地圖成熟，我們已經估計了Tam。我們就要確定兩幀的pitch和roll角低於某個閾值來確定是不是進行位置重識別。
B.視覺地圖融合
當位置重識別成功的時候產生active地圖Ma中的關鍵幀Ka和一個在Altas地圖Mm中的關鍵幀Km利用Tma$進行混合地圖的數據關聯來融合地圖。這里需要注意的是Altas中的地圖的信息需要被tracking線程重用來避免地圖復制。這里把Ma地圖放到以Mm為參考的地圖中，由於Ma中有很多的組件，所有融合可能需要一段時間。地圖融合分成兩部分：首先，在一個由Ka和Km相鄰點定義的連接窗口中進行合並，在第二階段，通過位姿圖優化將校正傳播到合並圖的其余部分。合並算法的具體步驟為:
- 連接窗口集合：連接窗口包括Ka和他的共視的關鍵幀，Km和他的共視關鍵幀，以及所有他們觀測到的地圖點。利用Tma把Ma中的地圖點和關鍵幀和Mm對齊然后再放到連接窗口中。
- 融合地圖：Ma和Mm融合組成一個新的active地圖。為了刪除重復的點，在Mm中的關鍵幀中主動搜索Ma的匹配點，對於每個匹配都刪除Ma中的點，Mm中的點保存下來了所有的觀測。利用中期的點關聯來更新共視和基本圖添加Mm和Ma的連接邊。
- 連接窗口的BA：在連接窗口中把所有來自Mm和Ma中的關鍵幀進行局部優化。為了保證滑窗中的關鍵幀數，Mm的共視幀保持固定。一旦優化完成，連接窗口中的所有幀都可以進行跟蹤，來快速准確的重用地圖Mm。
- 位姿圖優化：利用整個融合地圖的本質圖進行位姿圖優化，保持連接區域的關鍵幀固定。這個優化將修正從連接窗口傳播到地圖的其余部分。
C.視覺慣導地圖融合
視覺-慣性合並算法的步驟與純視覺合並相似。改進純視覺中的步驟1和步驟3，以更好地利用慣性信息:
- VI連接窗口集合：如果active成熟了，在把Ma包含在連接窗口前把地圖Ma利用Tma(SE3)進行變換。如果active沒成熟，利用Tma(Sim3)來對齊Ma。
- VI連接窗口BA：active關鍵幀Ka以及最新的五個關鍵幀的位姿，速度和偏置是可以優化的。這些變量通過IMU預積分來進行關聯。對於Mm，我們也對Km和他時序上的五個相鄰幀的位姿、速度和偏置進行優化，如下圖所示：
對於Mm，包含了緊靠局部窗口前的關鍵幀，但是固定的;而對於Ma，包含了類似的關鍵幀，但其姿態仍然是可優化的。所有關鍵幀所能看到的所有點，以及觀察這些點的關鍵幀姿態也得到了優化。利用重投影誤差將關鍵幀和關鍵點關聯起來。
D.閉環檢測
閉環檢測和地圖融合類似，但是是在場景重識別的兩個關鍵幀都在active地圖下。根據匹配的關鍵幀組成連接窗口，重復的點被檢測融合然后叜共視圖和本質圖中構建新的邊。然后進行位姿圖優化來傳播閉環校正的結果到剩余的地圖中。最后一步是全局BA，在考慮閉環檢測中期和長期的匹配后得到MAP估計。在視覺慣導的情況下，只有在關鍵幀數量低於閾值的時候才進行以避免巨大的運算成本。
參考文獻
[1] Campos C , Elvira R , Rodríguez, Juan J. Gómez, et al. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM[J]. 2020.[2] Campos C , Montiel, José M. M, Tardós, Juan D. Inertial-Only Optimization for Visual-Inertial Initialization[J]. 2020.[3] https://blog.csdn.net/qq_40878688/article/details/105291348(ORBSLAM-Atlas a robust and accurate multi-map system介紹)本文僅做學術分享，如有侵權，請聯系刪文。下載1在「3D視覺工坊」公眾號后台回復：3D視覺，即可下載 3D視覺相關資料干貨，涉及相機標定、三維重建、立體視覺、SLAM、深度學習、點雲后處理、多視圖幾何等方向。
下載2在「3D視覺工坊」公眾號后台回復：3D視覺github資源匯總，即可下載包括結構光、標定源碼、缺陷檢測源碼、深度估計與深度補全源碼、點雲處理相關源碼、立體匹配源碼、單目、雙目3D檢測、基於點雲的3D檢測、6D姿態估計匯總等。
下載3在「3D視覺工坊」公眾號后台回復：相機標定，即可下載獨家相機標定學習課件與視頻網址；后台回復：立體匹配，即可下載獨家立體匹配學習課件與視頻網址。