視覺慣性SLAM：VINS-Mono

• 這篇博客
• 一些符號說明
• IV 測量數據的預處理
• • A.視覺處理前端
  • B.IMU預積分
• V. 初始化
• A.Vision-Only SfM in Sliding Window
• • B.Visual-Inertial Alignment
• VI.TIGHTLY COUPLED MONOCULAR VIO
• • A.公式介紹
  • B.IMU誤差
  • C.視覺誤差
  • D.邊緣化
  • E.位姿優化
  • F.以IMU采集的速率估計位姿值
• VII 重定位（和VI ORB不同）
• • A.閉環檢測
  • B.特征找回
  • C.緊耦合的重定位
• VIII全局優化和地圖復用
• • A.四個存在漂移問題的變量
  • B.向位姿圖中加KF
  • C. 4自由度的位姿圖優化
  • D.位姿圖合并
  • E.位姿圖保存
  • F.加載位姿圖
• 結尾

這篇博客

?最近看了VINS-Mono，所以就寫篇關于系統各部分內容的總結，方便自己之后進行論文的回顧，也希望大家能從中有所收獲。一些我認為比較重要的地方和自己的理解都用了顏色標注。好的，話不多說，直接莽。
【轉載聲明】本篇文字均為原創，如轉載請注明出處

一些符號說明

1、$(.{)}^w$：變量表示在世界坐標系下；
2、$(.{)}^b$：變量表示在$IMU$坐標系(Body坐標系)下；
3、$(.{)}^c$：變量表示在相機坐標系下；
4、$R、q$：旋轉矩陣及其對應的四元數(在緊耦合的狀態變量中使用)；
5、$q_b^w,p_b^w$：從$IMU$坐標系到世界坐標系的旋轉和平移變換；
6、$?$：表示四元數之間的乘法；
7、$b_k,c_k$：第$k$幀對應的$IMU$和相機坐標系；
8、$g^w=[0,0,G{]}^T$：世界坐標系下的重力向量；
9、$\widehat{(.)}$：表示帶有噪聲的測量值或一個定值估計；
10、$b_w,b_a$：表示陀螺儀和加速度偏差。

IV 測量數據的預處理

?這部分介紹如何處理視覺和IMU的測量數據。大致內容為：跟蹤每一幀，在新幀上提取一定數量特征點；預積分IMU數據。

A.視覺處理前端

?對于新獲得的當前幀：
1、用$KLT$稀疏光流法跟蹤已有特征點(上一幀中的)；
2、若跟蹤到的特征點較少，則在當前幀中提取新的特征點，以保證至少有100-300個特征點（提取的特征點應滿足均勻分布）；
3、用跟蹤上的特征點求解前后幀的$F$單應矩陣，并用RANSAC法進行外點剔除；
4、將剩余內點反投影到當前幀的歸一化平面上
?此外，這個部分還需要判斷是否產生新的關鍵幀。判斷條件是：
$?$ 跟蹤到的特征點的平均視差大于一定閾值；
$?$ 跟蹤到的特征點數量小于一定閾值。

B.IMU預積分

1)IMU的噪聲和偏差：
?IMU 元件獲得的原始測量值(角速度$\hat{w}$和加速度$\hat{a}$)會受到重力、偏差和噪聲的影響：

這些測量值是在$t$時刻的IMU坐標系中表示 。式中$(.{)}_t$表示真實值，$n$表示白噪聲。而某一時刻的加速度$b_{a_t}$和陀螺儀偏差$b_{w_t}$被建模為隨機行走(它們的導數為高斯白噪聲)。

2)預積分：
?先設定一個偏差的估計值，將$b_k$和$b_{k+1}$這前后兩幀的 IMU 數據進行預積分。積分后的結果在$b_k$的坐標系中表示：


?可以看出，式3 中的每一項都能直接通過 IMU 的測量數據來求解，它們表示的是前后兩幀之間的相對變換關系。

