簡介
ORB-SLAM3是第一個能在單目、雙目、RGBD魚眼相機和針孔相機模型下運行視覺、視覺-慣導以及多地圖SLAM的系統。其貢獻主要包括兩方面：提出了完全依賴於最大后驗估計的緊耦合視覺-慣導SLAM系統，IMU初始化階段也采用最大后驗估計。可以在室內室外大小各種環境下運行，比其他算法快2-5倍。其次提出了多地圖系統，DBoW2用於回環檢測需要保證時間一致性，在驗證幾何一致性前需要匹配三個連續的關鍵幀到同一區域，雖然精度高，但是召回率低，因而回環檢測過程太慢，之前的地圖很難重復利用到。ORB-SLAM3采用了新的位置識別方法，先檢測候補幀的幾何一致性，再要求連續三幀局部一致性，雖然提高了一點計算代價，但是改進了召回率。可以長期在比較惡劣的環境下運行。丟失時會重新開啟一個子地圖，當重新回到原來的位置會與之前的地圖合並，系統允許不連接的子圖存在，這些子圖可以用於位置識別、相機重定位、回環檢測、地圖合並。與視覺里程計只使用最新的幾幀數據相比，ORB-SLAM3利用了所有之前的信息，這能夠在BA關鍵幀時提供高的視察觀測，提高精度。
最終雙目在EuRoC上的精度為3.6cm，，在TUM-VI上達到了9mm的精度。
系統概述
與之前的版本相比，多了IMU，回環檢測里面多了子圖融合，MAP變為了地圖集ATLAS，主要包括動態地圖和活動地圖，前者指新的關鍵幀所在的圖，系統為關鍵幀建立DBoW2數據庫用於重定位、回環檢測和地圖合並。
追蹤線程和之前的沒有區別，主要是特征點估計位姿，多了IMU預積分，速度和bias估計，追蹤丟失時，嘗試在ATLAS圖中重新定位當前幀。
局部地圖線程向動態地圖中增加關鍵幀和路標點，移除冗余信息，使用視覺或者視覺-慣導BA對地圖中當前關鍵幀的局部窗口進行優化，IMU參數使用最大后驗估計進行優化。
回環和地圖合並線程檢測動態地圖和所有地圖之間的重合區域，如果公共區域都在動態地圖中，則執行回環糾正，否則將合並為一個地圖，並且該地圖設為動態地圖。在回環糾正后執行一次全局BA優化。
關鍵技術
重定位
ORB-SLAM中使用ePnP算法，這里因為使用的算法要和相機模型無關，因此采用MLPnP算法，僅適用投影射線作為輸入，相機僅需要提供一個重投影函數，將像素映射為投影射線即可。
非校正雙目SLAM
許多雙目SLAM系統假設雙目圖像都是已校正的，即兩張圖像都是使用針孔模型采用相同的焦距進行建模，使得圖像平面是共面的，並與水平極線對齊，這樣一幅圖像中的特征可以很容易地通過查看另一幅圖像中的同一行進行匹配。而雙目圖像已校正的假設是很嚴格的，如對於魚眼相機則需要嚴重的圖像裁剪，失去了大視野的優勢。論文假設了左右相機之間存在一個常數SE(3)變換，並且兩邊能夠觀測到一個重合的區域。這使得估計地圖的尺度很方便，因此SLAM系統估計一個六自由度的剛體姿態。只要左右目能看到重疊區域，則第一次看到時就能估計出准確的尺度，並對這些點進行三角化，剩余的信息可以作為單目信息。
視覺-慣導SLAM：相比較於ORB-SLAM-VI，前者只能用針孔相機，而且初始化太慢，本文采用基於IMU的初始化，更加迅速且准確。
理論基礎
估計的狀態量
位姿、世界坐標系的速度，重力和加速度偏差（公式1）。IMU在圖像幀之間進行預積分，得到旋轉、速度、位置變化，進而與估計量一起得到慣導測量的殘差項（公式2）。除了慣導殘差，還使用了第i幀和3D點之間的重投影誤差（公式3）。已知慣導測量項和殘差項，根據關鍵幀的狀態和3D點可以得到優化問題的表達式（公式4）。使用了魯棒huber核降低誤匹配的影響，而對於慣導項的殘差則不需要（不存在誤匹配問題）。該優化問題需要有一個好的初始化，才能使得最終收斂的解較准確。
IMU初始化
為了獲得較好的初始值：速度、重力方向、IMU偏差。基於以下事實：1.純單目SLAM能提供准確的初始地圖，但是尺度未知。2. 相比較於在BA中使用隱式表示，尺度如果准確表示為優化變量將會收斂得更快。3.忽視IMU初始化中傳感器的不確定性會產生較大的不可預知誤差。根據以上三點，將IMU初始化問題表示為最大后驗估計，主要分為三步：
僅視覺的最大后驗估計：初始化單目SLAM，跑2s，以4Hz插入關鍵幀，可以得到有10個位姿、上百個點的統一尺度的地圖，優化也使用僅視覺BA。最終得到一些統一尺度的位姿。
僅慣導的最大后驗估計：使用上一步得到的軌跡（位姿）和關鍵幀之間慣導測量估計狀態量（公式5），包括：上一步中的尺度因子、重力方向、加速度計和陀螺儀偏差（初始化中假設為常量）、統一尺度下的速度，以上一步的位姿作為初始化（位姿固定）。優化函數見公式6-8。公式9和公式10分別為重力方向和尺度因子的更新方式。一旦僅慣導優完成后，位姿、速度、地圖點都要用尺度因子更新，且與重力方向的z軸對齊。
視覺-慣導最大后驗估計：當視覺和慣導參數都估計之后，使用聯合估計繼續優化解，對於所有的關鍵幀有同樣的bias和先驗信息。
以上的初始化方法比其他方法更准確，對於雙目，則固定尺度因子為1，且不作為優化變量，相對來說簡單一點。
追蹤和地圖
追蹤采用視覺-慣導優化最新的兩幀，固定地圖點。對於建圖，采用滑動窗口，維護關鍵幀和點（ 固定共視關鍵幀）。當運動太小，慣導優化無法准確估計參數時，僅僅估計重力方向和尺度。此時bias為常數的假設不再成立，從每一幀估計出來的bias都校正后使用。使用以上方法直到地圖中超過100個關鍵幀或者初始化過去了75秒。
追蹤丟失
當少於15個點被追蹤到時，認為丟失了:如果是短期丟失，則當前位姿通過IMU讀取，地圖通過在估計的相機位姿投影，在一個較大的窗口內進行匹配。如果超過了五秒，認為進入了長期丟失，此時一個新的視覺-慣導地圖以第一步的方法建立，並成為動態地圖。
地圖合並以及回環檢測
對於短期和中期運行的地圖，數據處理是將地圖點通過估計位姿投影。DBoW2的問題是准確率高但是召回率太低了。時間一致性要求連續三幀相似，帶來的系統延時太頻繁。本文提出了新算法：只要出現了新的關鍵幀，就和ATLAS中的任一幀進行匹配，如果匹配的關鍵幀也在動態地圖中，則進行回環檢測；如果匹配幀不在動態地圖中，則進行合並成新的動態地圖。本文的另一個創新點：當前關鍵幀和匹配地圖估計之后，在匹配到的關鍵幀和其周圍關鍵幀（共視圖）中建立局部窗口，在該窗口中查找中距離的數據關聯。改進了回環檢測和地圖合並的准確率。
位置識別
- 找與動態關鍵幀\(K_a\)最相似的三個關鍵幀\(K_m\)（三個關鍵幀不在\(K_a\)的共視圖中）。
- 對於每個\(K_m\)定義一個局部窗口，包括\(K_m\)和他共視度最高的一些關鍵幀，對應觀測的地圖點。該窗口的關鍵幀理論上和\(K_a\)之間存在匹配關系，包括對應的2D特征和3D點。
- 使用RANSAC計算\(K_m\)中地圖點和\(K_a\)中地圖點的位姿變換\(T_{am}\)， 地圖未成熟（其實就是尺度不知道）使用Sim3，否則使用SE3。計算出位姿變換后進行重投影，如果每個3D點重投影誤差在一定范圍內則投一票，找到候選票數最多的\(T_{am}\)。
- \(T_{am}\)也要進一步優化，使用雙向投影。Huber函數降低無匹配影響。如果優化后內點較多，則選擇則一個更小的窗口，進行非線性優化。
- 找到與\(K_a\)共視點超過閾值的兩個關鍵幀，找不到則等待新的關鍵幀，不需要使用詞袋，直到三個關鍵幀能確定\(T_{am}\)或者連續兩個新的關鍵幀都失敗了。
- 在匹配的過程中還考慮了當前幀和候選幀的pitch和raw角 是否在閾值之內。
視覺地圖合並
當前幀和非動態子地圖幀匹配到后需要地圖合並，合並后信息不能冗余，且保證合並速度較快：
- 聯合窗口：包括\(K_a\)和其共視幀，\(K_m\)和其共視幀，以及對應的地圖點。\(K_a\)對應的動態地圖設為\(M_a\),\(K_m\)對應的地圖為\(M_m\)。在構建聯合窗口前，\(M_a\)中的幀和點都通過\(T_{ma}\)與\(M_m\)中進行對齊。
- 合並地圖時，要移除冗余的點，在\(M_m\)關鍵幀中搜索\(M_a\)的點，\(M_a\)中的冗余點將會被移除，同時共視圖和本征圖都會被更新，即在\(M_a\)和\(M_m\)中的關鍵幀中加邊。
- 聯合窗口BA：BA對象為\(Ma\)和\(M_m\)中的幀。為了固定gauge freedom，與\(M_m\)公視的關鍵幀被固定，一旦優化完成，所有聯合窗口內的關鍵幀可以立即用於追蹤，地圖可以快速准確利用。
- 位姿圖優化：在本征圖上對合並后的地圖進行位姿優化，這里讓聯合窗口的關鍵幀固定住（上一步已經優化了），誤差校正從聯合窗口傳遞到該地圖的剩余部分。
視覺-慣導地圖合並
和視覺地圖合並類似：
-
如果是動態地圖，則估計\(T_{ma}\in Sim(3)\),否則估計\(T_{ma}\in SE(3)\)
-
BA：在\(K_a\)和\(K_m\)的最近的五個關鍵幀上進行優化。位姿、速度、bias。對於\(M_m\),當前關鍵幀窗口前的幀被固定，而\(M_a\)中可以被優化。
閉環檢測
和地圖合並類似，只有在同一動態地圖的兩幀匹配后，才會建立聯合窗口，冗余的點會被檢測，並且在共視圖和本征圖中加上新的邊。然后進行位姿圖優化，將誤差傳遞到動態圖的其他部分。最終是一個全局BA，找到最大后驗估計。在視覺-慣導中，為了減少計算量，全局BA只在關鍵幀太少時采用。
圖例
論文中除了系統圖還有兩張對優化狀態量描述很詳細的圖：
圖2是不同模式下的因子圖，正方形為殘差量，圓形（包括橢圓）為優化變量，虛框中為固定的量，不需要優化。(a)為視覺慣導因子圖，包括重投影殘差、隨機游走殘差、慣性測量殘差，優化地圖點、位姿、速度和bias。圖(b)為僅視覺因子圖只有重投影殘差，優化地圖點和位姿。圖(c)是僅慣導因子圖，位姿已經通過僅視覺部分提供，這里直接固定，位姿固定，速度、bias、尺度、重力方向需要被估計，注意根據公式(6)-(8)，這里有一個先驗殘差。圖(d)是追蹤時如果運動量太小，慣導估計不准確，則只估計尺度和重力方向。
該圖為聯合窗口中做BA時優化的量。
實驗
包括在EuRoC上跑的單地圖實驗、TUM-VI 上的視覺慣導SLAM，EuRoC上的多地圖SLAM，效果驚人，具體見論文，數據很詳細。
一些問題
- 哪些幀會出現在動態地圖？
新的關鍵幀肯定會，長期丟失后重新初始化建立的關鍵幀，合並地圖后產生動態地圖
- 運動量太小，為什么只估計尺度和重力方向？圖2(d)。