VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化
本講是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)容,希望文章能夠幫你解決所遇到的問題。
- 上一篇: 计算机d盘d桌面不见了,打开电脑,D、E
- 下一篇: 大学计算机应用基础考试题库,大学计算机应