當前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

视觉惯性SLAM: VI ORB-SLAM

發布時間：2023/11/27 生活经验 40 豆豆

生活随笔收集整理的這篇文章主要介紹了视觉惯性SLAM: VI ORB-SLAM 小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

視覺慣性SLAM: VI ORB-SLAM

這篇博客
視覺慣性SLAM
預備知識
- 符號說明：
- 相機投影變換矩陣
- IMU數據更新方程
- IMU數據的預積分
VI ORB-SLAM各環節工作方式
- Initialization
- Tracking
- LocalMapping
- Loop Closing
- Full BA
IMU初始化
- 估計 $b_{g}$
- 估計尺度 $s$ 和重力向量 $g_{W}$ （不考慮加速度偏差 $b_{a}$ ）
- 估計加速度偏差( $b_{a}$ )，尺度( $s$ )和重力方向
- 速度( $v$ )估計
- 在重定位后重新初始化 $b_{g},b_{a}$
結尾

這篇博客

?之前看了VI ORB-SLAM的論文，現在終于有時間來寫論文的學習筆記(不容易啊)。因為這是第一次看視覺慣性SLAM論文，所以寫這篇博客來詳細地記錄一下，以后發現問題會再做修改。話不多說，直接進入正題。
【轉載聲明】本篇文字均為原創，如轉載請注明出處

視覺慣性SLAM

?只使用相機或慣性元件(IMU)傳感器搭建的SLAM系統會存在諸多問題。因為相機雖然可以很好地完成閉環檢測和定位，但是它易受到模糊等影響，所以運動速度不能過快。而IMU卻可以在速度較快，或周圍環境紋理較少時完成定位操作。不過它不能實現閉環檢測，且很容易產生漂移現象（因為存在加速度偏差和陀螺儀偏差：即使IMU元件不動，測量值也可能不為0）。
?這么看來這兩個傳感器的功能剛好能夠互補，用相機來幫助IMU消除累積誤差，并完成閉環檢測；用IMU幫助相機解決在紋理少、運動快的情況下的定位問題。因此，視覺慣性SLAM也就誕生了。
?VI ORB-SLAM是由ORB-SLAM和IMU模塊組成，它的大部分環節功能和ORB-SLAM是相似的，所以相似部分之后就不會過多地介紹。

預備知識

符號說明：

?這里提前說一些之后會在公式中使用的符號和變量。
1、慣性傳感器（IMU）的坐標系記為： $B$ (Body)
2、相機傳感器的坐標系記為： $C$ （Camera）
3、世界坐標系記為： $W$
4、IMU中的加速度偏差： $b_{a}$
5、IMU中的陀螺儀偏差： $b_{g}$
6、IMU中兩次測量值之間的時間間隔：Δt
7、IMU在世界坐標系中的方向： $R_{WB}$ （從B坐標系到W坐標系的旋轉矩陣）
8、將B坐標系的速度在W坐標系中表示： $_{W}V_{B}$
9、將B坐標系的位置在W坐標系中表示： $_{W}P_{B}$ （B到W坐標系之間的平移變換）
10、重力加速度： $g_{W}$
11、重力方向： $gW^=gW?∥gW?∥\hat{g_{W}}=\frac{g_{W}^{*}}{\left \| g_{W}^{*} \right \|}$
12、重力的大小 $G$
13、IMU測得的加速度大小： $a_{B}$
14、IMU測得的角速度大小： $w_{B}$
15、B坐標系到相機坐標系的變換矩陣： $T_{CB}=[R_{CB} | _{C}P_{B}]$

相機投影變換矩陣

?基于小孔成像模型來算出相機坐標系與像素坐標系之間的變換矩陣，并定義投影函數 $π\pi$ :
? $π(XC)=[fuXCZC+cufvYCZC+cv],XC=[XCYCZC]T.............(1)\pi(X_{C})=\begin{bmatrix} f_{u}\frac{X_{C}}{Z_{C}}+c_{u}\\ f_{v}\frac{Y_{C}}{Z_{C}}+c_{v} \end{bmatrix} ,X_{C}=[ X_{C} Y_{C} Z_{C}]^{T}.............(1)$
式中 $f_{u},f_{v}]^{T}$ 表示焦距長度， $c_{u},c_{v}]$ 表示主點位置。它們的單位都為像素。

IMU數據更新方程

