本講是VINS最核心部分了，前面經(jīng)歷了

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

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

在完成前面的初始化環(huán)節(jié)后，本節(jié)將進(jìn)行第3部分，基于滑動窗的緊耦合后端非線性優(yōu)化：將視覺約束、IMU約束、閉環(huán)約束放到一個大的目標(biāo)函數(shù)中進(jìn)行非線性優(yōu)化，求解出滑動窗口中所有幀的PVQ、bias等。

在視覺約束和IMU約束中，基本思想是找到優(yōu)化狀態(tài)向量，然后通過視覺殘差和IMU測量殘差分別對狀態(tài)向量求導(dǎo)，獲得視覺和IMU預(yù)積分的Jacobian和協(xié)方差矩陣。

閱讀本文所需知識儲備：

1、最小二乘、梯度下降法、牛頓與高斯-牛頓與LM關(guān)系總結(jié)

2、《視覺SLAM十四講精品總結(jié)》4：非線性優(yōu)化g2o(BA基礎(chǔ))

3、IMU預(yù)積分integrationBase類

目錄

一、VIO殘差函數(shù)的構(gòu)建

1、需要優(yōu)化的狀態(tài)向量：

2、目標(biāo)函數(shù)為：

二、視覺約束（細(xì)致）

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

2、優(yōu)化變量

3、Jacobian

4、協(xié)方差

三、IMU約束（粗略，祥細(xì)見之前）

1、殘差：

2、優(yōu)化變量：

3、IMU測量殘差公式推導(dǎo)

4、殘差對狀態(tài)量的Jacobian

5、殘差對狀態(tài)量的協(xié)方差

四、基于舒爾補的邊緣化

1、論文部分

2、基本公式

3、舒爾補

4、marg后形成的先驗

5 具體例子

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

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

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

?

---

一、VIO殘差函數(shù)的構(gòu)建

1、需要優(yōu)化的狀態(tài)向量：

滑動窗口內(nèi)IMU狀態(tài)（PVQ、加速度bias、陀螺儀bias）、IMU到Camera的外參、m+1個3D路標(biāo)點逆深度。

第一個式子是滑動窗口內(nèi)所有狀態(tài)量，n是關(guān)鍵幀數(shù)量，m是滑動窗內(nèi)所有觀測到的路標(biāo)點總數(shù)，維度是15*n+6+m。特征點逆深度為了滿足高斯系統(tǒng)。

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

第三個式子是相機外參。

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

2、目標(biāo)函數(shù)為：

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

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

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

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

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

二、視覺約束（細(xì)致）

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

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

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

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

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

逆深度參數(shù)化

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

論文中：

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

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

在第i幀第一次觀測到第l個路標(biāo)點，在第j幀中對該路標(biāo)點進(jìn)行觀測的殘差為：

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

2、優(yōu)化變量

包括兩個時刻的狀態(tài)量、外參，以及逆深度

值得注意的是：和純視覺不同的是，待優(yōu)化的狀態(tài)量中的旋轉(zhuǎn)平移都是IMU系到w系，而不是Camera到w系。

狀態(tài)量特征：

1、待優(yōu)化量和IMU約束中待優(yōu)化量有重疊，再次體現(xiàn)了緊耦合。

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

3、Jacobian

求解Jacobian就是用視覺殘差對上述7個狀態(tài)變量進(jìn)行求導(dǎo)。

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

2）

方法二：深藍(lán)學(xué)院詳細(xì)推導(dǎo)

?

?

4、協(xié)方差

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約束（粗略，祥細(xì)見之前）

1、殘差：

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

2、優(yōu)化變量：

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

之前對IMU預(yù)積分進(jìn)行了詳細(xì)講解。參考：VINS-Mono 代碼詳細(xì)解讀——IMU離散中值預(yù)積分

3、IMU測量殘差公式推導(dǎo)

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

原PVQ積分公式：

基于預(yù)積分的PVQ積分公式

預(yù)積分量為：

最新的PVQ積分公式變?yōu)?#xff1a;

IMU測量殘差e為：

4、殘差對狀態(tài)量的Jacobian

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

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

整個殘差對某個變量求導(dǎo)；

例如：?指的是

5、殘差對狀態(tài)量的協(xié)方差

四、基于舒爾補的邊緣化

為什么進(jìn)行邊緣化操作？

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

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

1、論文部分

為了防止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)鍵幀的所有測量值。

2、基本公式

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

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

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

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

3、舒爾補

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

5 具體例子

?

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

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

矩陣乘法公式寫成連加：

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

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

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

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

?

?

?

參考文章：

VINS-Mono理論學(xué)習(xí)——后端非線性優(yōu)化? ?by Manii

VINS-mono詳細(xì)解讀 by 極品巧克力

VINS-Mono之后端非線性優(yōu)化 (目標(biāo)函數(shù)中視覺殘差和IMU殘差，及其對狀態(tài)量的雅克比矩陣、協(xié)方差遞推方程的推導(dǎo))? by Hansry

深藍(lán)學(xué)院手寫VIO課程

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

?

?

總結(jié)

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

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