ORB-SLAM3 論文筆記

• 這篇博客
• ORB-SLAM3系統
• 相機模型的抽象(Camera Model)
• • 重定位的問題
  • 圖片矯正的問題
• 視覺慣性SLAM的工作原理
• • 相關公式
  • IMU初始化
  • 跟蹤和建圖
  • 系統對跟蹤丟失的應對
• 多地圖的閉環檢測和地圖融合
• • 位置識別
  • 視覺地圖融合方法
  • 視覺+慣性地圖的融合方法
  • 單個地圖中的閉環融合
• 結尾

這篇博客

?最近ORB-SLAM3全能王橫空出世，在看完ORBSLAM-Atlas系統論文后，就直接莽ORB-SLAM3的論文了。關于系統中$IMU$初始化的相關論文之后在慢慢理解（概率好頭疼），剩下的就直接莽了！
【轉載聲明】本篇文字均為原創，如轉載請注明出處

ORB-SLAM3系統

?ORB-SLAM3是基于 VI ORB-SLAM 和 ORBSLAM-Atlas 兩個系統（點擊可查看我寫的關于這兩個系統論文的筆記）實現的，它相當于是這兩個系統的融合升級版，是在視覺、視覺+慣性SLAM中的全能王。下圖是ORB-SLAM3的流程框圖：

?這里大致介紹一下各個線程的工作內容：
?a) Atlas：Atlas相當于一個子系統，它保存了許多ORB-SLAM3所創建的子地圖。所有子地圖可以分成兩類：Active Map(跟蹤定位所使用的地圖)，Non-active Map(之前保留下來的地圖)。
?b) Tracking：處理傳感器的輸入信息，通過Active Map來實時地對機器進行跟蹤定位，同時還會決定是否創建新的關鍵幀 KF 。如果跟蹤丟失，跟蹤線程會先在 Atlas 的所有地圖上進行重定位：重定位成功，則繼續跟蹤；否則將當前 Active Map 保存為 Non-active Map ，并重新創建地圖。
?c) Local Mapping：維護 Active Map 中的一小部分地圖，可以幫助提高跟蹤定位的精度。如果是慣性系統，則該線程還承擔著 IMU 初始化的任務。
?d) Loop and Map Merging：檢測地圖內或地圖之間存在的閉環，根據閉環對地圖進行修正，降低系統累積誤差。

?通過整個工作流程圖以及各線程的功能，我們可以看出，相比于普通的視覺SLAM、視覺慣性SLAM（這里可用ORB-SLAM2和VI ORB-SLAM作為對比對象），ORB-SLAM3主要有以下幾個創新點：
（1）能夠進行單目、雙目的視覺、視覺+慣性跟蹤定位，使用最大后驗估計（MAP）方法計算位姿。同時在IMU初始化方面也采用了一種精度、效率更好的新方法；
（2）使用一種新的位置識別方法，來完成重定位和閉環檢測操作。這個方法耗時更少，且在保證100%準確率的同時，能夠提高閉環的召回率；
（3）加入了 ORBSLAM-Atlas 系統的多地圖特性，并使用（2）中的方法完成地圖的位置識別+閉環檢測；
（4）系統對相機模型進行了抽象表達，把系統中與相機模型有關的所有特性、函數（如投影、反投影函數等）都提取出來，組成一個獨立的相機類。這樣系統就能適用于不同的相機模型。

相機模型的抽象(Camera Model)

?為了讓ORB-SLAM3能適應于不同類型相機，這里將相機模型從系統中獨立了出來，構建抽象類對象(代碼實現是這樣的)。用戶可通過類繼承的方法，創建想要的相機模型類對象。之后就能在ORB-SLAM3代碼里生成相應實例并使用。論文里有針孔模型和魚眼相機模型。這種作法會導致系統使用的相機模型不再是確定的，因此在算法等方面會存在一些問題。

重定位的問題

