VINS-Mono论文阅读笔记

1. 测量预处理

1.1 视觉处理

涉及两个方面：特征提取与跟踪、关键帧选择

• 特征提取与跟踪：FAST角点+KLT稀疏光流+RANSAC过滤异常值
• 关键帧选择：1）视差变化超过一定阈值；2）跟踪的特征小于一定数量
  注意：由于纯旋转无法进行三角化，因此旋转的阈值通过短期IMU预积分获得

1.2 IMU预积分

参考这篇：

C. Forster, L. Carlone, F. Dellaert and D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual–Inertial Odometry,” in IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1-21, Feb. 2017, doi: 10.1109/TRO.2016.2597321.

2. 初始化

松散地将IMU预积分与纯视觉SFM对齐来获得必要的初始值

2.1 视觉SFM

如果我们能找到最新帧滑动窗口中任何其他帧稳定的特征跟踪（超过30个跟踪特征）和足够的视差（超过20像素），就使用五点法(八对点)恢复这两帧之间的相对旋转和尺度平移。

然后，任意设置尺度，对这两帧中观察到的所有特征进行三角化。 基于这些三角化特征，采用PnP(3D-2D，可以是DLT、P3P、BA三种方法求解)来估计窗口中所有其他帧的姿态。 最后，应用全局BA最小化所有特征观测值的总重投影误差。

最后，将求解出的每一帧到初始帧的姿态变换到IMU坐标系，以此来恢复尺度s。

2.2 视觉-惯性对齐

1）IMU陀螺仪偏置初始化

由于视觉求解得滑动窗口中的旋转没有尺度缺失的问题，因此使用视觉SFM求解的旋转部分对陀螺仪的旋转偏置进行求解。

求解优化后的陀螺仪偏置后，使用这个新的偏置对滑动窗口内的IMU预积分进行重新计算。

2）速度、重力、尺度初始化

构造状态变量：

在相邻两帧k和k+1之间可以得到以下方程：

把论文中的(6)代入，就可以得到这个公式：

里面的z可以通过视觉SFM或IMU预积分得到，右边的X为待求项，这样就构造了一个线性方程组，可以通过优化的方式求解，所以这是线性初始化。

3）重力细化

在其切空间上用两个变量扰动重力，从而保持2自由度：

把这个扰动后的重力代入方程(9)，优化求解$\delta g$，以此获得更精确的重力矢量。

4）完成初始化

将重力方向旋转至世界坐标系c0的z轴，并将速度估计也旋转至z轴，得到之间的相对变换，完成初始化过程。

根据上述初始化过程可以发现，如果想要VIO表现比较好，初始化时需要给相机一定的平移，以及对IMU足够的激励。

3. 紧耦合单目VIO

在滑动窗口中进行进行状态估计

3.1 公式定义

待优化状态如下：

包括滑动窗口中每个关键帧的状态$x_k$、相机到IMU的外参$x_c^b$、滑动窗口中的特征${\lambda }_l$。

然后将上述的外参求解的残差(10)、IMU预积分残差、视觉重投影误差结合在一起，构建优化函数：

使用Huber鲁棒核函数+BA进行优化

3.2 IMU预积分残差

3.3 视觉测量残差

与传统针孔相机模型在广义图像平面上定义重投影误差不同，本文将相机测量残差定义在单位球面上。好处就是，几乎所有类型的相机的光学元件，包括广角、鱼眼或全向相机，都可以被建模为连接单位球体表面的单位射线。

这个过程是将2D特征$c_i$反向投影，然后转到IMU坐标系，然后转到世界坐标系，然后转到第j帧的IMU坐标系，然后转到第j帧的相机坐标系，然后进行归一化，转到单位球面上，然后与同样反向投影转到单位球面坐标系的特征$c_j$作差，构建残差模型。

然后在单位球上自由度为2，因此将残差向量投影到切平面上，b1和b2是任意选择的两个正交基。

3.4 边缘化

当新的关键帧加入滑动窗口后，最老的关键帧被边缘化，状态直接被边缘化掉，对于其中的测量部分，仅保留空间分离的测量，也就是只丢弃没有共同观测的特征部分。

被边缘化的部分被固定为先验。

3.5 相机帧率的纯运动视觉惯性优化

对于计算能力较低的设备，如手机，紧耦合的单目VIO由于非线性优化的计算量大，无法实现相机速率的输出。为此，除了全面优化之外，我们还采用轻量级的仅运动视觉惯性优化来提高相机速率（30 Hz）的状态估计。

不优化滑动窗口中的所有状态，而是只优化固定数量的最新IMU状态的姿态和速度。我们将特征深度、外在参数、偏差和我们不想优化的旧IMU状态视为常数值。

3.6 IMU频率的前向传播

尽管我们的VIO频率受到图像捕获频率的限制，但我们仍然可以直接将最新的VIO估计与最近的IMU测量一起传播，以实现IMU速率性能，相当于做了个状态融合。

4. 重定位

主要是用于减小累计误差

4.1 回环检测

用DBoW2做回环检测，同时还保留所有的BRIEF描述符用于特征检索，但丢弃原始图像以减少内存消耗。

4.2 特征检索

当检测到环路时，通过检索特征对应关系来建立局部滑动窗口与闭环候选窗口之间的连接。对应是通过BRIEF描述符匹配找到的。描述符匹配可能会导致一些错误的匹配对。 为此，采用了两步几何离群值剔除。

• 2D-2D: RANSAC离群值去除
• 3D-2D：滑动窗口中的3D特征与候选帧中的2D特征进行PnP匹配

4.3 紧耦合重定位

如果发生重定位，就对以下目标函数进行优化，相比于之前的优化函数，只是增加了重定位约束项：

5. 全局位姿图优化和地图复用

由于可以估计到绝对的重力方向，相当于已经可以获得估计的绝对roll和pitch角，因此只进行4DOF(x,y,z,yaw)的位姿图优化即可。

每个关键帧构建为一个节点，节点之间存在两种类型的边：

• 顺序边：来自于VIO测量
• 回环边：来自于重定位模块

最后的优化函数如下：

另外，如果之前存在已经建好的位姿图和地图，在先验地图中检索到回环后可以在当前位姿图和先验位姿图之间建立约束，将它们合并在一起。

位姿图中每个节点变量的定义如下：

在加载了位姿图后，立刻执行一次全局位姿图优化，构建与先验地图的连接和约束。

6. 实验

6.1 EuRoC数据集与OKVIS比较

总体来说不带回环和OKVIS差不多，带回环精度比较优秀。

6.2 EuRoC数据集重定位与地图合并

可以实现与先验地图比较精确的合并。

6.3 实机实验

在两个实验：室内手持场景和室外大尺度场景。

两个实验中证明了VINS-Mono的精度和鲁棒性。

6.4 应用

应用一：无人机定位，下游任务是避障
应用二：手机应用，实时性好