目錄

VIO引出原因：

融合方案：

本文貢獻：

0、總體框架

0.1 數據預處理（IMU預積分見標題1）

0.2 初始化

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

B. 視覺慣性校準（IMU預積分與視覺結構對齊）

0.3 緊耦合后端非線性優化（IMU約束+視覺約束+閉環約束）

A 公式

B IMU殘差

C 視覺殘差

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

E 相機速率下的狀態估計--只有運動的VIO

0.4 重定位

A、回環檢測（只對關鍵幀）

B、回環候選幀之間的特征匹配

C、緊耦合重定位

0.5 全局位姿圖優化

A、位姿圖中添加關鍵幀

B、4自由度位姿圖優化

C、位姿圖管理

1、IMU預積分

1.1、IMU模型

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

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

1.4、 IMU預積分

1.5、預積分量

1.6、預積分誤差

1.7、 預積分離散形式（IMU增量）

1.8、bias 預積分量（bias發生變化）

2、基于舒爾補的邊緣化

2.1、舒爾補

2.2 marg后形成的先驗

2.3 具體例子

---

VIO引出原因：

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

? ? ? ? ? ? ? ? ? ? ? ? 優點：不產生漂移、直接測量旋轉與平移。

2）單純IMU：缺點：零偏導致漂移、低精度IMU積分位姿發散

? ? ? ? ? ? ? ? ? ? ? ?優點：快速響應、可估計絕對尺度、角速度估計準確。

3）結合視覺+IMU：可用視覺彌補IMU的零偏，減少IMU由于零偏導致的發散和累計誤差，IMU可為視覺提供快速響應的定位。

融合方案：

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

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

? ? ? ? ? ? ? ? ? ? ?

本文貢獻：

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

---

0、總體框架

包括五個部分：數據預處理、初始化、后端非線性優化、閉環檢測、位姿圖優化。

0.1 數據預處理（IMU預積分見標題1）

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

0.2 初始化

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

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

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

1、選擇一個滑動窗，在最后一幀與滑動窗之前幀尋找幀：跟蹤到的點數目大于30個的并且視差超過20的，找到后用5點法本質矩陣初始化恢復出R和t。否則，滑動窗內保留最新圖像幀，繼續等待下一幀。

2、隨意設置一個尺度因子，三角化這兩幀觀測到的所有路標點。再用PnP算法估計滑動窗內所有其余幀的位姿。滑動窗內全局BA重投影誤差優化所有幀位姿。

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

B. 視覺慣性校準（IMU預積分與視覺結構對齊）

1、陀螺儀零偏bg標定

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

目標函數：visual給出的相鄰幀間的旋轉應等于IMU預積分的旋轉值Q之間的差。

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

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

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

優化變量：速度、重力向量和尺度?

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

?

結合得到：

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

3、重力矢量修正

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

主要做的是優化方向，一個二維向量。

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

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

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

其中，待優化變量變為：

0.3 緊耦合后端非線性優化（IMU約束+視覺約束+閉環約束）

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

A 公式

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

第一個式子是滑動窗口內所有狀態量，n是幀數，m是滑動窗口內特征點總數。特征點逆深度為了滿足高斯系統。

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

第三個式子是相機外參。

xk只與IMU項和Marg有關；特征點深度也只與camera和Marg有關；

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

BA優化模型分為三部分：

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

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

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

B IMU殘差

殘差：狀態量傳播預測與IMU預積分的殘差

優化變量：IMU時刻下的p位置，v速度，Q旋轉，兩個偏置ba,bw

C 視覺殘差

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

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

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

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

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

為了防止pose和特征的個數的復雜度隨著時間不斷增長，引入邊緣化，在移除位姿時將關聯的約束轉化為先驗放入優化問題中。

為了限制基于優化的VIO計算復雜度，引入邊緣化。有選擇地從滑動窗口中將IMU狀態xK和特征λ1邊緣化，同時將對應于邊緣狀態的測量值轉換為先驗。

分為兩種情況，

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

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

為了保持系統的稀疏性，我們不會邊緣化非關鍵幀的所有測量值。

E 相機速率下的狀態估計--只有運動的VIO

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

代價函數不變。

1、只對固定數量的最新IMU狀態的姿態pose和速度v進行了優化，而不是對滑動窗口中的所有狀態進行優化

2、將特征深度、外部參數、偏置和舊的IMU狀態這些不希望優化的狀態作為常量來處理。

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

0.4 重定位

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

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

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

上圖展示了重定位步驟。

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

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

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

5-7是位姿優化。

A、回環檢測（只對關鍵幀）

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

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

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

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

B、回環候選幀之間的特征匹配

1、檢測到回環時，通過BRIEF描述子匹配找到對應關系。但是直接的描述子匹配會導致很多外點。

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

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

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

當內點超過一定閾值時，我們將該候選幀視為正確的循環檢測并執行重定位。

C、緊耦合重定位

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

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

和之前VIO優化模型不同的是，增加了回環項，從位姿圖獲得回環幀的姿態被視為常數。

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

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

0.5 全局位姿圖優化

這一步是為了確保基于重定位結果對過去的位姿進行全局優化。

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

A、位姿圖中添加關鍵幀

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

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

2、回路閉合邊（Loop Closure Edge）：如果最新的邊緣化掉的關鍵幀存在回路連接，它可以通過位姿圖中的回路比河邊和回路閉合幀相連接。回環邊的值由重定位結果得出。

B、4自由度位姿圖優化

關鍵幀i和j之間的殘差最小化表示為；

通過最小化以下代價函數，對順序邊和回環邊的整個圖進行優化：

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

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

C、位姿圖管理

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

---

1、IMU預積分

1.1、IMU模型

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

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

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

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

1.2、連續時間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（）函數中Ps[j]、Rs[j]、Vs[j]是一致的，代碼中j就是此處的i+1

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

歐拉法

中值法

1.4、 IMU預積分

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

將積分模型轉為預積分模型：

PVQ積分公式中的積分項變為相對于第i時刻的姿態，而不是相對于世界坐標系的姿態

1.5、預積分量

預積分量只與IMU測量值有關。

1.6、預積分誤差

一段時間內IMU構建的預積分量作為測量值，與估計值進行相減。

1.7、 預積分離散形式（IMU增量）

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

1.8、bias 預積分量（bias發生變化）

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

?

---

2、基于舒爾補的邊緣化

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

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

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

2.1、舒爾補

2.2 marg后形成的先驗

xa為需要marg的變量，假設為相機pose,我們更關心如何求解希望保留的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的構成

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

矩陣乘法公式寫成連加：

雅克比J和信息矩陣H的稀疏性：由于每個殘差只和某幾個狀態量有關，因此，雅克比矩陣求導時，無關項的雅克比為 0。

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

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

2.3.3 新測量信息和先驗構成新的系統

參考：

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

總結

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

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