?更新的量有三個： $R ， p ， v$ 。更新的公式如下：

?這個公式是從參考文獻[1]中獲得的，表示的是前后兩幀之間的變量關系（和參考文獻[1]不同的是，這里忽略了噪聲對角速度、加速度測量值的影響）。這是之后實現IMU數據預積分的基礎。

IMU數據的預積分

?預積分就是計算出兩相鄰幀或關鍵幀之間的IMU數據的總變化量 $（ Δ R ， Δ p ， Δ v ）$ ，它由相鄰幀或關鍵幀之間所有的IMU測量數據疊加后獲得。在VI ORB-SLAM中用這個公式表示預積分：

?這個公式也是從參考文獻[1]中獲得的。其中 $JΔvgbgiJ_{\Delta v}^{g}b_{g}^{i}$ 表示第 i 幀的陀螺儀偏差對于 $Δ v$ 的影響量， $JΔvgJ_{\Delta v}^{g}$ 是 $Δ v$ 關于 $b_{g}$ 的雅可比矩陣。其他與此類似的變量也是這個意思。

VI ORB-SLAM各環節工作方式

?因為是基于ORB-SLAM搭建的系統，所以VI ORB-SLAM的工作原理和ORB-SLAM十分相似，也是使用三個線程：Tracking、Local Mapping、Loop Close。不過它們各線程的具體工作內容有所不同，下面來介紹這些不同之處。

Initialization

?在ORB-SLAM中，初始化的任務有：創建出初始地圖、定義第一參考幀和軌跡的尺度。在VI ORB-SLAM中，除了上述任務外，初始化還要估計出IMU設備的重力方向、加速度和陀螺儀偏差等變量。它的步驟是：
1、先使用最原始的視覺初始化，獲得初始地圖和一定數量的關鍵幀；
2、進行IMU傳感器的初始化，初始化其重力方向、加速度和陀螺儀偏差、速度，以及整個軌跡對應的尺度。
?這個部分的內容放在最后再說。

Tracking

?跟蹤線程負責計算狀態變量： $(R, p, v, b)$ ，即機器位姿、速度和IMU傳感器的偏差。計算速率和相機的幀率相同（因為IMU數據的采集速率快，不能給每個數據找到其對應的圖像數據。圖像數據則剛好相反）。
?在估計出狀態變量后，對估計值進行優化。所以整個線程的工作流程如下：
1、估計出當前幀的狀態變量；
2、將地圖中的地圖點投影到當前幀上，尋找匹配特征點；
3、計算這些匹配點的重投影誤差以及相應幀之間的IMU誤差；
4、求得總誤差，通過最小化總誤差來完成狀態變量的優化。

圖1

?這里需要注意的是：根據地圖是否更新(有無新的關鍵幀產生或檢測到閉環)，上述流程中第3、4步會有兩種不同的計算方法
(a)當地圖發生了更新：只有優化當前幀的狀態
?使用更新后的地圖和最后一個關鍵幀作為優化的參考，它們在優化過程中保持不變。如下圖所示：

圖2

圖中來自最后一個關鍵幀的變量用下標 $i$ 表示，來自當前幀的則用下標 $j$ 表示。重投影誤差用藍色矩形表示，IMU誤差則是綠色矩形。紅色框表示固定的變量。
?在當前情況下，待優化的狀態變量和優化方式為：

式中 $E_{proj}$ 表示地圖點與當前幀產生的重投影誤差， $E_{IMU}$ 表示關鍵幀與當前幀產生的IMU誤差。 $E_{proj}$ 的計算公式:

式中 $X_{C}$ 是地圖點在相機坐標系下的坐標(計算原理可理解為： $_{W}p_{B}$ 是IMU在世界坐標系下的坐標，用 $X_{W}$ （地圖點的世界坐標）減去 $_{W}p_{B}$ 就能獲得該地圖點與 $_{W}p_{B}$ 之間的向量(世界坐標系)。然后對該向量進行旋轉平移變換，就能獲得地圖點在B坐標系下的坐標)。
?IMU誤差的計算公式為：

?式6是在文獻[1]中獲得的。
個人理解：感覺 $e_{v}, e_{p}$ 的計算和文獻[1]中不符，使用的 $b_{(.)}^{j}$ 應該是 $b_{(.)}^{i}$ 。
?之后使用非線性優化的方式優化誤差。最后將當前幀的優化結果和Hessian矩陣設為一個先驗信息，給后續幀做參考。

