VINS-MONO阅读记录

VINS是VIO研究中一个相当经典的架构，现在来读一读这篇论文
有一些在阅读过程中自己提出的问题，会慢慢解决掉

Abstract

VINS系统由相机和低成本的IMU组成，最大的挑战在于没有直接的距离测量手段，这直接导致了初始化、外部校准、非线性优化上的问题。VINS则是一个鲁棒的，多种用途的系统。故事从估计器的初始化和失败恢复讲起。使用紧耦合的，基于非线性优化的方法来获得高准确度的视觉惯性里程计。回环检测模块和我们的紧耦合方式相结合，可以降低计算代价，还额外加入了四个姿态图优化来增强全局一致性

Introduction

单目相机的视觉系统无法获得尺度信息，所以性能受限，加入IMU之后系统还不容易受光照变化的影响，但是需要知道加速度信息才能获得尺度信息，也就是VINS不能从静止状态开始工作，必须有一段运动；视觉-惯性系统是高度非线性的，估计器的初始化会有问题，外部校正会有问题，最后长期运行的系统中的漂移问题还要解决。

在VINS系统中单目的VIO模块提供了准确的局部姿态、速度、方向估计、相机-IMU外部校正和IMU biases的校正，是online风格的。回环检测是用DBoW2。通过与单眼VIO进行特征级融合，可以在紧密耦合的环境中完成重新定位。最后，将经过几何验证的循环添加到姿势图中。

related works

最简单的VIO系统是松耦合的，也就是IMU作为独立模块处理，这样的融合经常通过EKF（卡尔曼滤波）实现，IMU用作状态传播，纯视觉得到的位姿用来更新。随后的紧耦合系统中要么基于EKF要么基于图优化。为了获得恒常的运行时间，采用一个窗口的方法，将过往的状态边缘化。然而图优化的迭代求解很复杂，很难达到实时

对于视觉测量，算法要么是直接法要么是间接法，区别在于视觉残差模型的定义，直接法最小化几何误差，间接法最小化几何位移。直接法由于感知范围小，需要很好的初始化，间接法在提取和匹配特征点中更消耗计算资源，然而更加鲁棒。但是直接法在稠密建图中更容易使用，因为它们是在像素级别操作的，EKF中的IMU是用作状态传播的，而图优化中的IMU是用预积分方式的，预积分可以防止重复做IMU的积分

精确的初始值对单目VINS系统是相当重要的，之前有很多初始化方法，它们都有各自的缺点。另外闭环检测也很重要，像orb这样的纯视觉要估算7个自由度，而我们由于加入了IMU可以只估算四个自由度（三维变换、绕重力方向的旋转角度），所以位姿图优化使用四自由度的松限制做的

整体架构

系统从测量预处理开始，随后是初始化，提供了各种东西，这也是我们要尽量做到精准的一些参数，随后做VIO和重定位的共同优化，最后对漂移进行校正

一些表示约定：表示旋转的时候大多数在用四元数，偶尔会为了方便而选用旋转矩阵；world frame，imu frame，camera frame都是啥？——see p4的上面

测量预处理

对于视觉测量，跟踪连续帧之间的特征，并且在最近一帧中探测新的特征。对于IMU测量，将两帧之间的IMU测量进行预积分，但是要注意预积分过程中的bias

视觉处理前端

对于每一张新图像，现有的特征通过KLT光流来追踪，然后在新图像中提取不少于100个的角点。探测器通过设置两个相邻特征之间的最小距离来保证特征的均匀分布。得到的2D特征首先经过畸变校正，然后用RANSAC去掉outlier后投影到单位球面上

