目錄

VIO引出原因：

融合方案：

本文貢獻：

0、總體框架

0.1 數(shù)據(jù)預(yù)處理（IMU預(yù)積分見標題1）

0.2 初始化

A.滑動窗口(Sliding Window)純視覺SfM

B. 視覺慣性校準（IMU預(yù)積分與視覺結(jié)構(gòu)對齊）

0.3 緊耦合后端非線性優(yōu)化（IMU約束+視覺約束+閉環(huán)約束）

A 公式

B IMU殘差

C 視覺殘差

D 邊緣化 Marginalization（詳見標題2）

E 相機速率下的狀態(tài)估計--只有運動的VIO

0.4 重定位

A、回環(huán)檢測（只對關(guān)鍵幀）

B、回環(huán)候選幀之間的特征匹配

C、緊耦合重定位

0.5 全局位姿圖優(yōu)化

A、位姿圖中添加關(guān)鍵幀

B、4自由度位姿圖優(yōu)化

C、位姿圖管理

1、IMU預(yù)積分

1.1、IMU模型

1.2、連續(xù)時間IMU運動模型，積分 PVQ（兩幀之間）

1.3、運動模型的離散積分（前后IMU）

1.4、 IMU預(yù)積分

1.5、預(yù)積分量

1.6、預(yù)積分誤差

1.7、 預(yù)積分離散形式（IMU增量）

1.8、bias 預(yù)積分量（bias發(fā)生變化）

2、基于舒爾補的邊緣化

2.1、舒爾補

2.2 marg后形成的先驗

2.3 具體例子

---

VIO引出原因：

1）單純視覺：缺點: 尺度不確定性、單目純旋轉(zhuǎn)無法估計、快速運動易丟失、受圖像遮擋運動物體干擾。

? ? ? ? ? ? ? ? ? ? ? ? 優(yōu)點：不產(chǎn)生漂移、直接測量旋轉(zhuǎn)與平移。

2）單純IMU：缺點：零偏導(dǎo)致漂移、低精度IMU積分位姿發(fā)散

? ? ? ? ? ? ? ? ? ? ? ?優(yōu)點：快速響應(yīng)、可估計絕對尺度、角速度估計準確。

3）結(jié)合視覺+IMU：可用視覺彌補IMU的零偏，減少IMU由于零偏導(dǎo)致的發(fā)散和累計誤差，IMU可為視覺提供快速響應(yīng)的定位。

融合方案：

松耦合：將 IMU 定位與視覺的位姿直接后處理融合，融合過程對二者本身不產(chǎn)生影響，典型方案為卡爾曼濾波器

緊耦合：融合過程本身會影響視覺和 IMU 中的參數(shù)（如 IMU 的零偏和視覺的尺度）典型方案為 MSCKF 和非線性優(yōu)化。

? ? ? ? ? ? ? ? ? ? ?

本文貢獻：

1、一個緊耦合、基于優(yōu)化的單目視覺慣性里程計，具有相機-IMU外部校準和IMU偏置估計。
2、基于有界滑動窗口迭代進行估計。
3、基于滑動窗口里的關(guān)鍵幀維持視覺結(jié)構(gòu)，基于關(guān)鍵幀之間的IMU進行預(yù)積分維持慣性測量。
4、魯棒性：未知狀態(tài)的初始化、相機和IMU外參數(shù)的在線標定、球面不統(tǒng)一重投影誤差、回環(huán)檢測、四自由度位姿圖優(yōu)化（三位置和航向）

---

0、總體框架

包括五個部分：數(shù)據(jù)預(yù)處理、初始化、后端非線性優(yōu)化、閉環(huán)檢測、位姿圖優(yōu)化。

0.1 數(shù)據(jù)預(yù)處理（IMU預(yù)積分見標題1）

• 視覺：1）提取Harris角點，KLT金字塔光流跟蹤相鄰幀；2）2 維特征點先矯正為不失真的，然后在通過外點剔除后投影到一個單位球面上? ；3）去除異常點：先進行F矩陣測試，通過RANSAC去除異常點；4）關(guān)鍵幀選取：1、當前幀相對最近的關(guān)鍵幀的特征平均視差大于一個閾值就為關(guān)鍵幀（因為視差可以根據(jù)平移和旋轉(zhuǎn)共同得到，而純旋轉(zhuǎn)則導(dǎo)致不能三角化成功，所以這一步需要IMU預(yù)積分進行補償）2、當前幀跟蹤到的特征點數(shù)量小于閾值視為關(guān)鍵幀；
• IMU：1）兩幀k和k+1之間進行位置、速度、姿態(tài)（PVQ）預(yù)測；2）避免每次姿態(tài)優(yōu)化調(diào)整后重復(fù)IMU傳播，采用預(yù)積分算法，計算預(yù)積分誤差的雅克比矩陣和協(xié)方差項。

