ORB-SLAM3論文解讀

ORB-SLAM1和2的做者Juan已經畢業了，其工做仍然由同一課題組的Carlos繼續完成並撰寫論文和代碼，此次論文一樣對應着開源代碼，youtube演示效果驚人，不少場景下無需大量修改便可直接使用，可謂是工做黨的KPI，學生黨的SCI。
廢話少說，直入主題。ORB-SLAM3與前兩次做品相比，引入和視覺慣性和多地圖模式的SLAM。其算法支持的傳感器也有單目、雙目、RGBD相機等。在相機的成像模型中，採用了針孔、魚眼相機模型（見摘要的第一段）。
代碼傳送門：連接: https://github.com/UZ-SLAMLab/ORB_SLAM3.
論文傳送門：連接: https://arxiv.org/pdf/2007.11898.pdf.

git

主要的創新點

大佬的自吹自擂部分中也提到，他們的主要創新點有兩點：github

「徹底」基於最大後驗估計MAP的VI-SLAM ，不管在初始化階段仍是運行階段，都採用了MAP進行狀態估計，所以ORB-SLAM3在室內外、大小場景中魯棒性很好，且精確度是其餘方法的2~5倍；如文中所講，本文的IMU和視覺的組合系統是extemely robust的。
多地圖系統，當定位丟失即lost時，ORB-SLAM3會自動創建一個新的小地圖，並在revisit的時候進行地圖的seamlessly merge，所以，這一算法可以使用不只僅幾幀以前的信息，而是運用了全局的信息，可以在bundle adjustmen中利用視差較大的幀來增長BA求解的準確性（由於當視察較小時，求解不許確，且優化容易進入局部極值）。這種方法裏對同一特徵的觀測可能在時間上的間隔較大，由於咱們對其上一次的觀測甚至可能出如今以前一張小地圖中（中間過程裏lost了至少一次）；

數據關聯模型

short-term，在短程模式下，路標點一旦退出咱們的視野，咱們將這個路標點丟棄，其在優化時計算效率高，但存在累計偏差，即便從新回到同一地點，也沒法實現重定位或者回環檢測，位姿圖沒法構成閉環。
mid-term，在中程模式下，和短程模式相似，累計偏差只有在從新回到地圖中已知區域纔有可能實現重定位，創建一個封閉的環狀位姿圖以消除累計偏差。這與大多數SLAM系統中的方法相似。這一方法可以保證局部定位的準確性。
long-term，長期模型中，採用place recognition的方式，例如Dbow詞袋庫的迴環檢測與重定位，無視累計偏差和跟蹤丟失，直接將當前位姿和以前一記錄位姿對齊。並經過圖優化的方式消除（平攤）累計偏差。這一方法可以保證大環境重定位的穩定性。在運用詞袋庫的迴環檢測模型中，須要被檢測幀具備時間一致性，即須要連續三幀均能出發詞袋庫召回匹配。在此以後，還須要檢查是否具備幾何一致性，所以，召回的準確性獲得了較大提高，然而其召回率卻難以獲得保證，這種方法的計算過於緩慢，難以將以前創建的小地圖充分應用起來。所以ORB-SLAM3先進行幾何一致性的檢測，隨後再對局部一致性（即連續三個存在共視的幀的匹配性）進行檢測。其中，時間一致性將連續三個關鍵幀進行比較，而此處局部一致性將連續三個共視關鍵幀進行了檢查。因爲其存在共視，關鍵點的匹配會更多，因此其準確性和召回率獲得了提高，而計算量會少量增大。

系統總體

這部分就直接按照論文中的部分進行依次解讀 (fan)(yi)。

算法

Atlas地圖集

整個ORB3最牛逼的地方就在這裏了，最先在IROS2019的一篇文章，即參考文獻[9]中提到。在ORB3中，Atlas地圖集由一系列不連續的小地圖構成，並可以無縫鏈接，實現重定位、迴環檢測、地點識別等功能。當每個新的視覺幀進入流程，跟蹤線程當即追蹤並定位新一幀的位姿。地圖集自己也隨着時間逐步優化且會將新的視覺幀通過挑選做爲關鍵幀。在Atlas中，DBoW2被用於重定位、迴環檢測、地圖合併。地圖的狀態也分爲active和non-active兩個狀態，在跟蹤線程的介紹裏將會詳細介紹這一機制。windows