3)偏差修正：
?如果估計的偏差$b_{a_k}$、$b_{w_k}$發生了微小的變化，就要相應的調整預積分的結果(根據它們對$b_{a_k}$、$b_{w_k}$的雅可比矩陣)：

?如果偏差變化太大，則需要用新的值重新進行預積分。

V. 初始化

?通過視覺數據和IMU數據構建約束關系，以獲得一些必要變量的初值。

A.Vision-Only SfM in Sliding Window

?先只使用視覺來獲得一系列相機位姿和地圖點坐標(未知尺度)，構建最初的滑動窗口。為控制復雜程度，只在窗口中保留一定數量的幀。
具體的步驟如下：
1、構建含有一定數量幀的滑動窗口，此時所有幀的位姿都未知；
2、獲得新的幀，為其在窗口內尋找一個匹配幀(要求：匹配的特征個數多于30，同時平均視差大于20個像素單位)；
3、使用五點法恢復兩幀之間的R和sp(帶尺度的平移);
4、隨意設一個尺度$s_{{}^{\prime }}$，對所有被兩幀觀測到的特征進行三角化，構建初始地圖點；
5、用這些地圖點估計出窗口中其他幀的位姿(使用PnP法)；
6、用BA優化各幀的位姿(優化重投影誤差)。

?因為還沒有關于世界坐標系的任何信息，所以將窗口內第一幀作為第一參考幀$(.{)}^{c_0}$，其余幀的位姿和地圖點的坐標都用第一參考幀為基準來表示，分別記為$(.{)}_{c_k}^{c_0}，(.{)}^{c_0}$。
?根據標定好的相機到 IMU 元件的變換關系，可以將相機坐標系轉換到IMU坐標系，但是 $s$ 是未知的:

B.Visual-Inertial Alignment

?原理：將 A部分估計的位姿和IMU數據的預積分結合起來，估計出尺度、偏差等變量。（具體求解方法和 VI ORB-SLAM 存在一些不同，最大的不同在于這里使用四元數表示旋轉）

1）陀螺儀偏差$b_w$估計:
?對于滑動窗口內相鄰兩幀有用視覺估計出的位姿$q_{b_k}^{c_0},q_{b_{k+1}}^{c_0}$，以及IMU預積分的結果${\hat{\tau }}_{k+1}^k$。所以可通過最小化下面公式的誤差來獲得$b_w$：

式中$B$表示窗口內所有幀。在獲得$b_w$新的估計值后，重新計算一次IMU預積分。

2）速度$v$、重力向量$g^{c_0}$和尺度估計$s$：
?這一階段估計的所有狀態變量如下：

?對于滑動窗口內前后兩幀$b_k,b_{k+1}$有下面的公式(這里也忽視了先加速度偏差，和VIORB一樣)：

然后將式(9)和式(6)混合，可以獲得下面的線性方程組：

其中${\hat{z}}_{k+1}^k$是由 IMU 測量數據計算得出（帶噪聲），而$H_{b_{k+1}}^{b_k}$由視覺的計算結果組成：

最后通過優化下面的誤差獲得各幀的速度，重力向量（在第一參考幀坐標系$(.{)}^{c_0}$下）和尺度:

3）優化重力向量：
?由于重力向量$g^{c_0}$的大小是已知的G，所以可以想象重力向量在一個半徑為G的圓上，如圖4：

此時重力向量就只有2個自由度，可在圖中的重力向量的切平面上用兩個正交的變量去擾動它(改變其方向)：$b1、b2$。擾動的公式為：$G(\bar{\hat{g}}+\delta g),\delta g=w_1{b}_1+w_2{b}_2$。其中G是重力的大小（用這個可能更好理解一點），$\bar{\hat{g}}$是重力的單位方向向量，$b_1、b_2$是切平面上的兩個正交基；$w_1、w_2$是兩個擾動方向的大小。最后$G(\bar{\hat{g}}+\delta g)$替換掉式子(9)中的$g$，將2自由度的 $\delta g$ 和其他變量一起求解（包括速度$v$、尺度$s$、加速度偏差$b_a$）。
個人理解：所有變量的求解思路和VI ORB-SLAM的思路是相似的，但更新重力向量的方式有所不同

