視覺慣性SLAM: VI ORB-SLAM

• 這篇博客
• 視覺慣性SLAM
• 預(yù)備知識(shí)
• • 符號(hào)說明：
  • 相機(jī)投影變換矩陣
  • IMU數(shù)據(jù)更新方程
  • IMU數(shù)據(jù)的預(yù)積分
• VI ORB-SLAM各環(huán)節(jié)工作方式
• • Initialization
  • Tracking
  • LocalMapping
  • Loop Closing
  • Full BA
• IMU初始化
• • 估計(jì)$b_g$
  • 估計(jì)尺度$s$和重力向量$g_W$（不考慮加速度偏差$b_a$）
  • 估計(jì)加速度偏差($b_a$)，尺度($s$)和重力方向
  • 速度($v$)估計(jì)
  • 在重定位后重新初始化$b_g,b_a$
• 結(jié)尾

這篇博客

?之前看了VI ORB-SLAM的論文，現(xiàn)在終于有時(shí)間來寫論文的學(xué)習(xí)筆記(不容易啊)。因?yàn)檫@是第一次看視覺慣性SLAM論文，所以寫這篇博客來詳細(xì)地記錄一下，以后發(fā)現(xiàn)問題會(huì)再做修改。話不多說，直接進(jìn)入正題。
【轉(zhuǎn)載聲明】本篇文字均為原創(chuàng)，如轉(zhuǎn)載請(qǐng)注明出處

視覺慣性SLAM

?只使用相機(jī)或慣性元件(IMU)傳感器搭建的SLAM系統(tǒng)會(huì)存在諸多問題。因?yàn)橄鄼C(jī)雖然可以很好地完成閉環(huán)檢測(cè)和定位，但是它易受到模糊等影響，所以運(yùn)動(dòng)速度不能過快。而IMU卻可以在速度較快，或周圍環(huán)境紋理較少時(shí)完成定位操作。不過它不能實(shí)現(xiàn)閉環(huán)檢測(cè)，且很容易產(chǎn)生漂移現(xiàn)象（因?yàn)榇嬖诩铀俣绕詈屯勇輧x偏差：即使IMU元件不動(dòng)，測(cè)量值也可能不為0）。
?這么看來這兩個(gè)傳感器的功能剛好能夠互補(bǔ)，用相機(jī)來幫助IMU消除累積誤差，并完成閉環(huán)檢測(cè)；用IMU幫助相機(jī)解決在紋理少、運(yùn)動(dòng)快的情況下的定位問題。因此，視覺慣性SLAM也就誕生了。
?VI ORB-SLAM是由ORB-SLAM和IMU模塊組成，它的大部分環(huán)節(jié)功能和ORB-SLAM是相似的，所以相似部分之后就不會(huì)過多地介紹。

預(yù)備知識(shí)

符號(hào)說明：

?這里提前說一些之后會(huì)在公式中使用的符號(hào)和變量。
1、慣性傳感器（IMU）的坐標(biāo)系記為：$B$ (Body)
2、相機(jī)傳感器的坐標(biāo)系記為：$C$（Camera）
3、世界坐標(biāo)系記為：$W$
4、IMU中的加速度偏差：$b_a$
5、IMU中的陀螺儀偏差：$b_g$
6、IMU中兩次測(cè)量值之間的時(shí)間間隔：Δt
7、IMU在世界坐標(biāo)系中的方向：$R_{WB}$（從B坐標(biāo)系到W坐標(biāo)系的旋轉(zhuǎn)矩陣）
8、將B坐標(biāo)系的速度在W坐標(biāo)系中表示：${}_W{V}_B$
9、將B坐標(biāo)系的位置在W坐標(biāo)系中表示：${}_W{P}_B$（B到W坐標(biāo)系之間的平移變換）
10、重力加速度：$g_W$
11、重力方向：$\widehat{g_W}=\frac{g_W^{?}}{\Vert g_W^{?}\Vert }$
12、重力的大小$G$
13、IMU測(cè)得的加速度大小：$a_B$
14、IMU測(cè)得的角速度大小：$w_B$
15、B坐標(biāo)系到相機(jī)坐標(biāo)系的變換矩陣：$T_{CB}=[R_{CB}{|}_C{P}_B]$

相機(jī)投影變換矩陣

?基于小孔成像模型來算出相機(jī)坐標(biāo)系與像素坐標(biāo)系之間的變換矩陣，并定義投影函數(shù)$\pi$:
? $\pi (X_C)=\left[\begin{array}{c}{f}_u\frac{X_C}{Z_C}+c_u\\ f_v\frac{Y_C}{Z_C}+c_v\end{array}\right],X_C=[X_C{Y}_C{Z}_C{]}^T.............(1)$
式中 $[f_u,f_v{]}^T$表示焦距長(zhǎng)度，$[c_u,c_v]$表示主點(diǎn)位置。它們的單位都為像素。

IMU數(shù)據(jù)更新方程

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