0.2 初始化

采用松耦合的傳感器融合方法得到初始值。首先用SFM進行純視覺估計滑動窗內(nèi)所有幀的位姿以及路標點逆深度，然后與IMU預(yù)積分對齊，繼而恢復(fù)對齊尺度s，重力g，imu速度v，和陀螺儀偏置bg。

VINS本文初始化過程中忽視掉了加速度計的bias，因為加速度計與重力耦合，并且重力向量很大，初始化過程動態(tài)過程很短，幅度又不大，加速度計偏置很難觀測到。

A.滑動窗口(Sliding Window)純視覺SfM

1、選擇一個滑動窗，在最后一幀與滑動窗之前幀尋找?guī)?#xff1a;跟蹤到的點數(shù)目大于30個的并且視差超過20的，找到后用5點法本質(zhì)矩陣初始化恢復(fù)出R和t。否則，滑動窗內(nèi)保留最新圖像幀，繼續(xù)等待下一幀。

2、隨意設(shè)置一個尺度因子，三角化這兩幀觀測到的所有路標點。再用PnP算法估計滑動窗內(nèi)所有其余幀的位姿?；瑒哟皟?nèi)全局BA重投影誤差優(yōu)化所有幀位姿。

3、假設(shè)IMU-Camera外參已知，乘上視覺得到的位姿，轉(zhuǎn)換到IMU坐標系下。

B. 視覺慣性校準（IMU預(yù)積分與視覺結(jié)構(gòu)對齊）

1、陀螺儀零偏bg標定

旋轉(zhuǎn)兩種方式：陀螺儀測量值和視覺觀測值，二者的誤差其實就是陀螺儀偏置bg。

目標函數(shù)：visual給出的相鄰幀間的旋轉(zhuǎn)應(yīng)等于IMU預(yù)積分的旋轉(zhuǎn)值Q之間的差。

? ?考慮到了陀螺儀bias噪聲，

我們得到了陀螺儀偏置bias的初始校準,需要將陀螺儀偏置bg代入到IMU預(yù)積分重新計算預(yù)積分。

2、速度v、重力g和尺度初始化s

優(yōu)化變量：速度、重力向量和尺度?

目標函數(shù)：相鄰兩幀IMU預(yù)積分增量與預(yù)測值之間平移、速度（P、V）的差。通過HX=B 利用cholesky分解獲得

?

結(jié)合得到：

通過求解線性最小二乘問題：

3、重力矢量修正

重力向量的大小是已知的，加入了模長限制，這導(dǎo)致三維重力向量只剩2個自由度。

主要做的是優(yōu)化方向，一個二維向量。

在其切線空間上用兩個變量重新參數(shù)化重力，采用球面坐標進行參數(shù)化：

其中，是已知的重力的大小，為重力方向的單位向量。b1和b2是跨越切平面的兩個正交基。w1和w2是待優(yōu)化變量，表示沿著兩個正交基方向的位移。

替換后，Hx=b，變化為：之后采用最小二乘對變量重新優(yōu)化。

其中，待優(yōu)化變量變?yōu)?#xff1a;

0.3 緊耦合后端非線性優(yōu)化（IMU約束+視覺約束+閉環(huán)約束）

初始化后，采用基于滑動窗口的緊耦合單目VIO進行狀態(tài)估計。

A 公式

需要優(yōu)化的量：IMU狀態(tài)（PVQ、加速度bias、陀螺儀bias）、IMU到Camera的外參、m+1個路標點逆深度。

第一個式子是滑動窗口內(nèi)所有狀態(tài)量，n是幀數(shù)，m是滑動窗口內(nèi)特征點總數(shù)。特征點逆深度為了滿足高斯系統(tǒng)。

第二個式子xk是在第k幀圖像捕獲到的IMU狀態(tài)，包括位置，速度，旋轉(zhuǎn)（PVQ）和加速度偏置，陀螺儀偏置。

第三個式子是相機外參。

xk只與IMU項和Marg有關(guān)；特征點深度也只與camera和Marg有關(guān)；

視覺慣性BA，最小化邊緣化的先驗信息和IMU、視覺測量殘差之和。

BA優(yōu)化模型分為三部分：

1、Marg邊緣化殘差部分（滑動窗口中去掉位姿和特征點約束）

2、IMU殘差部分（滑動窗口中相鄰幀間的IMU產(chǎn)生）

