VINS（四）初始化與相機IMU外參標定

 和單目純視覺的初始化只須要獲取R，t和feature的深度不一樣，VIO的初始化話一般須要標定出全部的關鍵參數，包括速度，重力方向，feature深度，以及相機IMU外參$R_{c}^{b}$和$p_{c}^{b}$。

一. 外參旋轉矩陣初始化

在Feature Detection and Tracking模塊中，利用Harris特徵點匹配經過基礎矩陣和Ransac恢復出$R_{Ck+1}^{Ck}$；

相應的使用IMU陀螺儀數據積分獲得$R_{bk+1}^{bk}$；

這兩個測量知足：

$R_{bk+1}^{bk}\cdot R_{c}^{b} = R_{c}^{b}\cdot R_{Ck+1}^{Ck}$

將測量獲得的$R$用四元數表示，並利用四元數的運算（左乘右乘），能夠獲得一個約束方程：

$Q_{k+1}^{k}\cdot q_{c}^{b} = 0$

利用多組數據能夠得到一個超定方程:

$Q_{N}\cdot q_{c}^{b} = 0$

經過求解最小二乘解，獲得$q_{c}^{b}$。注意其中$Q_{N}$是包含使用Huber核的權重的，權重是經過殘差矩陣計算獲得，權重經過計算IMU項和視覺findFundamentalMat計算獲得的旋轉矩陣之間的差異獲得，差異越大的$Q_{k+1}^{k}$，在$Q_{N}$中權重越小。

 

旋轉初始化成功斷定依據：

若是IMU三軸充分旋轉，那麼4*4矩陣$Q_{N}$的null space零空間的秩爲1，能夠獲得穩定解；

若是旋轉退化，則$Q_{N}$零空間的秩大於1，使用SVD分解，判斷倒數第二小（第三大）的奇異值是否大於閾值。

所以，旋轉標定初始化的關鍵在有充分的三軸旋轉。

 

二. 速度，重力方向，feature深度，以及外參平移矩陣初始化

能夠將$R_{c}^{b}$，$R_{bk+1}^{bk}$，$R_{bk+1}^{b0}$當作已知固定值，使用最大似然估計（Maximum Likelihood Estimation, MLE）和Mahalanobis範數（能夠理解爲帶權重的二範數（橢球向量範數））構建cost function，構建無偏的狀態估計問題。

在Sliding Window（例如選取5幀）中對測量值構建最小二乘問題：

 

VINS中feature沒有經過描述子匹配跟蹤，也不只像ORB-SLAM中選取觀測次數多（跟蹤效果好）的特徵點，另外加入了視角足夠大這個條件。雖然最後選取出的特徵點數量比較少（150個左右），可是都是可觀性很好的特徵點。

（一）IMU測量模型

構建目標函數須要對測量模型建模，IMU的測量模型和預積分同樣，預積分獲得的測量值$\hat{z}$與狀態估計值之間的差值取Mahalanobis範數，其中權重爲協方差矩陣的逆（信息矩陣）。

參考預積分公式：

獲得IMU測量的偏差方程：

其中

 

爲了求解高斯分佈的最大似然，須要知道測量的協方差及其對應的狀態轉移方程（用於協方差的更新）

經過IMU的運動學方程微分，能夠得到狀態轉移方程

假設初始IMU協方差$P_{bk}^{bk}$爲0，則協方差傳播方程爲

 

協方差矩陣的傳播和卡爾曼濾波的預測過程當中協方差的傳播是同樣的，從系統的微分運動方程獲取的。VINS中使用的是first-order discrete-time propagation，或者稱爲forward Euler method，其中加速度計的噪聲$Q_{t}$須要預先知道或者使用imu_tk標定。

（二）視覺測量模型

重投影偏差在VIO中，一般須要通過IMU座標系的變換；這樣有兩個好處：

• 將IMU的參數（外參）整合在視覺的residual中一塊兒參與優化；
• IMU的座標系在預積分中是已知的，不須要再去計算；

所以將i幀獲取的2D feature重投影到j幀上知足觀測方程：

其中的座標變換以下圖所示：

  

初始化收斂斷定：

cost function的協方差矩陣中外參平移向量$p_{c}^{b}$對應的協方差值（SVD分解）小於設定閾值時，收斂，初始化完成。因爲要得到協方差矩陣（對應整個狀態向量，而不是每一塊偏差項），須要對信息矩陣求逆，計算量較大，所以會放在新的線程中去完成（這裏彷佛有能夠優化的空間）。