(b) 地圖未更新：同時優化前后兩幀，但保持地圖點不變

圖3

?從圖中可看出，這個情況下只用地圖點保持不變，且優化的對象從一個當前幀增加到前后兩幀，對應的待優化狀態變量也變成了：

式中 $E_{prior}$ 是前一幀保留的先驗信息產生的先驗誤差（圖3中的小灰色正方形），它的計算公式為：

式中帶橫線的變量表示前一幀的先驗信息，不帶橫線的表示前一幀在當前優化中的結果。 $ρ$ 是一類魯棒核函數。
個人理解： $E_{prior}$ ：在當前優化過程中可以調節前一幀的狀態變量，但不能使其變化太多(好像可以防止累積誤差)
?在完成優化后，上一幀就被邊緣化（拋棄），而當前幀的優化結果又將作為新的先驗信息。這個方法一直持續到地圖發生更新或先驗信息無效為止。

LocalMapping

?VIORB-SLAM和ORB-SLAM的局部建圖環節不同的地方在于：前者在進行LocalBA時要多考慮一個IMU的誤差項約束。這兩個系統在LocalBA上的對比圖如下：

圖4

相同點：只保留和優化N個關鍵幀，以及它們觀測到的所有地圖點。不過其余能夠觀測到這些地圖點的關鍵幀也會在LocalBA中被使用。它們給地圖點提供重投影誤差的約束，自身的狀態是不變的（在紅色框內的部分）。
不同點：VIORB-SLAM要考慮各關鍵幀之間的IMU約束，以及第N+1個關鍵幀（紅框最右邊）對其下一關鍵幀的IMU約束。

Loop Closing

?VIORB-SLAM 和 ORB-SLAM在閉環檢測環節上是差不多的。當檢測到閉環之后，會對閉環中的關鍵幀進行位姿圖優化（減少計算量），只優化位姿 $（ R, p ）$ ，而不優化IMU變量 $（ v ， b ）$ 。
?值得注意的是，位姿圖中優化的狀態變量只有6個自由度，因為尺度信息可以在IMU初始化時估計出來。這和ORB-SLAM是不同的。
個人理解：因為閉環匹配上的兩個關鍵幀之間很難有IMU的數據約束，所以位姿優化不優化IMU變量

Full BA

?使用一個獨立的線程進行Full BA，優化所有狀態量：位姿，速度和偏差。

IMU初始化

?這是整個VI ORB-SLAM的核心部分，用于估計重力方向、尺度( $s$ )、速度( $v$ )、IMU的偏差( $b_{a},b_{g}$ )。整個過程為：
1、估計陀螺儀偏差( $b_{g}$ )
2、粗略估計尺度( $s$ )和重力向量( $g_{W}$ )（不考慮加速度偏差的影響）
3、估計加速度偏差( $b_{a}$ )，并優化尺度( $s$ )和重力方向
4、估計速度( $v$ )
需要注意的是任兩個相鄰的關鍵幀的時間間隔要短，以減少測量噪聲的影響。下面介紹各步驟的具體內容。

估計 $b_{g}$

?可以通過每兩個相鄰關鍵幀之間的旋轉測量值來估計陀螺儀偏差：

式中$R_{(.)}$表示由視覺求出的關鍵幀間的旋轉變換，$\Delta R_{i,i+1}$表示通過預積分求出的關鍵幀間變換關系。

?優化過程中假設 $b_{g}$ 保持不變，通過最小化上述誤差項來估計出 $b_{g}$ 。

估計尺度 $s$ 和重力向量 $g_{W}$ （不考慮加速度偏差 $b_{a}$ ）

?在求出 $b_{g}$ 之后，可以預積分出速度( $v$ )和位置( $p$ )。
?由于單目SLAM估計的軌跡存在一個尺度因子 $s$ ，所以在軌跡中描述IMU元件與相機之間的變換關系時要考慮 $s$ ，即構建如下變換公式：

?將式(10)帶入到式(3)，同時忽視其中的 $b_{a}$ 后可得公式：

?該式子用于求解 $s$ 和 $g_{W}$ ，由求解線性方程來實現。為了避免求解速度變量，降低方程復雜度，使用三個連續關鍵幀之間存在的兩個相對位姿關系，以及式子(3)中的速度計算公式，來構建線性方程組：