?這個(gè)公式是從參考文獻(xiàn)[1]中獲得的，表示的是前后兩幀之間的變量關(guān)系（和參考文獻(xiàn)[1]不同的是，這里忽略了噪聲對(duì)角速度、加速度測(cè)量值的影響）。這是之后實(shí)現(xiàn)IMU數(shù)據(jù)預(yù)積分的基礎(chǔ)。

IMU數(shù)據(jù)的預(yù)積分

?預(yù)積分就是計(jì)算出兩相鄰幀或關(guān)鍵幀之間的IMU數(shù)據(jù)的總變化量$（\Delta R，\Delta p，\Delta v）$，它由相鄰幀或關(guān)鍵幀之間所有的IMU測(cè)量數(shù)據(jù)疊加后獲得。在VI ORB-SLAM中用這個(gè)公式表示預(yù)積分：

?這個(gè)公式也是從參考文獻(xiàn)[1]中獲得的。其中$J_{\Delta v}^g{b}_g^i$表示第 i 幀的陀螺儀偏差對(duì)于$\Delta v$的影響量，$J_{\Delta v}^g$是$\Delta v$關(guān)于$b_g$的雅可比矩陣。其他與此類似的變量也是這個(gè)意思。

VI ORB-SLAM各環(huán)節(jié)工作方式

?因?yàn)槭腔贠RB-SLAM搭建的系統(tǒng)，所以VI ORB-SLAM的工作原理和ORB-SLAM十分相似，也是使用三個(gè)線程：Tracking、Local Mapping、Loop Close。不過它們各線程的具體工作內(nèi)容有所不同，下面來介紹這些不同之處。

Initialization

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

Tracking

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

圖1

?這里需要注意的是：根據(jù)地圖是否更新(有無新的關(guān)鍵幀產(chǎn)生或檢測(cè)到閉環(huán))，上述流程中第3、4步會(huì)有兩種不同的計(jì)算方法
(a)當(dāng)?shù)貓D發(fā)生了更新：只有優(yōu)化當(dāng)前幀的狀態(tài)
?使用更新后的地圖和最后一個(gè)關(guān)鍵幀作為優(yōu)化的參考，它們?cè)趦?yōu)化過程中保持不變。如下圖所示：

圖2

圖中來自最后一個(gè)關(guān)鍵幀的變量用下標(biāo) $i$ 表示，來自當(dāng)前幀的則用下標(biāo) $j$ 表示。重投影誤差用藍(lán)色矩形表示，IMU誤差則是綠色矩形。紅色框表示固定的變量。
?在當(dāng)前情況下，待優(yōu)化的狀態(tài)變量和優(yōu)化方式為：

式中$E_{proj}$表示地圖點(diǎn)與當(dāng)前幀產(chǎn)生的重投影誤差，$E_{IMU}$表示關(guān)鍵幀與當(dāng)前幀產(chǎn)生的IMU誤差。$E_{proj}$的計(jì)算公式:

式中$X_C$是地圖點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)(計(jì)算原理可理解為：${}_W{p}_B$是IMU在世界坐標(biāo)系下的坐標(biāo)，用$X_W$（地圖點(diǎn)的世界坐標(biāo)）減去${}_W{p}_B$就能獲得該地圖點(diǎn)與${}_W{p}_B$之間的向量(世界坐標(biāo)系)。然后對(duì)該向量進(jìn)行旋轉(zhuǎn)平移變換，就能獲得地圖點(diǎn)在B坐標(biāo)系下的坐標(biāo))。
?IMU誤差的計(jì)算公式為：

?式6是在文獻(xiàn)[1]中獲得的。
個(gè)人理解：感覺$e_v,e_p$的計(jì)算和文獻(xiàn)[1]中不符，使用的$b_{(.)}^j$應(yīng)該是$b_{(.)}^i$。
?之后使用非線性優(yōu)化的方式優(yōu)化誤差。最后將當(dāng)前幀的優(yōu)化結(jié)果和Hessian矩陣設(shè)為一個(gè)先驗(yàn)信息，給后續(xù)幀做參考。

(b) 地圖未更新：同時(shí)優(yōu)化前后兩幀，但保持地圖點(diǎn)不變

圖3

?從圖中可看出，這個(gè)情況下只用地圖點(diǎn)保持不變，且優(yōu)化的對(duì)象從一個(gè)當(dāng)前幀增加到前后兩幀，對(duì)應(yīng)的待優(yōu)化狀態(tài)變量也變成了：

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

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

LocalMapping

?VIORB-SLAM和ORB-SLAM的局部建圖環(huán)節(jié)不同的地方在于：前者在進(jìn)行LocalBA時(shí)要多考慮一個(gè)IMU的誤差項(xiàng)約束。這兩個(gè)系統(tǒng)在LocalBA上的對(duì)比圖如下：

圖4

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

Loop Closing

