VINS-mono是当前常用的VIO框架之一,出自港科大的沈劭劼老师团队。VINS-mono是基于优化的紧耦合方案,精度、速度、鲁棒性俱佳。
1、整体框架
从上图可得,VINS-mono可以分为4个部分,跟踪、初始化、滑窗优化、回环优化。
2、跟踪
2.1、视觉跟踪
这里采用的是光流跟踪的前端,一般过程是通过特征点匹配获得本质矩阵或者基础矩阵,然后恢复出R,t;之后再将特征点三角化,后面就可以通过pnp求解位姿。
对于关键帧的选择有以下两个准则:
1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧(因为视差可以根据平移和旋转共同得到,而纯旋转则导致不能三角化成功,所以这一步需要IMU预积分进行补偿)
2、当前帧跟踪到的特征点数量小于阈值视为关键帧,这个标准是为了避免完全丧失特征的情况
2.2、IMU预积分
根据IMU中陀螺仪和加速度计的测量方程:
可以得到k+1时刻,IMU位置、速度、姿态的积分公式:
上面的积分公式中,
R
t
w
R_t^w
Rtw为t时刻的世界坐标系姿态。作为待优化变量,在每次更新后就意味着重新积分。为了减少计算量,预积分的结果被转换到相对位姿,即:
相应的积分项为:
离散化形式:
3、初始化
这里需要标定陀螺仪的偏差,重力加速度,相机与IMU之间的相对位姿。
3.1、SfM
首先需要利用SfM获得一段较精确的图像估计位姿。
3.2、标定陀螺仪的偏移
由于旋转不受尺度影响,可以根据下面的约束获得陀螺仪的偏移
δ
b
w
\delta b_w
δbw
在求得偏移后,需要重新计算IMU预积分。
具体过程可参考:https://xhy3054.github.io/vins-gyro-bias/
3.3、标定速度、重力加速度和尺度
待标定的所有变量:
根据位置和速度的预积分公式
转化为:
求解下式即可
3.4、重力修正
一般情况下,重力的模值是固定的,所以重力向量只有两个自由度。可以据此对上面的变量进一步优化。
具体过程参考:https://xhy3054.github.io/vins-scale-gravity-velo/
4、滑窗优化
待优化变量为:
残差项:
这里的残差项包括边缘化误差、IMU测量误差、视觉测量误差。
- IMU测量误差:
- 图像测量误差:
- 边缘化误差:
原文中未给出公式
5、回环优化
目标函数:
位姿图中的优化变量仅有如下的4个自由度,因为横滚角和俯仰角对于IMU是可观的。