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分解）小于设定阈值时，收敛，初始化完成。因为要得到协方差矩阵（对应整个状态向量，而不是每一块偏差项），须要对信息矩阵求逆，计算量较大，所以会放在新的线程中去完成（这里彷佛有能够优化的空间）。