跟蹤線程

跟蹤線程被用來根據當前的active地圖，實時地跟蹤最新一幀的位置，利用最小化重投影偏差的方式實現位姿的最大後驗估計MAP。決定新一幀是否做爲關鍵幀加入地圖也是在這個線程中完成的。跟蹤線程接受IMU、Frame輸入，IMU 被預積分處理，而Frame被提取ORB特徵，總體跟蹤流程中規中矩，沒什麼值得一提的地方。
當定位跟蹤失敗時：首先會在全部的Atlas地圖集中搜索當前位置的匹配，若是成功，則將當前的地圖設置爲non-active，而將在Atlas搜索到的地圖設置爲active，並繼續在active的地圖上進行局部跟蹤。若是在全圖搜索失敗，則在必定時間後，當前的active地圖會強行被設置爲non-active，新的小地圖將被構建，並設置爲active。這樣，就的地圖的正確性不會被局部定位的失敗影響，新舊地圖之間的位姿變換關係不肯定性也有但願被以後的共視減少或者消除。
app

局部建圖線程

局部建圖線程中規中矩，只有active的地圖被添加關鍵幀，在添加關鍵幀的時候，重複的關鍵點被移除。VI-Bundle Adjustment在當前被插入幀先後窗口內被用來提高地圖的質量。可是，在Local mapping框圖中的後三項，ORB3加入了一些改進，用了他們的novel MAP，後續會介紹。less

迴環和地圖合併線程

當每個關鍵幀被插入，地點識別都被啓動，這一關鍵幀被和整個Atlas地圖集關鍵幀進行比較，若是檢測到兩關鍵幀是同一地點，分爲兩種狀況：若被召回幀是當前的active地圖中的一部分，則須要進行loop correction迴環矯正，在迴環矯正以後，Full BA在另外一個線程中被悄悄進行，全局位姿圖獲得優化，總體的地圖一致性獲得了提高；若被召回幀是屬於一個non-active的map，兩個地圖則會被合併成一個地圖並將大地圖設置爲當前的active地圖。這一過程和跟蹤並行，不會影響跟蹤的實時性。函數

相機模型

相機模型帶來的重定位問題

相機模型這塊，ORB3將系統中全部與相機模型相關的部分（投影和反投影公式以及其雅可比）提取了出來，讓相機模型成爲了一個獨立的模塊兒，以便隨時替換其餘的相機模型。在重定位中，因爲須要使用PnP（Perspective n points）算法，因此須要一個標定好的針孔模型，所以，其餘相機模型再也不適用，在此咱們選擇MLPnP（Maximum Likelihood PnP）來做爲求解算法。這種算法直接應用光線投影做爲輸入，只須要使用者提供一個將像素反投影爲光束的unprojection公式以及相應像素點的座標就能夠求解相機的位姿。oop

相機模型帶來的雙目問題

在針孔相機雙目視覺中，在尋找特徵點匹配時，能夠直接在另外一個相機的同一行像素上進行搜索，實現快速的匹配，但這一加速技巧在魚眼相機或者兩個不一樣的針孔相機上難以實現。所以咱們將一個雙目相機看做是兩個具備固定位姿變化SE3的單目相機，且兩個相機幀之間存在較大的共視區域。兩個單目被分別加入BA優化。爲了利用雙目的優點，若是兩個相機之間存在共視區域，這一區域的特徵點能夠在第一次被看見的時候被三角化，且尺度的不肯定性被雙目SE3的尺度信息所消除。在沒有共視的區域，特徵點依舊按照多視圖幾何的方式進行三角化。優化

視覺慣性SLAM部分