?在重定位方面，ORB-SLAM 是通過 EPnP 算法來計算位姿的。而 EPnP 算法的公式是基于針孔相機模型的。如果系統使用的是魚眼相機那么 EPnP 就失效了。因此系統需要使用一個不依賴某一個相機模型的PnP算法，即最大似然 PnP（MLPnP) 。

圖片矯正的問題

?在多數雙目SLAM中，都假設雙目圖片能夠矯正好，然后就能通過極線搜索的方法輕松地找到圖片間的匹配特征。但這種假設具有較大局限性，且圖片矯正在某些時候并不實用，比如矯正魚眼相機的圖片要去掉其中大部分區域。這就丟掉了很多環境信息，會降低系統的魯棒性。所以ORB-SLAM3不依靠圖片矯正，而將雙目看作是兩個滿足下述條件的單目相機：

?1）兩個單目相機間存在一個固定的相對位姿變換矩陣。
?2）（可選）兩個相機間存在共視區域（當兩個相機一前一后時就沒有共視了）。

?由于不依靠矯正后的圖片，所以 ORB-SLAM3 采用的是暴力匹配的方式，在全局使用knn搜索為兩個相機尋找匹配特征。
?在跟蹤和建圖時，使用一個單目相機作為視覺上的參考坐標系，另一個則用來幫助構建地圖點。可以通過三角化構建兩相機間的共視部分中特征點對應的地圖點。而共視部分以外的圖片區域則按照單目相機圖像的方式來處理（單目建圖）。

視覺慣性SLAM的工作原理

相關公式

?在視覺慣性SLAM中的狀態變量有（用下標C(Camera)表示相機坐標系，下標B(Body)表示慣性元件坐標系）:
1）$T_i=[R_i,p_i]\in SE(3)$：第 i 時刻Body的位姿；
2）$v_i$：每幀的速度（在地圖中第一參考坐標系下表示）；
3）$b_i^g$：第 i 時刻 IMU 元件的陀螺儀偏差；
4）$b_i^a$：第 i 時刻 IMU 元件的加速度偏差；
?將這些變量整合起來，用一個向量 S 來公式 (1)表示：

?因為是視覺慣性SLAM，所以系統通過優化慣性誤差、視覺誤差來獲得高精度結果。其中IMU誤差如公式 (2)所示：

其中$\Delta R_{i,i+1},\Delta v_{i,i+1},\Delta p_{i,i+1}$ 是兩幀間 IMU 測量值預積分后的結果。
是所有測量向量的信息矩陣。$R_i,p_i$ 是系統估計（視覺估計）出來的結果，$v_i$ 由 IMU 的速度計算公式求出。
?視覺誤差就是重投影誤差，如公式 (3)所示：

其中 $r_{ij}$ 是第 j 個地圖點到第 i 幀上的重投影誤差。Π 是相應的相機投影模型，$u_{ij}$是地圖點對應的像素坐標，它具有一個信息矩陣 ${\Sigma }_{ij}$。$T_{CB}$ 表示Body–>Camera坐標系的變換矩陣，可以提前標定。
?將公式(2)和公式(3)的誤差聯合起來，就是系統所優化的總誤差，如公式 (4)所示：

其中$K^j$ 是能夠觀測到第 j 個地圖點的所有 KFs 。${\rho }_{Hub}$ 是魯棒核函數。公式 (4) 的優化問題可由下圖表示。

（圖中的展示的優化方法在跟蹤和建圖過程中，會依據情況做出相應調整）

IMU初始化

?因為有些IMU的參數：速度 $v$、加速度偏差 $b^a$、陀螺儀偏差 $b^g$、重力方向 $g$ 是未知的，所以要通過初始化將它們估計出來。如果用的是單目相機，還需要估計出尺度 $s$。
?論文用的初始化方法速度更快、精度更高。由于考慮了傳感器的不確定性，所以將初始化當作一個最大后驗估計（MAP）問題來求解。以單目系統為例，初始化的步驟為：

