VINS-Mono 理論詳細解讀——緊耦合後端非線性優化 IMU+視覺的殘差residual、Jacobian、協方差、基於舒爾補的邊緣化

預積分和後端優化IMU部分**
https://blog.csdn.net/weixin_44580210/article/details/93377806

本講是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、優化變量

3、Jacobian

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

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

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

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）的過程其實就是根據概率圖模型（多元高斯分佈）建立各個節點變量間的信息矩陣（協方差矩陣的逆）的過程，而邊緣化則是去掉概率圖中的某一個節點後信息矩陣會發生怎樣的變換的問題。
基於高斯牛頓的非線性優化理論可知，Hdelta_x=b可以寫成：

其中，delta_xa和delta_xb分別是希望marg掉的部分和保留部分。
VINS中需要邊緣化滑動窗口中的最老幀，目的是希望不再計算這一幀的位姿或者與其相關的路標點，但是希望保留該幀對窗口內其餘幀的約束關係。我們基於與移除狀態相關的所有邊緣化測量值構造一個先驗。新的先驗項被添加到現有的先驗項中。
3、舒爾補

4、marg後形成的先驗

xa爲需要marg的變量，假設爲相機pose,**我們更關心如何求解希望保留的xb，**而不再求解xa（即marg的變量改爲0，左乘時左上是0），這裏是要變化爲上三角。

**即：new_Hdelta_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