ORB-SLAM3 論文筆記

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

這篇博客

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

ORB-SLAM3系統(tǒng)

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

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

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

相機(jī)模型的抽象(Camera Model)

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

重定位的問題

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

圖片矯正的問題

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

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

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

視覺慣性SLAM的工作原理

相關(guān)公式

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

?因?yàn)槭且曈X慣性SLAM，所以系統(tǒng)通過優(yōu)化慣性誤差、視覺誤差來獲得高精度結(jié)果。其中IMU誤差如公式 (2)所示：

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

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

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

（圖中的展示的優(yōu)化方法在跟蹤和建圖過程中，會依據(jù)情況做出相應(yīng)調(diào)整）

IMU初始化

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

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

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

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

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

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

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

再進(jìn)行一些假設(shè)，就能獲得最終的優(yōu)化問題，如公式 (8) 所示：

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


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

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

跟蹤和建圖

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

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

系統(tǒng)對跟蹤丟失的應(yīng)對

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

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

多地圖的閉環(huán)檢測和地圖融合

?由于系統(tǒng)具有多地圖特性，所以會構(gòu)建出多個子地圖。為了更有效地檢測地圖內(nèi)和地圖之間的閉環(huán)，ORB-SLAM3使用一種新的位置識別方法和閉環(huán)融合方法。

位置識別

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

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

（對第(6)條的個人理解：如果 $K_a$ 和$K_m$形成閉環(huán)，那么他們的重力方向應(yīng)該是差不多的，因?yàn)樗麄冎挥性?$pitch、roll$ 方向上差不多的位置觀測同一區(qū)域時，才會形成閉環(huán)）

視覺地圖融合方法

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

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

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

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

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

1）(與上面的第(1)步相比較)如果當(dāng)前地圖成熟了（尺度、重力方向估計優(yōu)化完成），$T_{ma}\in SE(3)$，否則$T_{ma}\in Sim(3)$。之后的操作和之前一樣。
2）視覺慣性接合窗口的局部 BA 優(yōu)化：將$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 觀測到的地圖點(diǎn)，記為$M_p$，并在$M_a$ 和 $M_m$ 地圖中找到這些地圖點(diǎn)其余的觀測幀 KFs，記為$K_c$ 。

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


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

單個地圖中的閉環(huán)融合

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

構(gòu)建接合窗口(閉環(huán)接合處)-->優(yōu)化接合窗口-->優(yōu)化地圖剩余KFs

在這之后也會進(jìn)行一次全局的BA優(yōu)化（優(yōu)化地圖中所有KFs），以獲得 MAP 估計結(jié)果。如果是視覺地圖，最后的全局 BA 優(yōu)化一定會使用；如果是視覺慣性地圖，只有當(dāng)?shù)貓D中的 KFs 數(shù)量較少時 (代碼中默認(rèn)為200以內(nèi)) 才會執(zhí)行最后一步（為了防止計算量過大）。

結(jié)尾

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

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。