?1）Vision-only MAP Estimation(這里和普通單目初始化一樣)：將系統以 4Hz 插入 KF 的頻率運行 2s ，構建一個尺度不確定的視覺地圖。該地圖內應含有 10 個 KFs 和幾百個地圖點。然后通過Visual-Only BA 來優化各幀的位姿（如圖 2b 所示）。通過各幀位姿，獲得對應的 Body 位姿${\bar{T}}_{0:k}=[R,\bar{p}]$。
?2）Inertial-only MAP Estimation：通過${\bar{T}}_{0:k}$和 KFs 間 IMU 預積分的值，來估計 IMU 的參數和尺度。將這些待估計變量用下式表示：

其中 $s$ 是地圖的尺度。$R_{wg}\in SO(3)$ 是重力的旋轉矩陣，它表示的是地圖中第一參考坐標系到真實世界坐標系的旋轉關系。$b$ 則是加速度和陀螺儀偏差（在初始化過程中被認為是定值）。${\bar{v}}_{0:k}$ 是基于尺度的速度。

（下面部分涉及到論文：Inertial-Only Optimization for Visual-Inertial Initialization）

?此時，如果只考慮相鄰兩幀間的慣性測量值：

那么，根據就能建立以下 MAP 估計問題：

考慮到每個測量值的獨立性，MAP 問題可以寫成 公式 (7)：

再進行一些假設，就能獲得最終的優化問題，如公式 (8) 所示：

公式中不包含視覺誤差，僅優化慣性誤差。$r_p$ 表示先驗誤差。這個優化過程如 圖2c 所示：
其中關于$R_{wg},s$ 兩個待估計參數的更新公式如下：


?優化完后，地圖的尺度就確定了。之后通過估計的重力方向，將地圖中所有元素變換到真實世界坐標系下表示。
?3）Visual-Inertial MAP Estimation：完成上述兩步后，同時優化視覺誤差和慣性誤差。這一步和圖 2c 很相似，但要保證各KFs都有相同的偏差$b^a,b^g$，以及和第二步具有一樣的先驗信息。

?通過上述步驟就能僅根據 2s 內產生的軌跡，獲得尺度誤差為 5% 的估計結果。在初始化后的 5、15s 會各執行一次視覺 + 慣性 BA（代碼中類似于又執行了兩次初始化，但稍微有些不同）。這樣可將尺度誤差降低到 1% 。完成上述所有操作后，就認為地圖已經成熟。
?雙目情況下的初始化和上述類似，只需要將尺度 $s$ 固定為 1，且不對它進行優化。

跟蹤和建圖

