VINS（四）初始化与相机IMU外参标定

和单⽬纯视觉的初始化只需要获取R，t和feature的深度不同，VIO的初始化话通常需要标定出所有的关键参数，包括速度，重⼒⽅

b

向，feature深度，以及相机IMU外参Rbc和pc。

⼀. 外参旋转矩阵初始化

在Feature Detection and Tracking模块中，利⽤Harris特征点匹配通过基础矩阵和Ransac恢复出RCkCk+1；相应的使⽤IMU陀螺仪数据积分得到Rbkbk+1；这两个测量满⾜：

bbCkRbkbk+1⋅Rc=Rc⋅RCk+1

将测量得到的R⽤四元数表⽰，并利⽤四元数的运算（左乘右乘），可以得到⼀个约束⽅程：

bQkk+1⋅qc=0

利⽤多组数据可以获得⼀个超定⽅程:QN⋅qbc=0

通过求解最⼩⼆乘解，得到qbc。注意其中QN是包含使⽤Huber核的权重的，权重是通过残差矩阵计算得到，权重通过计算IMU项和视觉findFundamentalMat计算得到的旋转矩阵之间的差别得到，差别越⼤的Qkk+1，在QN中权重越⼩。

旋转初始化成功判定依据：

如果IMU三轴充分旋转，那么4*4矩阵QN的null space零空间的秩为1，可以得到稳定解；

如果旋转退化，则QN零空间的秩⼤于1，使⽤SVD分解，判断倒数第⼆⼩（第三⼤）的奇异值是否⼤于阈值。因此，旋转标定初始化的关键在有充分的三轴旋转。

⼆. 速度，重⼒⽅向，feature深度，以及外参平移矩阵初始化

bkb0可以将Rbc，Rbk+1，Rbk+1当做已知固定值，使⽤最⼤似然估计（Maximum Likelihood Estimation, MLE）和Mahalanobis范数（可以理解为

带权重的⼆范数（椭球向量范数））构建cost function，构建⽆偏的状态估计问题。在Sliding Window（例如选取5帧）中对测量值构建最⼩⼆乘问题：

VINS中feature没有通过描述⼦匹配跟踪，也不仅像ORB-SLAM中选取观测次数多（跟踪效果好）的特征点，另外加⼊了视⾓⾜够⼤这个条件。虽然最后选取出的特征点数量⽐较少（150个左右），但是都是可观性很好的特征点。（⼀）IMU测量模型

ˆ与状态估计值之间的差值取Mahalanobis范数，其构建⽬标函数需要对测量模型建模，IMU的测量模型和预积分⼀样，预积分得到的测量值z

中权重为协⽅差矩阵的逆（信息矩阵）。参考预积分公式：

得到IMU测量的误差⽅程：

其中

为了求解⾼斯分布的最⼤似然，需要知道测量的协⽅差及其对应的状态转移⽅程（⽤于协⽅差的更新）

通过IMU的运动学⽅程微分，可以获得状态转移⽅程

假设初始IMU协⽅差Pbkbk为0，则协⽅差传播⽅程为

协⽅差矩阵的传播和卡尔曼滤波的预测过程中协⽅差的传播是⼀样的，从系统的微分运动⽅程获取的。VINS中使⽤的是first-order discrete-time propagation，或者称为forward Euler method，其中加速度计的噪声Qt需要预先知道或者使⽤imu_tk标定。（⼆）视觉测量模型

重投影误差在VIO中，通常需要经过IMU坐标系的变换；这样有两个好处：

将IMU的参数（外参）整合在视觉的residual中⼀起参与优化；

IMU的坐标系在预积分中是已知的，不需要再去计算；因此将i帧获取的2D feature重投影到j帧上满⾜观测⽅程：

其中的坐标变换如下图所⽰：

初始化收敛判定：

cost function的协⽅差矩阵中外参平移向量pbc对应的协⽅差值（SVD分解）⼩于设定阈值时，收敛，初始化完成。由于要获得协⽅差矩阵（对应整个状态向量，⽽不是每⼀块误差项），需要对信息矩阵求逆，计算量较⼤，因此会放在新的线程中去完成（这⾥似乎有可以优化的空间）。　　

Processing math: 100%