?VIORB-SLAM 和 ORB-SLAM在閉環(huán)檢測(cè)環(huán)節(jié)上是差不多的。當(dāng)檢測(cè)到閉環(huán)之后，會(huì)對(duì)閉環(huán)中的關(guān)鍵幀進(jìn)行位姿圖優(yōu)化（減少計(jì)算量），只優(yōu)化位姿$（R,p）$，而不優(yōu)化IMU變量$（v，b）$。
?值得注意的是，位姿圖中優(yōu)化的狀態(tài)變量只有6個(gè)自由度，因?yàn)槌叨刃畔⒖梢栽贗MU初始化時(shí)估計(jì)出來。這和ORB-SLAM是不同的。
個(gè)人理解：因?yàn)殚]環(huán)匹配上的兩個(gè)關(guān)鍵幀之間很難有IMU的數(shù)據(jù)約束，所以位姿優(yōu)化不優(yōu)化IMU變量

Full BA

?使用一個(gè)獨(dú)立的線程進(jìn)行Full BA，優(yōu)化所有狀態(tài)量：位姿，速度和偏差。

IMU初始化

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

估計(jì)$b_g$

?可以通過每?jī)蓚€(gè)相鄰關(guān)鍵幀之間的旋轉(zhuǎn)測(cè)量值來估計(jì)陀螺儀偏差：

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

?優(yōu)化過程中假設(shè)$b_g$保持不變，通過最小化上述誤差項(xiàng)來估計(jì)出$b_g$。

估計(jì)尺度$s$和重力向量$g_W$（不考慮加速度偏差$b_a$）

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

?將式(10)帶入到式(3)，同時(shí)忽視其中的$b_a$后可得公式：

?該式子用于求解 $s$ 和 $g_W$ ，由求解線性方程來實(shí)現(xiàn)。為了避免求解速度變量，降低方程復(fù)雜度，使用三個(gè)連續(xù)關(guān)鍵幀之間存在的兩個(gè)相對(duì)位姿關(guān)系，以及式子(3)中的速度計(jì)算公式，來構(gòu)建線性方程組：

?為了方便，將關(guān)鍵幀 $i、i+1、i+2$寫作$1、2、3$。方程組中各項(xiàng)的表達(dá)式為：

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

估計(jì)加速度偏差($b_a$)，尺度($s$)和重力方向

?前面獲得的$s^{?},g_W^{?}$未考慮加速度偏差($b_a$)，原因是加速度偏差和重力很難區(qū)分開==(重力會(huì)影響到$b_a$，兩者相關(guān)性較大)==。而重力的方向和加速度偏差區(qū)分性很好，所以可以同時(shí)估計(jì)這兩者的量。
?首先認(rèn)為在地圖的第一參考慣性坐標(biāo)系I中，重力的方向?yàn)?span id="ze8trgl8bvbq" class="katex--inline">gI^={0,0,?1}\hat{g_{I}}=\{0,0,-1\}gI?^?={0,0,?1}，大小為$G$。而之前計(jì)算出來的$g_W^{?}$的方向?yàn)?span id="ze8trgl8bvbq" class="katex--inline">$\widehat{g_W}=\frac{g_W^{?}}{\Vert g_W^{?}\Vert }$。這時(shí)計(jì)算$W、I$坐標(biāo)系之間的旋轉(zhuǎn)變換：

個(gè)人理解：之前求出的$g_W^{?}$是在地圖第一參考幀中的重力在世界坐標(biāo)系下的向量表示，而$g_I$是重力方向在第一參考幀中的表示。求出$R_{WI}$后就能夠獲得所有軌跡與真實(shí)世界之間的旋轉(zhuǎn)關(guān)系(修正位姿估計(jì)值的方向)
?此時(shí)$g_W$可以表示成：$g_W=R_{WI}\widehat{g_I}G$ …(15)

式中的$R_{WI}$只包含x、y兩個(gè)軸上的旋轉(zhuǎn)角度，因?yàn)閦軸的旋轉(zhuǎn)不會(huì)影響到重力的方向。
?通過下面這個(gè)擾動(dòng)來優(yōu)化旋轉(zhuǎn)$R_{WI}$：

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

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

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

?仍是使用三個(gè)關(guān)鍵幀，得到如下線性方程組：

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

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

速度($v$)估計(jì)

?可以通過式(18)求出（用來初始化使用的）關(guān)鍵幀的速度，因?yàn)榇藭r(shí)$s、g_W、b_a、b_g$都已知。此外，還可以使用公式(3)求出最近的關(guān)鍵幀的速度。

在重定位后重新初始化$b_g,b_a$

?如果系統(tǒng)在長(zhǎng)期運(yùn)行后進(jìn)行了重定位操作，則使用式(9)重新初始化$b_g$。而$b_a$則通過式(19)求出，此時(shí)$s、g_W$都是已知的。此過程使用20個(gè)連續(xù)的幀來完成，這些幀都是由視覺估計(jì)位姿。

結(jié)尾

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

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

總結(jié)

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

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