?跟蹤定位當前幀可以采用兩種方法：通過運動模型跟蹤（相鄰兩幀間的運動矩陣已知）和通過當前參考 KF 跟蹤。在完成跟蹤之后，需要進行位姿的優化，其方法和VI ORB-SLAM類似(單目系統是一樣的優化方法，而雙目系統需要考慮地圖點是在哪一個相機上成像的。如果在右邊相機，那在構建g2o誤差邊時要考慮兩相機間的相對變換矩陣。關于VI ORB-SLAM可以參考這篇博客）。優化過程中只優化兩幀的狀態變量，且保持地圖點不變。但當地圖剛更新完畢時，優化方法會有所改變，具體可見VI ORB-SLAM中相關部分。
?建圖環節的優化主要考慮局部地圖優化，優化局部地圖中的 KFs 和地圖點，其他的共視 KFs （局部地圖以外）可以提供約束（重投影誤差），但不會被優化（這里和代碼實現不一樣，也許是自己理解沒有到位。代碼中共視 KFs 的位姿也會被優化。）
?在某些特殊情況下，比如機器移動很慢，IMU參數的可觀測性較差。此時使用之前的初始化方法無法在15s內獲得精度較好的結果。所以需要采用一個新的尺度提煉方法來預防這種特殊情況。這個方法是 Inertial-only 優化的修改版，它只優化尺度和重力方向。此時不再認為各 KFs 的偏差相同。該優化方法如下圖所示（即圖 2d ）：

?這個尺度提煉的方法將在 Local Mapping 線程中每隔10s執行一次，直到地圖中已有100個以上KFs或者在初始之后系統已經運行了75s。

系統對跟蹤丟失的應對

?一般的視覺SLAM，如ORB-SLAM2在跟蹤丟失時會直接進行重定位操作。此時地圖停止更新，直到重定位完成。而ORB-SLAM3有兩種方法來應對跟蹤丟失的情況：

?1）在視覺上發生跟蹤丟失時，則只使用IMU進行位姿估計。根據估計的位姿，將地圖點投影到當前圖像上以尋找匹配點。然后通過視覺+慣性優化獲得最終位姿結果。
?2）如果通過上面方法在5s內無法重新定位，則重新構建地圖。當前使用的地圖根據情況決定是否保存。
（如果當前地圖中信息較多（ KFs 數量多）則認為其比較重要，將其保留；否則直接殘忍拋棄）

多地圖的閉環檢測和地圖融合

?由于系統具有多地圖特性，所以會構建出多個子地圖。為了更有效地檢測地圖內和地圖之間的閉環，ORB-SLAM3使用一種新的位置識別方法和閉環融合方法。

位置識別

?將跟蹤時正在使用的地圖稱為Active Map，其余子地圖為Non-active Map。在Active Map中的關鍵幀記為 $K_a$ 。整個位置識別的大致步驟為：

?1）為當前關鍵幀 $K_a$ ，找出三個候選者 $K_m$（不包括之前就與 $K_a$ 有共視關系的 KFs ）；
?2）為每個 $K_m$ 構建一個局部窗口，其中包含了 $K_m$ 和與之共視程度較高的一些 KFs ，以及它們觀測到的所有地圖點；
?3）先進行幾何一致性檢測。通過 $RANSAC$ 算法，估計 $K_a$ 和 $K_m$ 間適應性最高的變換矩陣 $T_{am}$；
?4）將 $K_m$ 的局部窗口中所有地圖點通過 $T_{aw}=T_{am}{T}_{mw}$投影到 $K_a$ 中，尋找其匹配點。反過來也一樣，可以把 $K_a$ 觀測到的地圖點投影到 $K_m$ 的局部窗口中的KFs上。根據這些匹配關系，通過非線性優化來提高 $T_{am}$ 的精度。
?5）在 Active Map 中找 3 個和 $K_a$ 共視程度大于一定閾值的 KFs（與$K_a$觀測到相同的局部地圖中的地圖點的數量較多） 。如果沒找到，就等待在 $K_a$ 后面的兩個連續KFs**（不需要對這兩個KFs進行DBoW2方式的位置識別，所以能節約時間）**。此時用 $K_a$ 和其他 3 個KFs來驗證$T_{am}$。（驗證方法應該就是看這兩個KFs在經過$T_{am}$變換后能否看到一定數量的地圖點（地圖點在 $K_m$ 的局部地圖中））
?6）基于重力方向的驗證。在視覺慣性系統中，如果地圖已初始化完畢，那么 $T_{am}\in SE(3)$。檢查$T_{am}$中 $pitch、roll$ 方向的旋轉角度是否低于一定閾值。低于則認為這次位置識別成功。

（對第(6)條的個人理解：如果 $K_a$ 和$K_m$形成閉環，那么他們的重力方向應該是差不多的，因為他們只有在 $pitch、roll$ 方向上差不多的位置觀測同一區域時，才會形成閉環）

視覺地圖融合方法

?將系統正在使用的地圖定義為Active map，記為 $M_a$，其中的 KF 記為$K_a$；其余子地圖記為Non-active map，記為$M_m$；與$K_a$匹配的閉環幀記為$K_m$；$K_a$和$K_m$間相對變換矩陣為$T_{ma}$。

?1）用$K_a$+$K_m$+他們倆的共視圖（系統默認$K_a$選取 15個，$K_m$也一樣）和觀察到的地圖點來構建接合窗口。構建前，先將屬于$M_a$的元素通過$T_{ma}$變換到 $M_m$ 地圖坐標系中。
?2）第一步后，Ma和Mm就完成了融合，一起作為新的Active map。此時會有重復的地圖點產生，它們是$M_a$與$M_m$間的匹配點。對它們進行融合：把在$M_a$地圖中的地圖點信息（它的觀測KFs和描述子之類的）轉移到與其匹配的$Mm$中的地圖點上。最后再更新融合后地圖的共視圖和本質圖。
?3）對融合地圖中的接合窗口部分進行局部 BA 優化。其中，對于$M_m$地圖中剩余的KFs，只要能觀測到接合窗口中的地圖點，就能參與局部 BA 優化。但它們只是提供約束，不會被優化。這個過程如下面的 圖(a) 所示：
?4）以優化好的接合窗口為基礎（固定其中KFs），通過位姿圖優化來優化融合地圖中剩余的 KFs 。

PS：融合后的地圖直接用于跟蹤定位，成為新的Active Map。在融合之前的Active Map中所使用的Local Window（跟蹤線程使用的局部窗口），在融合之后的地圖中繼續使用（仍然是那些 KFs 構建局部窗口）。

視覺+慣性地圖的融合方法

?這里和視覺地圖融合的方法相似，只是原來的第（1）和（3）步做了修改（接合窗口中KFs的數量和選取方法變了）：

1）(與上面的第(1)步相比較)如果當前地圖成熟了（尺度、重力方向估計優化完成），$T_{ma}\in SE(3)$，否則$T_{ma}\in Sim(3)$。之后的操作和之前一樣。
2）視覺慣性接合窗口的局部 BA 優化：將$K_a+K_a$前面的5個 KFs記為 $K_a$ 的局部窗口、$K_m+K_m$附近5個 KFs 記為$K_m$ 的局部窗口。將在$K_m$ 局部窗口之前的 KF 記為$K_{bm}$，同理 $K_a$ 那邊則記為$K_{ba}$ 。找出上面 KFs 觀測到的地圖點，記為$M_p$，并在$M_a$ 和 $M_m$ 地圖中找到這些地圖點其余的觀測幀 KFs，記為$K_c$ 。

?最后將上述所有 KFs 和地圖點都包含到優化算法中，其中各部分的優化情況為：
?1）$K_a$局部窗口中的 KFs 的所有狀態變量都會優化；
?2）$K_m$局部窗口中的 KFs 的所有狀態變量都會優化；
?3）$K_c$中的 KFs 只會優化位姿狀態變量會被優化；
?4）$M_p$中的所有地圖點都優化；
?5）$K_{ba}$只優化其位姿狀態變量，其余狀態變量保持不變；
?6）$K_{bm}$提供約束，但不會被優化。
這個優化過程的示意圖如下：


疑問：在查看代碼時，代碼中進行視覺慣性地圖融合的函數應該是 MergeLocal2()，但是該函數是將 $M_m$ 地圖中的元素變換到了 $M_a$ 中，而且最后只完成了接合窗口部分優化，而沒有對融合地圖中剩余部分進行全局位姿優化。看來仍需要繼續深入學習。

單個地圖中的閉環融合

?這里形成閉環的兩個KFs都屬于Active map。處理方式和上面的地圖融合是相似的，即：

構建接合窗口(閉環接合處)-->優化接合窗口-->優化地圖剩余KFs

在這之后也會進行一次全局的BA優化（優化地圖中所有KFs），以獲得 MAP 估計結果。如果是視覺地圖，最后的全局 BA 優化一定會使用；如果是視覺慣性地圖，只有當地圖中的 KFs 數量較少時 (代碼中默認為200以內) 才會執行最后一步（為了防止計算量過大）。

結尾

?上面是自己結合代碼對 ORB-SLAM3 論文的初步理解 ，如有錯誤，請大家指出，謝謝！之后有新的思路會對這篇博客進行更新（路漫漫其修遠兮，吾將上下而求索！）。

參考資料：
1、ORB_SLAM3和之前版本有什么不同？
2、ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM

總結

以上是生活随笔為你收集整理的ORB-SLAM3 论文笔记的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。