VINS-MONO论文笔记第一篇——IV预处理和V初始化

VINS-MONO

总摡：

系统从一个鲁棒的估计器初始化开始，一种紧耦合的、非线性的优化方法通过融合预积分IMU测量值和特征点观测值来得到高精度的VIO，回环检测与紧耦合系统结合，能使得重定位有最小的计算量。另外4维位姿图优化来保证全局一致性。通过高效的保存和加载地图能反复利用地图。通过全局位姿图优化能覆盖当前和旧的地图。
实验：在公开数据集和真实世界的实验上跑了系统，并与最新的方法进行了比较，在微飞行器平台进行了闭环自动飞行，并移植到iOS平台上演示

优点：观测到的尺度、roll、pitch是以米为单位的。

为了解决的问题：
1.严格的初始化：由于缺少直接的距离测量，很难直接将单目视觉结构与惯性测量融合；VINS是高度非线性的，初始化很难；在启动时，系统需要有一个已知的静止状态，移动也需要很缓慢。
2.长时间的漂移是无法避免的：对VIO来说，为了限制漂移，需要有回环检测、重定位、全局优化。
3.地图的保存和重复利用也是一个需求。

对应的方法：
（1）鲁棒的初始化程序能从未知的初始状态下启动系统
（2）紧耦合、基于优化的单目VIO有相机-IMU外参标定和IMU偏差矫正
（3）在线重定位和4自由度的全局位姿图优化
（4）重新使用的位姿图能保存、载入和覆盖多种局部的位姿图。
其中（1）（4）是之前的工作

系统框架：

Sect. IV 系统开始的地方：测量数据，检测跟踪特征点、IMU预积分
Sect. V 初始化：提供所有需要的数据，包括pose, v , gravity vector, gyroscope bias, and 3D feature location
Sect. VI： VIO & Sect. VII 重定位 ：紧融合IMU预积分测量量，特征观测值。
Sect VIII 位姿图优化模块：接受几何验证的重定位结果，执行全局优化减少漂移，还可以实现位姿图的重用。
VIO和位姿图优化同时在多线程中进行。

(·)^w 世界坐标系：重力的方向与z轴对齐
(·)^b body坐标系，也是IMU坐标系
(·)^c 相机坐标系
q_b^w p_b^w 由b系到w系的旋转和平移
b_k c_k 第k张图像时的b系、c系
⊗ 四元数的乘法运算
g^w = [0,0,g]^T 世界坐标系下的重力矢量
(ˆ·) 一个定值的噪声或估计

IV 测量值预处理

对visual测量值：跟踪连续帧之间的特征点，在最新帧检测新特征点
对IMU测量值：连续两帧之间预积分

A. 视觉处理前端

1.对每张新图像，用稀疏的光流法跟踪已有的特征点；
2.在每幅图像中保证至少100-300的特征点，否则检测新的特征点（通过设置相邻帧之间最小的像素间隔强制特征均匀分布）；
3.不失真的2D特征点在外点剔除后投影到单位球上。RANSAC进行外点剔除
4.关键帧的选取：
①平均视差：当前帧和最新关键帧之间跟踪到的特征点的平均视差大于特定阈值（为了避免在仅旋转运动不能三角化特征点的情况，使用陀螺仪测量值的短期积分来补偿进行视差计算时的旋转，这步仅在关键帧选取时使用，因此，尽管陀螺仪有误差，也只是影响关键帧的选择结果欠佳，并不会影响最后的估计质量）
②跟踪质量：跟踪到的特征点数量小于一定阈值，就选为关键帧。（这条是为了避免完全丢失特征跟踪）

B. IMU预积分

这步与[19] [24] 的计算结果一样，但是推导不同。

1）IMU噪声和偏差：

IMU测量值的影响因子：加速度偏差b_a 陀螺仪偏差b_w 以及重力、偏差的影响

假设加速度和陀螺仪的额外噪声n_a n_w是高斯白噪声，加速度和陀螺仪的偏差被建模为随机行走，其导数为高斯白噪声；

2）预积分：

给定偏差估计值，将连续两帧间时间间隔内 [t_k , t_k+1 ] 的惯性测量量在局部帧b_k中积分:

α、β、γ的协方差P_bk+1^bk也跟着更新。由（3）：将b_k看做指定偏差的参考帧，预积分项可以仅通过IMU测量量求解。

3）偏差校正：

如果偏差的估计发生了微小的变化，需要调整相应的预积分结果；
如果变化太大则需要在新的偏差估计下重新预积分。
因为不需太重复积分IMU测量值，所以这个策略可以节省基于优化方法的计算资源

V.初始化估计

通过松散对齐视觉数据和IMU预积分数据，以获得一些必要变量的初值

A 滑窗中的仅视觉SFM

在滑窗中维护多个多个框架，以实现有限的计算复杂性。
1.检查最新帧与所有先前帧的特征对应关系，若能找到稳定的特征跟踪（多于30跟踪到的特征点）和足够的视差（多于20个像素），用5点法恢复相对旋转和尺度缩放。
2.任意设置比例（？），并对上述两两帧间所有观测到的特征点进行三角化。
3.基于三角化的特征点，用PnP估计窗口中所有其它帧的位姿
4.全局BA来最小化所有特征观测的总重投影误差。

设置第一相机帧$(\cdot {)}^{c_0}$作为SFM的参考帧，给定camera和IMU间外参（$p_c^b,q_c^b$），camera到IMU系的位姿变换：

其中s是未知的尺度参数

B 视觉惯性对齐

基本思想是匹配视觉结构和IMU预积分量

1）陀螺仪偏差标定：

在滑窗中连续两帧b_k b_k+1 ,由视觉SFM得到了$q_{b_k}^{c_0}$和$q_{b_{k+1}}^{c_0}$，从IMU预积分得到了相对约束${\hat{\gamma }}_{b_{k+1}}^{b_k}$ ，相对陀螺仪偏差线性化IMU预积分项并最小化以下cost function
B表示窗口中所有帧。由此得到b_w 的初始标定值，然后用新的b_w 重新计算IMU预计分项${\hat{\alpha }}_{b_{k+1}}^{b_k}$ ${\hat{\beta }}_{b_{k+1}}^{b_k}$ ${\hat{\gamma }}_{b_{k+1}}^{b_k}$

2）速度v 重力矢量$g^{c_0}$和尺度s初始化：

$v_{b_k}^{b_k}$表示第k张图像在body系的速度，$g^{c_0}$表示$c_0$帧的重力矢量，$s$表示单目SfM尺度。

对滑窗中连续两帧：

联立（6）（9）到线性测量模型：

这些参数由SfM得到。通过解线性最小二乘问题能在滑窗里得到每帧的body系速度

3）重力加速度细化

重力矢量由前面线性初始化步骤得到，为了保留两自由度在线性空间对重力加了扰动
$g$是重力值，${\hat{g}}^{}$是表示重力方向的单位矢量，$b_1$,$b_2$是切平面的两个正交基，$w_1$ , $w_2$分别为$b_1$,$b_2$的扰动，然后，用$g(ˆg+\delta g)$将$g$代入（9），并与其他状态变量一起求解2-D$\delta g$。 此过程重复几次，直到收敛。

4） 完成初始化

在3）后通过将$g$旋转到z轴能得到世界坐标系和相机帧$c_0$间的旋转$q_{c_0}^w$，再将所有变量从${(}^{\prime }{)}^{c_0}$ 转到 ${(}^{\prime }{)}^w$，加上尺度缩放，初始化完成，变量进入单目紧耦合系统。