ORB-SLAM-VI將ORB-SLAM改裝成VI-SLAM。但這一算法仍有其侷限性，雖然可以實現地圖的複用，但其只能用針孔相機且初始化特慢，在此一一敘述。關於預積分和視覺重投影偏差的部分我就再也不多說了。分別將兩個殘差的信息矩陣加權馬氏距離進行求和最小化便是最終的損失函數。在優化時，調整路標點的三維座標以及機器人狀態（R，p，v，q，b）使目標損失函數最小。以下：spa

min ⁡ S ‾ k , X ( ∑ i = 1 k ∥ r I i − 1 , i ∥ Σ I i , i + 1 2 2 + ∑ j = 0 l − 1 ∑ i ∈ K j ρ Hub ( ∥ r i j ∥ Σ i j ) ) \min _{\overline{\mathcal{S}}_{k}, \mathcal{X}}\left(\sum_{i=1}^{k}\left\|\mathbf{r}_{\mathcal{I}_{i-1, i}}\right\|_{\Sigma_{\mathcal{I}_{i, i+1}}^{2}}^{2}+\sum_{j=0}^{l-1} \sum_{i \in \mathcal{K}^{j}} \rho_{\text {Hub }}\left(\left\|\mathbf{r}_{i j}\right\|_{\Sigma_{i j}}\right)\right) Sk,Xmin(i=1∑k∥∥rIi−1,i∥∥ΣIi,i+122+j=0∑l−1i∈Kj∑ρHub (∥rij∥Σij))

IMU初始化

ORB3中的初始化主要在accuracy和efficiency上進行了改進，以得到一個準確的IMU狀態初始值。主要有三點改進的insight：1.純視覺問題能夠單獨拿出來解決。2.尺度應該做爲一個單獨的優化變量而不是做爲一個隱含變量存在於逆深度、相機位移等包含尺度的量中，這樣的優化具備更快的收斂性。3.在初始化的時候應該考慮傳感器uncertainty，不然會帶來較大的偏差。

純視覺最大後驗估計

在這一過程當中，首先以4Hz的頻率插入關鍵幀，運行2s純視覺mapping，這時將會有大約10個相機位姿和幾百個特徵點，對這一系列變量作一個BA，如圖2b所示，得到了一條軌跡。

純慣性最大後驗估計