这一步也进行了关键帧的选取，关键帧选取有两个准则，
第一个是和前一个关键帧的平均视差，如果视差足够大的话，把它当成新的关键帧。我们知道，平移和旋转同样可以导致视差的出现，然而在纯旋转的情况下是无法对特征进行三角化的，对于这种情况我们使用陀螺仪的短期积分来弥补，然而我们知道，IMU的噪声是很大的，所以我们只是在关键帧选择的时候采用IMU短积分来代替，这样即使IMU积分是有问题的也只是影响关键帧的选择，而不会影响最终的结果。
第二个是跟踪的质量，如果可追踪的特征点低于某一个阈值，我们就把现在的最新一帧作为新的关键帧，为了避免tracking丢失。

IMU预积分

我们的IMU预积分是包含了bias的校正（bias是啥：IMU的加速度和角速度的测量都会有的误差）
我们的加速度和角速度的公式是

这里的n都是噪声项，是高斯分布的，b都是bias项，这些bias项的导数是高斯分布的，随后给出了position是啥的位姿啊，velocity，orientation states是啥啊的递推公式，里面的b_k和b_{k+1}分别是在t_k和t_k+1时刻的图像帧

可以看出，IMU状态传播需要帧bk的旋转，位置和速度。 当这些起始状态改变时，我们需要重新传播IMU测量值。 特别是在基于优化的算法中，每次我们调整姿势时，我们都需要在它们之间重新传播IMU测量值。 这种传播策略对计算要求很高。 为了避免重新传播，我们采用预整合算法
我们提取两次计算之中会变的东西，如下：

离散时间（啥叫discrete time）的完成中，可以采用不同的数值方法，比如欧拉，中点，RK4，我们在讲述论文的时候用欧拉，为了方便，实际实现的时候用的是中点
随后给了α，β，γ的递推估计公式

数学推导数学推导数学推导数学推导数学推导……………………数学推导数学推导数学推导

估计器初始化

单目紧耦合VIO是一个高度非线性的系统，如果没有好的初始值，是很难直接把二者的观测结合到一起的。一个方法是假设VINS启动时是处于稳定的状态，然而这是不切合实际的，因为我们经常要在运动的情况下进行初始化，所以对于单目VINS来说，初始化是极其重要的。

文章采取松耦合的方法来获取初始值，因为我们看到纯视觉SLAM和SfM的方法都可以拿到很好的初始值，通过八点或者五点算法，或者通过估计齐次矩阵中的初始值来进行自我引导。我们把IMU预积分的结果和纯视觉SfM的结果相对齐，就可以大致恢复scale，gravity，velocity甚至bias，这是够用的。

我们并不是在初始化阶段同时估计旋转和平移加速度bias，而是在初始化阶段只去关心旋转bias，因为加速度计的偏置与重力相关，并且由于重力项很大，可能会掩盖平移加速度，并且初始化阶段的时间相对较短，因此很难观察到这些偏置项。

基于滑动窗口的纯视觉SfM

如上所述，初始化过程是由纯视觉SfM开始的，做的是估计一个有着相机位姿和特征位置的图。为了减少计算开支，用滑动窗口的方式实现。具体来说，查看最新一帧和所有之前的帧的特征对应性。如果找到了稳定的特征对应（30个对应点）以及有效的视差（超过20个由旋转来补偿的像素),就用五点法来恢复两帧之间的旋转和平移，否则把最新的这一帧留在窗口中，等待下一帧。如果五点法成功，任意设置比例尺，对在这两帧中观察到的所有特征进行三角测量。基于这些三角特征，用PnP来估计窗口中所有其他帧的位姿，最后用全局BA来让所有特征观测的总重投影误差最小。因为我们到现在为止依然不知道世界帧的情况，所以我们把相机的第一帧设为SfM的参考帧，所有的其他帧的位姿和特征点为止都是以它为参考而定的。
orientation state是指啥啊 每一帧的的位姿都可以用一对参数（p,q）来表示，它代表了相机和IMU之间的外部参数（是啥啊），有这么一个公式，s就是那个尺度参数，求解这个s就是成功初始化的重点

视觉和惯性测量之间的对齐