3、視覺代價誤差函數(shù)部分（滑動窗口中特征點在相機下視覺重投影殘差）

B IMU殘差

殘差：狀態(tài)量傳播預(yù)測與IMU預(yù)積分的殘差

優(yōu)化變量：IMU時刻下的p位置，v速度，Q旋轉(zhuǎn)，兩個偏置ba,bw

C 視覺殘差

與傳統(tǒng)的針孔相機模型不同，這里用的是單位半球體的相機觀測殘差。是一個魚眼相機。

在相機的歸一化平面上比較殘差，再將視覺殘差投影到單位球面的正切平面上。由于視覺殘差的自由度是2，所以我們將殘差向量投影到切平面上。

在第i幀第一次觀測到第l個路標點，在第j幀中對該路標點進行觀測的殘差為：

第一個式子就是殘差的表達式，第二個式子是魚眼相機反投影函數(shù)將觀測到的像素坐標轉(zhuǎn)換成單位向量的觀測值數(shù)據(jù)，b1和b2是此單位向量的切平面上的一組基。第三個式子是重投影估計模型。其實VINS代碼中也可以使用普通的針孔相機模型。

D 邊緣化 Marginalization（詳見標題2）

為了防止pose和特征的個數(shù)的復(fù)雜度隨著時間不斷增長，引入邊緣化，在移除位姿時將關(guān)聯(lián)的約束轉(zhuǎn)化為先驗放入優(yōu)化問題中。

為了限制基于優(yōu)化的VIO計算復(fù)雜度，引入邊緣化。有選擇地從滑動窗口中將IMU狀態(tài)xK和特征λ1邊緣化，同時將對應(yīng)于邊緣狀態(tài)的測量值轉(zhuǎn)換為先驗。

分為兩種情況，

1、一種是倒數(shù)第二幀如果是關(guān)鍵幀的話，將最舊的pose移出Sliding Window，將最舊幀關(guān)聯(lián)的視覺和慣性數(shù)據(jù)邊緣化掉。把第一個老關(guān)鍵幀及其測量值被邊緣化；Margin_Old作為先驗值。

2、如果倒數(shù)第二幀不是關(guān)鍵幀的話，那么就只剔除倒數(shù)第二幀的視覺觀測，而不剔除它的IMU約束。原因是邊緣化保證關(guān)鍵幀之間有足夠視差而能夠三角化足夠多的地圖點。并且保證了IMU預(yù)積分的連貫性。

為了保持系統(tǒng)的稀疏性，我們不會邊緣化非關(guān)鍵幀的所有測量值。

E 相機速率下的狀態(tài)估計--只有運動的VIO

采用了一種輕量級的純運動視覺慣性BA，以提升狀態(tài)估計速率到相機速率(30Hz)。

代價函數(shù)不變。

1、只對固定數(shù)量的最新IMU狀態(tài)的姿態(tài)pose和速度v進行了優(yōu)化，而不是對滑動窗口中的所有狀態(tài)進行優(yōu)化

2、將特征深度、外部參數(shù)、偏置和舊的IMU狀態(tài)這些不希望優(yōu)化的狀態(tài)作為常量來處理。

與在最先進的嵌入式計算機上可能導(dǎo)致超過50ms的完全緊耦合單目VIO不同，這種純運動的視覺慣性BA只需大約5ms來計算。

0.4 重定位

盡管滑動窗和邊緣化減小了計算復(fù)雜度，但是仍舊引進了系統(tǒng)的累計漂移誤差。具體來說，就是全局三維位置（xyz）和圍繞重力方向的旋轉(zhuǎn)（yaw）。作者采用緊耦合重定位模塊與單目VIO進行組合實現(xiàn)漂移誤差的消除。

目的：局部滑動窗口移動并與過去的位姿對齊。

vins的重定位模塊主要包含回環(huán)檢測,回環(huán)候選幀之間的特征匹配,緊耦合重定位三個部分

上圖展示了重定位步驟。

1中VIO啟動時刻只進行位姿估計（藍色部分），過去狀態(tài)一直被記錄（綠色部分）。

2中如果最新幀中回環(huán)被檢測到，呈現(xiàn)紅色虛線連接，表示啟動重定位3。

4中多個特征的多個觀測直接用于重定位，從而提高了定位的精度和狀態(tài)估計的平滑性。

5-7是位姿優(yōu)化。

A、回環(huán)檢測（只對關(guān)鍵幀）

1、采用DBoW2詞袋位置識別方法進行回環(huán)檢測。經(jīng)過時間空間一致性檢驗后，DBoW2返回回環(huán)檢測候選幀。

