本講是VINS最核心部分了，前面經歷了

?1）視覺跟蹤feature_tracker、IMU預積分integrationBase類；

2）初始化中SFM純視覺估計滑動窗中所有幀的位姿和3D路標點深度、SFM與IMU預積分松耦合對齊求解初始化參數。

在完成前面的初始化環節后，本節將進行第3部分，基于滑動窗的緊耦合后端非線性優化：將視覺約束、IMU約束、閉環約束放到一個大的目標函數中進行非線性優化，求解出滑動窗口中所有幀的PVQ、bias等。

在視覺約束和IMU約束中，基本思想是找到優化狀態向量，然后通過視覺殘差和IMU測量殘差分別對狀態向量求導，獲得視覺和IMU預積分的Jacobian和協方差矩陣。

閱讀本文所需知識儲備：

1、最小二乘、梯度下降法、牛頓與高斯-牛頓與LM關系總結

2、《視覺SLAM十四講精品總結》4：非線性優化g2o(BA基礎)

3、IMU預積分integrationBase類

目錄

一、VIO殘差函數的構建

1、需要優化的狀態向量：

2、目標函數為：

二、視覺約束（細致）

1.視覺重投影誤差residual（歸一化平面）

2、優化變量

3、Jacobian

4、協方差

三、IMU約束（粗略，祥細見之前）

1、殘差：

2、優化變量：

3、IMU測量殘差公式推導

4、殘差對狀態量的Jacobian

5、殘差對狀態量的協方差

四、基于舒爾補的邊緣化

1、論文部分

2、基本公式

3、舒爾補

4、marg后形成的先驗

5 具體例子

5.1 原來的信息矩陣H的構成

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

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

?

---

一、VIO殘差函數的構建

1、需要優化的狀態向量：

滑動窗口內IMU狀態（PVQ、加速度bias、陀螺儀bias）、IMU到Camera的外參、m+1個3D路標點逆深度。

第一個式子是滑動窗口內所有狀態量，n是關鍵幀數量，m是滑動窗內所有觀測到的路標點總數，維度是15*n+6+m。特征點逆深度為了滿足高斯系統。

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

第三個式子是相機外參。

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

2、目標函數為：

視覺慣性BA：這三項分別為邊緣化的先驗信息、IMU的測量殘差、視覺的重投影誤差

BA優化模型分為三部分：

1、Marg邊緣化殘差部分（滑動窗口中去掉位姿和特征點約束）代碼中使用Google開源的Ceres solver解決。

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

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

二、視覺約束（細致）

這部分要擬合的目標可以通過重投影誤差約束，求解的是對同一個路標點的觀測值和估計值之間的誤差，注意是在歸一化平面上表示。

1.視覺重投影誤差residual（歸一化平面）

相關自變量是：該路標點被兩幀觀察到的相機幀位姿、Cam和IMU之間相對位姿、路標點在第一幀的逆深度。

?當某路標點在第i幀觀測到并進行初始化操作得到路標點逆深度，當其在第j幀也被觀測到時，估計其在第j幀中的坐標為：

此時的視覺殘差為：（左側為根據i幀反推估計的位置，右側為觀測值）

逆深度參數化

逆深度作為參數原因：1）觀測到的特征點深度可能非常大，難以進行優化；2）可以減少實際優化的參數變量；3）逆深度更加服從高斯分布。這里特征點的逆深度在第i幀初始化操作中得到。

論文中：

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

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

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

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

2、優化變量

包括兩個時刻的狀態量、外參，以及逆深度

值得注意的是：和純視覺不同的是，待優化的狀態量中的旋轉平移都是IMU系到w系，而不是Camera到w系。

狀態量特征：

1、待優化量和IMU約束中待優化量有重疊，再次體現了緊耦合。

2、和IMU不同的是，IMU每次優化的狀態量是相鄰兩幀的，但是視覺優化的2幀不一定是相鄰的，因此用i，j表示。

3、Jacobian

求解Jacobian就是用視覺殘差對上述7個狀態變量進行求導。

方法一：崔博直接給出答案

2）

方法二：深藍學院詳細推導

?

?

4、協方差

void Estimator::setParameter() {for (int i = 0; i < NUM_OF_CAM; i++){tic[i] = TIC[i];ric[i] = RIC[i];}f_manager.setRic(ric);ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();td = TD; }

三、IMU約束（粗略，祥細見之前）

1、殘差：

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

2、優化變量：

IMU的第i、j時刻下的p位置，v速度，Q旋轉（PVQ），兩個偏置ba,bw

之前對IMU預積分進行了詳細講解。參考：VINS-Mono 代碼詳細解讀——IMU離散中值預積分

3、IMU測量殘差公式推導

簡單來說，如果每一幀IMU都要相對于第一幀求位姿，計算量很大；考慮將PVQ積分公式改為相對第i時刻的姿態。

原PVQ積分公式：

基于預積分的PVQ積分公式

預積分量為：

最新的PVQ積分公式變為：

IMU測量殘差e為：

4、殘差對狀態量的Jacobian

這里直接給出結果，如果想看詳細推導，參考：VINS-Mono 代碼詳細解讀——IMU預積分的殘差、Jacobian和協方差 processIMU（）+intergrationBase類+imu_factor.h

對應位置補充上這個J的矩陣塊就行了，其他位置還是0。一共分為四大模塊：

整個殘差對某個變量求導；

例如：?指的是

5、殘差對狀態量的協方差

四、基于舒爾補的邊緣化

為什么進行邊緣化操作？

如果僅僅從前后兩幀圖像計算相機位姿，速度快但是精度低；但是采用全局優化BA，連接所有圖像幀，精度高但是復雜度高。

采用滑動窗，固定數量的幀進行優化，這樣能夠保證精度和速度。既然是滑動窗，在滑動的過程中會有新的圖像進來，舊的圖像離開，所謂邊緣化就是為了刪除圖像，但是把圖像信息約束保留下來。

1、論文部分

為了防止pose和特征的個數的復雜度隨著時間不斷增長，引入邊緣。

化，在移除位姿時將關聯的約束轉化為先驗放入優化問題中。

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

分為兩種情況，

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

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

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

2、基本公式

我們根據運動模型和觀測模型建立H矩陣（高斯牛頓法中的JJT）的過程其實就是根據概率圖模型（多元高斯分布）建立各個節點變量間的信息矩陣（協方差矩陣的逆）的過程，而邊緣化則是去掉概率圖中的某一個節點后信息矩陣會發生怎樣的變換的問題。

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

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

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

3、舒爾補

4、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后變得兩兩相連。

5 具體例子

?

5.1 原來的信息矩陣H的構成

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

矩陣乘法公式寫成連加：

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

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

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

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

?

?

?

參考文章：

VINS-Mono理論學習——后端非線性優化? ?by Manii

VINS-mono詳細解讀 by 極品巧克力

VINS-Mono之后端非線性優化 (目標函數中視覺殘差和IMU殘差，及其對狀態量的雅克比矩陣、協方差遞推方程的推導)? by Hansry

深藍學院手寫VIO課程

https://blog.csdn.net/iwanderu/article/details/104729332

?

?

總結

以上是生活随笔為你收集整理的VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化的全部內容，希望文章能夠幫你解決所遇到的問題。

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