1） 陀螺仪bias校正：
2）速度、重力向量和尺度的初始化
在陀螺仪bias初始化后，我们继续来初始化其他的对于导航至关重要的状态，包括速度、重力和尺度，vb0,vb1…代表第0，1…张图片，把它们放在一个长向量中

3）重力的优化
大多数时候重力的大小是知道的，所以重力向量的自由度只剩下两个，于是在切线空间中重新将重力参数化，变成

b1,b2是切线空间的正交基，可以迭代找到
4）完成初始化
优化重力后，我们将重力方向旋转到z轴，就可以得到从世界帧到相机帧之间的旋转q，随后我们就可以把参考帧的所有变量旋转到世界帧上去。物体帧的速度也要被旋转到世界帧上去，视觉SfM的平移分量会被尺度统一化，此时初始化完成，所有的参数被送入VIO

紧耦合的单目VIO

初始化之后，用一个基于滑窗的紧耦合的单目VIO来获得准确的鲁棒的状态估计

概述

滑窗中的状态向量被定义如下

xk代表在第k个图片被拍到的时候的IMU在世界帧中的状态，包括位置，速度和方向，以及加速度的bias和陀螺仪的bias，n是关键帧的综述，m是滑窗中的总特征数，λl是第l个特征在第一次观测时的深度的逆。
采用一个视觉-惯性联合的BA，里面包含残差项

IMU测量残差（残差=实际观察值和预测值之间的差）

在优化IMU残差时候为了简化运算，把两个bias也放进去一起优化了

视觉残差

我们测量的相机残差是定义在一个单位球上，而不是像传统的针孔摄像机一样在平面上定义

边缘化

如果第二个最新的帧是一个关键帧，我们将保持它在窗口中，并边缘化最老的帧及其相应的视觉和惯性测量值。边缘化的测量值被转换为先验值。如果第二个最新帧不是关键帧，我们将简单地移除该帧及其所有相应的视觉测量值。然而，对于非关键帧，保留预积分惯性测量值，并且在下一帧继续进行预积分过程。
边缘化是用了一个叫Schur的实现方式

用于相机速率状态估计的仅运动的视觉-惯性BA

由于我们的设定是在计算力有限的设备上运行VINS，所以完全体的BA肯定是不行的。我们采用了一个轻量级的只用运动的运动-惯性BA算法来让状态估计达到照相机的帧率（30Hz）。
我们用的方程和普通单目VIO是一样，的只不过我们在滑窗中不是优化所有的状态，而是只优化固定数量的最新的IMU状态的位姿和速度。其他的我们都当成常量，如图

在IMU-帧率状态估计中的IMU前向传播

IMU测量的帧率非常高，虽然我们的VIO的性能受制于图像更新速率，但是我们依然可以用最新的IMU值来进行对VIO的预测，可以被用作为闭环检测提供协助

失败检测和恢复

虽然我们的系统是鲁棒的，但是也绝不是没有任何问题的，主动的失败检测和恢复可以提升系统稳定性。我们的失败检测是一个独立的模块，检测从估计器中得到的不同寻常的输出，包括以下警戒情况：
在最近一帧中探测到的正在被追踪的特征数小于某一阈值；
最后两个估计器输出之间的位置或旋转有很大的不连续性；
偏差或外部参数估计的大变化
只要探测到失败，就重新初始化

重定位

我们的滑动窗和边缘化方案限制了计算复杂度，但也给系统带来了累积漂移。更具体地说，漂移发生在全局3D位置（x，y，z）和围绕重力方向的旋转。
为了消除漂移，提出了一种与单目VIO无缝集成的紧密耦合重定位模块。重定位过程从一个循环检测模块开始，该模块识别已经访问过的地方。然后在循环闭合候选帧和当前帧之间建立特征级连接。这些特征对应紧密地集成到单目VIO模块中，以最小的计算开销实现无漂移状态估计。多个特征的多个观测值直接用于重定位，从而获得更高的精度和更好的状态估计平滑度。重新定位程序的图解如图(a)所示。

回环检测