在這一過程當中，咱們肯定須要在IMU信息優化階段須要估計的變量有：
Y k = { s , R w g , b , v ‾ 0 : k } \mathcal{Y}_{k}=\left\{s, \mathbf{R}_{\mathrm{w} g}, \mathbf{b}, \overline{\mathbf{v}}_{0: k}\right\} Yk={s,Rwg,b,v0:k}
其中s是scale，Rwg是重力旋轉方向，b是bias，v顧名思義。在此咱們整一個最大後驗估計：
p ( Y k ∣ I 0 : k ) ∝ p ( I 0 : k ∣ Y k ) p ( Y k ) p\left(\mathcal{Y}_{k} \mid \mathcal{I}_{0: k}\right) \propto p\left(\mathcal{I}_{0: k} \mid \mathcal{Y}_{k}\right) p\left(\mathcal{Y}_{k}\right) p(Yk∣I0:k)∝p(I0:k∣Yk)p(Yk)
其中公式就是貝葉斯公式的去掉分母的版本，後驗=似然*先驗。其中花體的I就是IMU測量值，咱們將公式搞進最大似然argmax有：
Y k ∗ = arg ⁡ max ⁡ Y k ( p ( Y k ) ∏ i = 1 k p ( I i − 1 , i ∣ s , g d i r , b , v ‾ i − 1 , v ‾ i ) ) \mathcal{Y}_{k}^{*}=\underset{\mathcal{Y}_{k}}{\arg \max }\left(p\left(\mathcal{Y}_{k}\right) \prod_{i=1}^{k} p\left(\mathcal{I}_{i-1, i} \mid s, \mathbf{g}_{d i r}, \mathbf{b}, \overline{\mathbf{v}}_{i-1}, \overline{\mathbf{v}}_{i}\right)\right) Yk∗=Ykargmax(p(Yk)i=1∏kp(Ii−1,i∣s,gdir,b,vi−1,vi))
而後假設IMU預積分值分佈和參數的先驗分佈機率密度函數爲高斯分佈的密度函數，就能夠對整個函數整個求對數並加負號，argmax變argmin：
Y k ∗ = arg ⁡ min ⁡ Y k ( ∥ r P ∥ Σ p 2 + ∑ i = 1 k ∥ r I i − 1 , i ∥ Σ I i − 1 , i 2 ) \mathcal{Y}_{k}^{*}=\underset{\mathcal{Y}_{k}}{\arg \min }\left(\left\|\mathbf{r}_{\mathbf{P}}\right\|_{\Sigma_{p}}^{2}+\sum_{i=1}^{k}\left\|\mathbf{r}_{\mathcal{I}_{i-1, i}}\right\|_{\Sigma_{\mathcal{I}_{i-1, i}}}^{2}\right) Yk∗=Ykargmin(∥rP∥Σp2+i=1∑k∥∥rIi−1,i∥∥ΣIi−1,i2)
rp就是先驗，後面那個就是針對每一次IMU預積分的求和。這個y函數的最小值就是咱們所須要估計的參數的最大後驗估計。在優化上爲了保證旋轉矩陣還在流形上，咱們整了指數變換來更新這一李羣：
R w g n e w = R w g o l d Exp ⁡ ( δ α g , δ β g , 0 ) \mathbf{R}_{\mathrm{wg}}^{\mathrm{new}}=\mathbf{R}_{\mathrm{wg}}^{\mathrm{old}} \operatorname{Exp}\left(\delta \alpha_{\mathrm{g}}, \delta \beta_{\mathrm{g}}, 0\right) Rwgnew=RwgoldExp(δαg,δβg,0)
爲了讓尺度更新的時候不至於搞出來負數，咱們也用指數更新尺度：
s new = s old exp ⁡ ( δ s ) s^{\text {new }}=s^{\text {old }} \exp (\delta s) snew =sold exp(δs)
這一下純IMU部分就徹底ok了。值得一提的是，先驗選取中，IMU的bias設置爲0，使之不會偏離0太遠，且初始的協方差矩陣按照IMU的datasheet設定。當完成了IMU的MAP估計後，純視覺的尺度被對齊，而且重力方向上也與Rwg旋轉後的方向對齊，Bias值也獲得了一個合理的估計。

視覺慣性聯合最大後驗估計

這就沒啥好說的了，和其餘VIO算法同樣，作一個VIO的聯合BA，如圖2a所示。但bias是不變的。且IMU方面的先驗是和純慣性估計的先驗是同樣的。初始化在2s時，尺度偏差已經收斂到5%之內，此時咱們就稱初始化完成了。但VI聯合MAP優化是在延時5-15s以後才運行的，爲的是保證初始化的效率。VI聯合優化後，地圖才被稱爲時mature的，scale的偏差能夠收斂到1%之內。

跟蹤和建圖

跟蹤方面和ORB-SLAM2同樣，沒有什麼特別的地方，實際上跟蹤問題就是最新兩幀的VI-BA優化問題，而t-1幀路標點是fix的。在建圖時，須要在滑動窗口和共視圖裏作一個範圍更大的BA，以得到更高的地圖準確性，參與建圖BA優化的幀因爲計算量限制不能把全部幀加進來，而是僅僅包含窗口內的幀and與當前幀有共同觀測的幀。有的時候，初始化並不理想，這時候咱們把頭100幀或者前75s的keyframe所有歸入初始化優化階段，用於得到一個較好的重力旋轉向量和尺度。實際上這個過程就是「初始化很差的時候就讓他初始化久一點」，但這一過程當中只優化圖2d框框外的變量。當跟蹤過程當中小於15個可辨認特徵點時，進入visually lost即視覺丟失狀態。並採用應對策略：從IMU估計當前位姿，並擴大滑動窗口的長度，將IMU得到的位姿以及當前幀特徵點投影到這個大一點的滑動窗口中去尋找匹配。若是這一過程5s後仍沒有成功重定位，則開啓新的小地圖，並將新圖切換爲active狀態。從而進入我們的下一章節：

