imu相机标定_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，可以得到稳定解；

如果旋转退化，