VINS-mono阅读笔记

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$为t时刻的世界座标系姿态。作为待优化变量，在每次更新后就意味着重新积分。为了减少计算量，预积分的结果被转换到相对位姿，即：

相应的积分项为：

离散化形式：

3、初始化

这里需要标定陀螺仪的偏差，重力加速度，相机与IMU之间的相对位姿。

3.1、SfM

首先需要利用SfM获得一段较精确的图像估计位姿。

3.2、标定陀螺仪的偏移

由于旋转不受尺度影响，可以根据下面的约束获得陀螺仪的偏移$\delta b_w$

在求得偏移后，需要重新计算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是可观的。