一文详解VINS-Fusion初始化标定

作者丨卢涛@知乎

来源丨https://zhuanlan.zhihu.com/p/358232587

编辑丨3D视觉工坊

本文基于VINS-Fusion解释VIO系统的初始化，包括在线标定IMU-Camera的外参旋转，IMU角速度偏置，重力方向，单目尺度。单目初始化相比于双目，多一个构建SFM问题优化位姿、3D点的过程。如有错误，请您指正。

一、IMU与Camera外参旋转标定

只估计旋转  ，没有估计平移  ，平移通常可以指定，并且平移量很小，影响不如旋转大。原理就是常说的手眼标定  ，只不过这里只取旋转，丢掉了平移

推导很简单，用下面几个式子变换一下就能得到上面这个式子

对上面的式子做下变化，转换成  的形式

其中  是四元数的左乘、右乘矩阵，四元数向量相乘可以转换为一个矩阵与一个向量乘。得到上面这个形式就好办了，  用两帧图像间的IMU积分计算得到， 为两帧图像之间的位姿旋转，可以通过2d-2d计算H、E，恢复R。用多组数据通过SVD或者迭代最小二乘，就能把  算出来了，解  是  的SVD分解最小奇异值对应的右奇异向量。

在VINS中对每个  项还乘上了一个权重

 是camera旋转与IMU旋转对应的角度差，理论上角度差应该为0。VINS中角度差阈值  设为5°，如果旋转角度误差大于5°，该项会乘上一个系数  ，以降低该项的权重。相关函数如下，细节可以查看代码注释

https://github.com/smilefacehh/VINS-Fusion-DetailedNote

// initial_ex_rotation.cpp

/**
 * 在线标定外参旋转
 * 利用两帧之间的Camera旋转和IMU积分旋转，构建最小二乘问题，SVD求解外参旋转
 * 1、Camera系，两帧匹配点计算本质矩阵E，分解得到四个解，根据三角化成功点比例确定最终正确解R、t，得到两帧之间的旋转R
 * 2、IMU系，积分计算两帧之间的旋转
 * 3、根据旋转构建最小二乘问题，SVD求解外参旋转
 * @param corres            前一帧与当前帧匹配点
 * @param delta_q_imu       前一帧与当前帧IMU预积分得到的旋转
 * @param calib_ric_result  在线标定IMU与Camera之间的外参（旋转）
*/
bool InitialEXRotation::CalibrationExRotation(vector