2、除了用于單目VIO的角點特征外，還添加了500個角點并使用BRIEF描述子，描述子用作視覺詞袋在數(shù)據(jù)庫里進行搜索。這些額外的角點能用來實現(xiàn)更好的回環(huán)檢測。

3、VINS只保留所有用于特征檢索的BRIEF描述子，丟棄原始圖像以減小內(nèi)存。

4、單目VIO可以觀測到滾動和俯仰角，VINS并不需要依賴旋轉(zhuǎn)不變性。

B、回環(huán)候選幀之間的特征匹配

1、檢測到回環(huán)時，通過BRIEF描述子匹配找到對應(yīng)關(guān)系。但是直接的描述子匹配會導(dǎo)致很多外點。

2、本文提出兩步幾何剔除法：

1）2D-2D：使用RANSAC進行F矩陣測試，

2）3D-2D：使用RANSAC進行PnP，基于已知的滑動窗特征點的3D位置，和回路閉合候選處圖像的2D觀測（像素坐標）。

當內(nèi)點超過一定閾值時，我們將該候選幀視為正確的循環(huán)檢測并執(zhí)行重定位。

C、緊耦合重定位

1、重定位過程使單目VIO維持的當前滑動窗口與過去的位姿圖對齊。

2、將所有回環(huán)幀的位姿作為常量，利用所有IMU測量值、局部視覺測量和從回環(huán)中提取特征對應(yīng)值，共同優(yōu)化滑動窗口。

和之前VIO優(yōu)化模型不同的是，增加了回環(huán)項，從位姿圖獲得回環(huán)幀的姿態(tài)被視為常數(shù)。

在重定位之后（重定位只是基于檢測到的回環(huán)處暫時對滑動窗里的位姿進行重新優(yōu)化）

下一步要對過去位姿和閉合回路圖像幀的全局優(yōu)化。

0.5 全局位姿圖優(yōu)化

這一步是為了確保基于重定位結(jié)果對過去的位姿進行全局優(yōu)化。

由于視覺-慣性 使得橫滾角和俯仰角完全可以觀測，因此只有（XYZ和yaw航向）4個自由度存在累積漂移。接下來只進行4-DOF的位姿圖優(yōu)化。

A、位姿圖中添加關(guān)鍵幀

當一個關(guān)鍵幀被滑動窗口中邊緣化掉后，它會被添加到位姿圖中。該關(guān)鍵幀會作為位姿圖中一個定點，通過下面兩類邊與其他頂點相連接：

1、順序邊（Sequential Edge）：關(guān)鍵幀將建立與之前關(guān)鍵幀的幾個順序邊，一個順序邊表示局部滑動窗口中兩個關(guān)鍵幀之間的相對轉(zhuǎn)換，它的值直接從VIO中獲取。令最新邊緣化掉的關(guān)鍵幀為i，它的一個以前的關(guān)鍵幀為j，順序邊只包含相對位置和相對航向

2、回路閉合邊（Loop Closure Edge）：如果最新的邊緣化掉的關(guān)鍵幀存在回路連接，它可以通過位姿圖中的回路比河邊和回路閉合幀相連接?；丨h(huán)邊的值由重定位結(jié)果得出。

B、4自由度位姿圖優(yōu)化

關(guān)鍵幀i和j之間的殘差最小化表示為；

通過最小化以下代價函數(shù)，對順序邊和回環(huán)邊的整個圖進行優(yōu)化：

S是所有順序邊的集合，L是回環(huán)邊的集合。盡管緊耦合的重定位已經(jīng)有助于消除錯誤的回環(huán)，但我們添加了另一個Huber范數(shù)?ρ(·)，以進一步減少任何可能的錯誤回環(huán)的影響。相反，我們不對順序邊使用任何魯棒范數(shù)，因為這些邊是從VIO中提取出來的，VIO已經(jīng)包含了足夠多的外點排除機制。

位姿圖優(yōu)化和重定位(VII-C)異步運行在兩個獨立的線程中。以便在需要重定位時，能立即使用最優(yōu)化的位姿圖。同樣，即使當前的位姿圖優(yōu)化尚未完成，仍然可以使用現(xiàn)有的位姿圖配置進行重新定位。這一過程如圖9(b)所示。

C、位姿圖管理

隨著行程距離的增加，位姿圖的大小可能會無限增長，從而限制了長時間系統(tǒng)的實時性。為此，我們實行了一個下采樣過程：將位姿圖數(shù)據(jù)庫保持在有限的大小。所有具有回環(huán)約束的關(guān)鍵幀都將被保留，而其他與相鄰幀過近或方向非常相似的關(guān)鍵幀可能會被刪除。關(guān)鍵幀被移除的概率和其相鄰幀的空間密度成正比。

