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