?為了方便，將關鍵幀 $i 、 i + 1 、 i + 2$ 寫作 $1 、 2 、 3$ 。方程組中各項的表達式為：

?方程組(12)的形式為 $A_{3(N-2)X4}x_{4X1}=B_{3(N-2)X1}$ 。使用SVD求解方程組(12)，獲得 $s^{*}, g_{W}^{*}$ 。由于未知量的自由度為 4，所以至少需要有4個關鍵幀才能求解方程組。

估計加速度偏差( $b_{a}$ )，尺度( $s$ )和重力方向

?前面獲得的 $s^{*}, g_{W}^{*}$ 未考慮加速度偏差( $b_{a}$ )，原因是加速度偏差和重力很難區分開==(重力會影響到 $b_{a}$ ，兩者相關性較大)==。而重力的方向和加速度偏差區分性很好，所以可以同時估計這兩者的量。
?首先認為在地圖的第一參考慣性坐標系I中，重力的方向為 $gI^={0,0,?1}\hat{g_{I}}=\{0,0,-1\}$ ，大小為 $G$ 。而之前計算出來的 $g_{W}^{*}$ 的方向為 $gW^=gW?∥gW?∥\hat{g_{W}}=\frac{g_{W}^{*}}{\left \| g_{W}^{*} \right \|}$ 。這時計算 $W 、 I$ 坐標系之間的旋轉變換：

個人理解：之前求出的 $g_{W}^{*}$ 是在地圖第一參考幀中的重力在世界坐標系下的向量表示，而 $g_{I}$ 是重力方向在第一參考幀中的表示。求出 $R_{WI}$ 后就能夠獲得所有軌跡與真實世界之間的旋轉關系(修正位姿估計值的方向)
?此時 $g_{W}$ 可以表示成： $gW=RWIgI^Gg_{W}=R_{WI}\hat{g_{I}}G$ …(15)

式中的 $R_{WI}$ 只包含x、y兩個軸上的旋轉角度，因為z軸的旋轉不會影響到重力的方向。
?通過下面這個擾動來優化旋轉 $R_{WI}$ ：

?式(16)的一階近似為：

(PS：關于擾動和一階近似可以參考《視覺SLAM十四講》P75、P110)

?將式(17)帶入到式(11)中，并考慮 $b_{a}$ 的影響，可得：

?仍是使用三個關鍵幀，得到如下線性方程組：

其中各項的表達式為($\lambda (i)$和方程組(12)中的一樣)：

式中 $_{(:,1:2)}$ 表示矩陣的前兩列
?方程組(19)的形式為 $A_{3(N-2)X6}x_{6X1}=B_{3(N-2)X1}$ 。通過SVD的方法求解(19)，求出 $s?,δθxy?,ba?s^{*},\delta \theta _{xy}^{*} , b_{a}^{*}$ ，以及條件數。由于待求變量的自由度為6，所以至少需要4個關鍵幀。
PS：條件數可用于檢查此問題是否條件良好(如：IMU執行的運動使所有變量均可觀察)[2]

速度( $v$ )估計

?可以通過式(18)求出（用來初始化使用的）關鍵幀的速度，因為此時 $s、g_{W}、b _{a}、b_{g}$ 都已知。此外，還可以使用公式(3)求出最近的關鍵幀的速度。

在重定位后重新初始化 $b_{g},b_{a}$

?如果系統在長期運行后進行了重定位操作，則使用式(9)重新初始化 $b_{g}$ 。而 $b_{a}$ 則通過式(19)求出，此時 $s、g_{W}$ 都是已知的。此過程使用20個連續的幀來完成，這些幀都是由視覺估計位姿。

結尾

?這篇博客是自己對視覺慣性SLAM領域的一個初步探索，里面也加入了一些自己的理解。雖然VI ORB-SLAM的作者沒有開源代碼，但是網上有前輩根據論文復現了系統(感謝前輩！！)。所以又有事情可做了/(-o-)/~~。

參考資料：
1、https://arxiv.org/pdf/1610.05949.pdf
2、https://blog.csdn.net/myarrow/article/details/54694472(這篇博客寫的真不錯！！)

總結

以上是生活随笔為你收集整理的视觉惯性SLAM: VI ORB-SLAM的全部內容，希望文章能夠幫你解決所遇到的問題。

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