VINS_Mono中camera-imu的旋转外参标定

重磅干货，第一时间送达

众所周知，VINS_Mono是一种融合了视觉和惯导这两种传感器的viSLAM系统，而视觉和惯导的融合可以带来1+1>2的效果，其优点如下：

可以借助IMU较高的采样频率，进而提高系统输出频率

可以提高视觉鲁棒性

可以有效消除IMU预积分的漂移

能够矫正IMU的Bias

可以解决视觉单目尺度不可观的问题

但是，为了使这个两个传感器融合，我们首先需要做的事情是将两个传感器的数据对齐，除了时间上的对齐，还有空间上的对齐。空间上的对齐通俗的讲就是将一个传感器获取的数据统一到另一个传感器的坐标系中，其关键在于确定这两个传感器之前的外参，本文将详细介绍VINS_Mono中camera-imu的旋转外参标定算法原理并对其代码进行解读。

相机与 IMU 之间的相对旋转

上图表示相机和IMU集成的系统从

到

的运动，其中视觉可以通过特征匹配求得

到

时刻的旋转增量

，同时IMU也可以通过积分得到到

时刻的旋转增量

，设相机和IMU之间的旋转矩阵为

，那么对任意时刻

，满足：

将上式用四元数表示：

其中：

对于

对相对旋转的测量值，可得过约束的线性方程：

在标定成功前，

会一直增长，为了更好的处理可能存在的异常值，每个

都乘以了一个类似于huber核函数的权重

。对

进行svd分解，其中最小奇异值对应的右奇异向量便为结果

。

怎么确定标定成功的终止条件?

对于等式

，由于camera-imu的外参是一个固定的值，即

只有一个值，也就是说

的零空间非零解存在一组。在足够多的旋转运动中，我们可以很好的估计出相对旋转

，这时

对应一个准确解，且其零空间的秩为1。但是在标定过程中，某些轴向上可能存在退化运动(如匀速运动)，这时

的零空间的秩会大于1。判断条件就是

的第二小的奇异值是否大于某个阈值，若大于则其零空间的秩为1，标定成功；反之秩大于1，相对旋转

的精度不够，标定不成功。阈值为什么选取的是0.25，在文章《Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration》中对随着奇异值的变化求得外参对这个系统的影响做了分析，如下图。

从图中可以看到，随着

的增长，当

接近0.1的时候，相机和IMU之间的相对旋转

逐渐趋于一个稳定的值，此时求得的特征点的位置的精度已经达到5cm以内了。

对应代码及注释