---

1、IMU預(yù)積分

1.1、IMU模型

測量值：加速度計a^、陀螺儀w^, 加上了bias游走和隨機白噪聲。

真實值：加速度計a、陀螺儀w。

實際情況下，可以獲得測量值a^和w^，需要反推真實值。一般忽略隨機游走高斯噪聲n

w=w^-bg; ? ? a=qwb(a^-ba)-gw;

1.2、連續(xù)時間IMU運動模型，積分 PVQ（兩幀之間）

將第k幀和第k+1幀所有的IMU進行積分，可得到第k+1幀的 PVQ，作為視覺估計的初始值。

a和w是IMU測量的加速度和角速度，相對于Body坐標系。

1.3、運動模型的離散積分（前后IMU）

從第 i個IMU時刻到第 i+1個IMU時刻的積分過程。兩個相鄰時刻k到k+1的位姿是由第k時刻測量值a^,w^計算得出的。

這與Estimator::processIMU（）函數(shù)中Ps[j]、Rs[j]、Vs[j]是一致的，代碼中j就是此處的i+1

IMU積分出來第 j 時刻數(shù)值作為第 j 幀圖像初始值。

歐拉法

中值法

1.4、 IMU預(yù)積分

每次qwbt優(yōu)化更新后，都要重新進行積分，運算量較大。

將積分模型轉(zhuǎn)為預(yù)積分模型：

PVQ積分公式中的積分項變?yōu)橄鄬τ诘趇時刻的姿態(tài)，而不是相對于世界坐標系的姿態(tài)

1.5、預(yù)積分量

預(yù)積分量只與IMU測量值有關(guān)。

1.6、預(yù)積分誤差

一段時間內(nèi)IMU構(gòu)建的預(yù)積分量作為測量值，與估計值進行相減。

1.7、 預(yù)積分離散形式（IMU增量）

中值法：k到k+1時刻位姿由兩時刻的測量值a w的平均值來計算。

1.8、bias 預(yù)積分量（bias發(fā)生變化）

因為 i 時刻的 bias 相關(guān)的預(yù)積分計算是通過迭代一步一步累計遞推的，可以算但是太復(fù)雜。所以對于預(yù)積分量直接在 i 時刻的 bias 附近用一階泰勒展開來近似，而不用真的去迭代計算。

?

---

2、基于舒爾補的邊緣化

基于高斯牛頓的非線性優(yōu)化理論可知，H*delta_x=b可以寫成：

其中，delta_xa和delta_xb分別是希望marg掉的部分和保留部分。

VINS中需要邊緣化滑動窗口中的最老幀，目的是希望不再計算這一幀的位姿或者與其相關(guān)的路標點，但是希望保留該幀對窗口內(nèi)其余幀的約束關(guān)系。我們基于與移除狀態(tài)相關(guān)的所有邊緣化測量值構(gòu)造一個先驗。新的先驗項被添加到現(xiàn)有的先驗項中。

2.1、舒爾補

2.2 marg后形成的先驗

xa為需要marg的變量，假設(shè)為相機pose,我們更關(guān)心如何求解希望保留的xb，而不再求解xa（即marg的變量改為0，左乘時左上是0），這里是要變化為上三角。

即：new_H*delta_xb=new_b;

形成新的信息矩陣new_H具體流程：

注意：去掉了x1,但是之前和x1相連的所有量x2? x3? x4? x5 在marg掉x1后變得兩兩相連。

2.3 具體例子

?

2.3.1 原來的信息矩陣H的構(gòu)成

上述最小二乘問題，對用的高斯牛頓求解為：

矩陣乘法公式寫成連加：

雅克比J和信息矩陣H的稀疏性：由于每個殘差只和某幾個狀態(tài)量有關(guān)，因此，雅克比矩陣求導(dǎo)時，無關(guān)項的雅克比為 0。

將五個殘差的信息矩陣加起來，得到樣例最終的信息矩陣 Λ, 可視化如下

2.3.2 舒爾補后形成新的信息矩陣new_H,并構(gòu)造為先驗

2.3.3 新測量信息和先驗構(gòu)成新的系統(tǒng)

參考：

https://blog.csdn.net/qq_41839222/article/details/85793998?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task

https://blog.csdn.net/wangshuailpp/article/details/78461171

https://blog.csdn.net/u014527548/article/details/86632197

https://www.cnblogs.com/ilekoaiq/p/8836970.html

https://blog.csdn.net/max_hope/article/details/90046770

總結(jié)

以上是生活随笔為你收集整理的VINS-Mono 论文解读（IMU预积分残差+Marg边缘化）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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