4）完成初始化：
?使用最終求出的重力向量與真實世界的重力向量$[0,0,?1{]}^T$，求出$q_{c_0}^w$，即第一參考幀與世界坐標系之間的變換關系。然后將地圖點都轉到世界坐標系中，軌跡也用求出的尺度進行修正。

VI.TIGHTLY COUPLED MONOCULAR VIO

?在滑動窗口內求解每個視覺+慣性的緊耦合狀態變量，如圖5所示：

A.公式介紹

?緊耦合后的狀態變量如下：

$x_k$是第$k$個狀態變量，包含位姿、速度和偏差。${\lambda }_l$是第 $l$ 個地圖點在其第一觀測幀中的逆深度。此時待優化的誤差函數為：

式中第一項是先驗誤差，第二項是 IMU 誤差，第三項是重投影誤差，式(15) 表示魯棒核函數。

B.IMU誤差

?滑動窗口內相鄰兩幀間的IMU誤差為：

式中[.]XYZ表示提取出四元數Q中的向量部分來表示誤差狀態。

個人理解：每一個誤差項=當前估計的狀態變量求出的相對變換 — IMU預積分求出的相對變換。

C.視覺誤差

?在歸一化平面上計算視覺誤差。以在第 $i$ 幀上觀測到的第 $l$ 個地圖點為例，他在第 $j$ 幀上的視覺誤差為：
式中$\pi$是相機投影函數；$u，v$是像素坐標。
?由于上述視覺誤差的自由度為2，所及將誤差向量投影到切平面上，并用兩個正交方向上的擾動來優化誤差，如圖6所示：

個人理解：這里的2自由度是因為重投影誤差是在歸一化平面上的，有一維坐標一直為1

D.邊緣化

?用這個方法控制滑動窗口內KF和地圖點的數量，并把邊緣化的幀作為一個先驗信息。邊緣化的兩種情況如圖7所示：

?圖中棕色圖形表示兩個最新的幀，此時邊緣化的操作有兩種：
1、若圖中n-1幀是KF，則將其放入窗口中，并把窗口中最老的KF丟掉。（但保留它的IMU信息作為先驗）
2、若n-1幀不是KF，則只保留它的IMU預積分結果，也作為一個先驗信息。（幫助計算KF與下一幀之間的IMU預積分）
（對第一種情況的先驗信息不是很理解）

E.位姿優化

?為了降低計算量，只進行運動上的位姿優化，如圖8所示：

?如圖所示，不優化窗口內的所有$KF$，只優化最近一定數量的$KF$和當前幀。優化時使用的誤差式子和式（14）一樣，即 IMU 誤差、視覺誤差和先驗誤差。

F.以IMU采集的速率估計位姿值