优化了一下DBoW2来做回环检测，除了为单目VIO提供的角点以外，用BRIEF描述法增加了500个角点，增加的角点可以优化表现
我们的单目VIO能够使横摇和俯仰角变得可观察。因此，我们不需要依赖于旋转不变的特性，例如在ORB SLAM中使用的ORB特性

特征检索

当检测到循环时，通过检索特征对应关系来建立局部滑动窗口和循环闭合候选者之间的联系。通过BRIEF描述符匹配找到对应关系。直接匹配描述符可能会导致大量的离群值。为此，我们使用两步几何野值剔除
2D-3D，3D-2D(PnP)
当匹配的数量超过一个阈值的时候，我们认为这确实是一个回环，进行重定位

紧耦合重定位

重定位过程把当前滑窗和过去位姿组成的图对齐，在重定位过程中，我们把所有的闭环帧的位姿认为是常数，联合优化滑动窗口，使用IMU测量，本地视觉观测，和从闭环中抽取到的特征。就可以写出优化方程

全局图优化

重定位后，局部滑动窗口会和过去的位姿对齐，而我们增加的这步全局图优化是要保证这些位姿在被注册的时候是全局一致的
因为在视觉-惯性系统同中所有的roll和pitch都是可观测的，累积漂移只可能出现在四个自由度上（x,y,z和yaw角）因此，我们只需要做四自由度的优化

向位姿图中加入关键帧

当一个关键帧被边缘化后，它会被加入到位姿图中，这个关键帧在位姿图中作为一个体素存在，和其他体素之间的联系方式有两种：
1）序列边：
一个关键帧将建立与之前关键帧的多个连续边。序列边表示局部滑动窗口中两个关键帧之间的相对变换，该值直接取自VIO。考虑到新边缘化的关键帧i及其之前关键帧中的一个关键帧j，序列边仅包含相对位置和偏航角。
2）闭环边：
如果新边缘化的关键帧具有闭环连接，那就会被用闭环边和闭环帧连接在一起。我们的闭环边也只有4自由度，闭环边的值通过重定位结果得到

四自由度的位姿图优化

定义了两帧之间连边的残差，优化一个方程即可
位姿图优化和重定位在两个独立的线程中异步运行。这使得当最优化的位姿图变得可用时，立即使用它进行重新定位。同样，即使当前的位姿图优化尚未完成，仍然可以使用现有的姿势图进行重定位。该过程如图中的（b)所示。

位姿图管理

随着移动距离的增加，位姿图的尺寸会无限增大，从而限制了系统的实时性。为此，我们实现了一个降采样过程来将姿势图数据库保持在有限的大小。所有具有循环闭合约束的关键帧将被保留，而其他过于接近或与相邻关键帧方向非常相似的关键帧可能会被删除。关键帧被移除的概率与其相邻帧的空间密度成正比。

总结

在这篇论文中，我们提出一个强健且多功能的单目视觉惯性估计器。我们的方法具有先进的和新颖的解决方案的IMU预积分，估计器初始化和故障恢复，在线外部校准，紧密耦合视觉惯性里程计，再定位和有效的全局优化。通过与最先进的开源实现和高度优化的行业解决方案进行比较，我们展示了优越的性能。我们同时开放PC和iOS实现源代码，以造福社区。

虽然基于特征的VINS估计已经发展到了实际应用的成熟阶段，但是我们仍然看到了许多未来研究的方向。根据运动和环境的不同，单目VIN可能达到弱可见甚至退化的状态。我们最感兴趣的是在线方法来评估单目VIN的可观测性，以及在线生成运动计划来恢复可观测性。

另一个研究方向是将单目VIN大规模部署在各种消费设备上，如移动电话。这种应用需要在线校准几乎所有传感器的内部和外部参数，以及在线识别校准质量。

最后，我们感兴趣的是根据单目VINS得到的结果生成稠密的地图，为了进一步提高系统的精度和鲁棒性，还需要进行广泛的研究。