地圖合併和迴環檢測

做者又說了，這塊兒是他最牛逼的地方，牛逼之處主要有兩塊兒，第一點是咱們前面所說的迴環檢測與地圖合併機制，除此以外，他們用在共視圖local window上的一致性檢測，代替了時間上的一致性檢測，理論上講，graph上的距離範圍內的一致性確實比單純時間上的一致性要有道理地多，實驗效果反正是又好又快:-)

場景識別

首先，咱們用DBoW操做一番獲得三個Ka的匹配候選幀Km，而後將Km、Km的n個最佳共視幀、全部這些幀看到的路標點放進local window裏，經過詞袋庫的正向索引獲得各個匹配點的2D匹配關係，從而創建匹配點的3D匹配關係。而後須要咱們計算Km到Ka的一個類似變換Sim(3)若尺度不肯定。計算這一變換時，咱們採用Horn algorithm（霍恩法），同時還要作一個RANSAC操做：爲獲得候選變換，咱們把每一個匹配的點對重投影偏差計算出來，當小於必定閾值，就給這個變換投一票，採用民主共和的方式選出最好的位姿變換T。
獲得T後，因爲前面是基於採樣計算的位姿變換，沒有用到所有的特徵點匹配信息，咱們須要用上全部信息refine一下這個位姿變換。咱們在local window裏找全部與這幀圖像中特徵點匹配的路標，並使用雙向轉移偏差做爲偏差目標函數來優化這一位姿變換，還用一下Huber來加強一波穩定性。若是inlier的個數ok，咱們再在更小的local window（減少前面的n）中開始第二次的引導匹配。爲了驗證這一結果，咱們把後續進來的3幀都作一下檢驗：在Ka的active map中找兩個共視幀，而後看這兩幀與local windows的匹配點數目是否超過閾值。這一過程持續三幀成功或者連續兩幀失敗來做爲驗證成功失敗的判斷條件。最最最最後用IMU重力再驗證一波pitch和roll纔算最終經過考驗，成爲一個place recognition。

純視覺地圖合併

地圖合併主要分爲縫合和優化兩部分

地圖縫合

當咱們發現了Ka和Km的匹配發生在不一樣小地圖之間，就須要對他們進行縫合（wlding）操做。爲此咱們將Ka、Km兩幀、Ka和Km全部共視幀、全部這些幀的特徵點都放進縫合窗口（welding window）。在將地圖Ma（最新的地圖）裏面的特徵點放進窗口前，咱們見全部特徵點均經過Tma變換到Mm地圖（被召回的地圖）下。在地圖縫合時，須要將兩圖中重複的特徵點進行剔除：對Ma中的每一個特徵點都在Mm中尋找其匹配點，若是找到則把Ma中的該點去除，並將此次觀測放入Mm中去。同時，共視圖也應當進行更新。

地圖優化

縫合完以後須要進行優化，調整大的內部關係。優化分爲兩塊：縫合優化和總體優化。首先咱們進行縫合優化：咱們將縫合窗口以外的部分進行固定，再在縫合窗口內部進行一個BA優化，到此咱們的縫合後的地圖就能夠用於追蹤新來的幀了。可是爲了提高窗口內外的一致性，利用地圖縫合減少總體的累計偏差，咱們須要進行總體的位姿圖優化：在優化時，縫合窗口內被固定，窗口外採用essential graph進行優化。此時，迴環矯正從縫合窗口傳遞向整個地圖，縫合優化就完成了。

視覺慣性地圖合併

和上一節講的差很少，區別有兩點：

若是active的地圖是mature的，一切照舊；若是不mature，說明尺度信息暫時不可靠，在縫合時，須要用類似變換Sim(3)變換而不是SE(3)將Ma中的點對齊到Mm。
優化的變量和固定的變量如圖所示，Easy。

實驗和結論

效果很是好 :-）
結論很是硬 :-）

於2020/7/27 17：30