?系統的處理速率受幀速率影響，但可以結合最新幀的估計值，使用之后一系列(在下一幀到來前的）的IMU數據來獲得和IMU采集速率一樣的位姿估計。這個高速率的估計值可用于閉環的狀態反饋。(最后一句話不是很理解，期待各位博友指點迷津)

VII 重定位（和VI ORB不同）

?作用：消除累積誤差（漂移）

A.閉環檢測

?使用DBOW2詞袋中的單詞來描述每一幀，再通過這個描述完成閉環檢測。(這一部分可參考《視覺SLAM十四講》P303)

B.特征找回

目的：確定閉環對應的兩個$KFs$之間的特征匹配和變換關系。
?當檢測到閉環后，窗口（一般是當前最新的$KF$）與匹配的閉環幀（記為$K{F}_v$）（來自系統保存$KFs$的數據庫中）的關系就能被建立起來（通過特征匹配）。為了降低誤匹配，使用下面兩個幾何的方法來去除外點：
1）2d-2d：通過RANSAC求出$K{F}_v$和當前$KF$之間的單應矩陣$F$。
2）3d-2d：將滑動窗口內已有地圖點，和其在$K{F}_v$上匹配的觀測點進行基于RANSAC法的PnP問題求解。
?如此就能求出$v$和當前$KF$之間的變換關系，并剔除了外點。

C.緊耦合的重定位

目的：發現閉環后，將當前滑動窗口與以往幀進行位姿對齊。
?窗口內每產生一個新的$KF$就為其在數據庫中查詢是否存在匹配的閉環KF。在找到匹配的閉環$KF$后（記為$K{F}_v$），將它作為一個閉環約束，構建一個新的誤差函數：

式中$K{F}_v$提供的約束就是滑動窗口內地圖點在$K{F}_v$上的重投影誤差，它可以幫助修正窗口的所有地圖點坐標和$KFs$的位姿。圖9展示了整個閉環檢測和重定位的過程：

?從圖中可以看出，前三步為：正常跟蹤、發現閉環、重定位滑動窗口。在這之后，窗口內還有不斷產生新的$KF$，新來的KF也可能找到對應的閉環幀，所以閉環約束就會增加，這就形成了多視圖約束。
個人理解 ：在第一個發生閉環的KF即將被邊緣化時，進行一次全局BA，把整體軌跡整合進行修正。

VIII全局優化和地圖復用

A.四個存在漂移問題的變量

?受益于重力向量的慣性測量，系統可以估計$(.{)}^{b_i}$與$(.{)}^w$在roll、pitch上的絕對旋轉（x，y軸旋轉，z軸的旋轉不會影響重力向量的方向），如圖11所示。此時只會在$x，y，z，yaw$四個變量上產生累積誤差。所以全局位姿圖優化只用優化這四個變量。

B.向位姿圖中加KF

?視覺-慣性里程計處理后的KF會加到位姿圖中。它 會被當作一個頂點，并通過兩類邊和其他頂點產生聯系：
1） 連續邊：某個$KF$與之前的$KFs$之間的相對變換關系。只管那四個變量：

2） 閉環邊：只有形成閉環的$KFs$之間才有，表示它們之間的相對變換，公式和式(19)一樣。
?整個位姿圖的樣子就如下圖所示：

C. 4自由度的位姿圖優化

?定義$i、j$兩個KF之間的邊的誤差為：

其中$\hat{\theta },\widehat{?}$是固定的。整個位姿圖中所有邊產生的總誤差公式為：

式中$S$ 為所有連續邊，$L$為閉環邊。$\rho$ 是魯棒核函數。
?位姿圖優化和重定位在兩個異步的線程中同時進行，目的是：
1、可讓數據庫中優化后的位姿圖立馬能用于新的重定位操作中（異步不用互相等待）；
2、

D.位姿圖合并

?通過地圖間的閉環關系，將當前構建的位姿圖與已有的位姿圖合并。找到閉環關系后，直接使用全局BA即可。過程如圖13所示：

E.位姿圖保存

?數據的保存內容：所有頂點和邊；關鍵幀（頂點）的狀態變量。
舉個例子：

其中 $i$ 表示幀的編號，${\hat{p}}_i^w$、${\hat{q}}_i^w$是這一幀的平移和旋轉變換(從當前幀到世界坐標系)。$v$是與這個幀形成閉環的匹配幀的編號，${\hat{p}}_{iv}^i$和${\hat{\psi }}_{iv}$是他們之間的相對位移和 $yaw$ 方向的旋轉角度（由重定位獲得）。$D(u,v,des)$是這一幀中的特征點坐標和它對應的描述子。

F.加載位姿圖

?怎么保存的就怎么加載。

結尾

?一口氣看完兩篇視覺慣性SLAM，感覺也大致懂了這類SLAM的工作原理。還是要抓緊看代碼，畢竟編程能力十分重要（太忙了）。最后希望這篇博客能幫助到大家，其中存在的不足也希望大家能夠指出，十分感謝！

參考資料：
1、VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
2、《視覺SLAM十四講》

總結

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

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