VINS-Mono：一种功能强大且通用的单目视觉惯性状态估计器
摘要-由摄像机和低成本惯性测量单元（IMU）组成的单目视觉惯性系统（VINS）构成了六自由度（DOF）状态估计的最小传感器套件。 然而，缺乏直接距离测量在IMU处理，估计器初始化，外部校准和非线性优化方面提出了重大挑战。 在这项工作中，我们介绍了VINSMono：一种强大而通用的单目视觉惯性状态估计器。我们的方法从一个用于估计器初始化和故障恢复的稳健的程序开始。 通过融合预积分的IMU测量和特征点观测，使用紧耦合，非线性优化的方法来获得高精度的视觉惯性测距。 环路检测模块与我们的紧耦合模块相结合，可实现重定位，并且计算开销最小。我们还执行四个自由度姿势图优化以实现全局一致性。 我们在公共数据集和实际实验中验证了我们系统的性能，并与其他最先进的算法进行了比较。 我们还在MAV平台上执行板载闭环自主飞行，并将算法移植到基于iOS的演示。我们强调，所提出的工作是一个可靠，完整，通用的系统，适用于需要高精度定位的不同应用。 我们为PCs1和iOS移动设备2开源。
1.介绍
毫无疑问，状态估计是用于广泛应用的最基本模块，例如机器人导航，自动驾驶，虚拟现实（VR）和增强现实（AR）。由于其尺寸小，成本低且易于硬件设置[1] - [5]，仅使用单目相机的方法已获得极大兴趣。 然而，单目视觉系统无法恢复尺度，因此限制了它们在现实世界机器人应用中的使用。 最近，我们看到了使用低成本惯性测量单元（IMU）协助单目视觉系统的增长趋势。这种单目视觉惯性系统（VINS）的主要优点是可以观察到尺度，以及滚动和俯仰角。 这使得需要度量状态估计的导航任务成为可能 此外，通过IMU测量的耦合可以显着改善运动跟踪性能，桥接由于照明变化，无纹理区域或运动模糊导致的视觉轨迹损失之间的差距。事实上，单目VINS不仅可以在移动机器人，无人机和移动设备上广泛使用，它也是足够的感知自我环境的最小传感器。
然而，所有这些优势都需要付出代价。 对于单目VINS，众所周知，需要加速度激励来使尺度可观。 这意味着单目VINS估计器不能从静止状态启动，而是从未知的移动状态发射。同时认识到视觉惯性系统是高度非线性的这一事实，我们在估算器初始化方面面临重大挑战。 两个传感器的存在也使得相机IMU外部校准变得至关重要。 最后，为了消除可接受的处理窗口内的长期漂移，必须开发包括视觉惯性测距，环路检测，重新定位和全局优化的完整系统。
为了解决所有这些问题，我们提出了VINS-Mono，一种强大而通用的单目视觉惯性状态估计器。 我们的解决方案从即时估算器初始化开始。 同样的初始化模块也用于故障恢复。 我们的解决方案的核心是基于紧耦合滑动窗非线性优化的强大的单目视觉惯性测距仪（VIO）。 单目VIO模块不仅提供精确的局部姿态，速度和方向估计，还以在线方式执行相机IMU外部校准和IMU偏差校正。 使用DBoW2 [6]检测循环。重新定位由特征点级融合与单目VIO紧密耦合的完成。 这使得能够以最小的计算开销实现稳健且准确的重定位。 最后，可验证的几何环被添加到姿势图中，并且由于来自单目VIO的可观察的滚动角和俯仰角，执行四个自由度（DOF）姿势图以确保全局一致性。（为什么是四自由度？）
VINS-Mono结合并改进了我们之前关于单眼视觉惯性融合的研究[7] - [10]。 它基于我们紧密耦合，基于优化的单眼VIO公式[7]，[8]，并结合了[9]中介绍的改进的初始化程序。 [10]中给出了移植到移动设备的第一次尝试。 与之前的工作相比，VINS-Mono的进一步改进包括改进的IMU预积分与偏差校正，紧耦合重新定位，全局姿势图优化，广泛的实验评估以及强大而通用的开源实现。
整个系统完整且易于使用。 它已成功应用于小规模AR场景，中型无人机导航和大规模状态估计任务。 与其他最先进的方法相比，表现出了卓越的性能。 为此，我们总结了我们的贡献如下：
a. 一个强大的初始化过程，能够从未知的初始状态引导系统。
b. 一种基于优化的紧密耦合的单目视觉测量仪，具有相机-IMU外部校准和IMU偏差估计。
c. 在线环路检测和紧密耦合的重新定位。
d. 四自由度的全局姿态图优化。
e. 实时演示自主导航、大尺度定位和移动AR应用。
f. ROS的PC版本以及在iPhone6s或更高版本上运行的iOS版本的开源。
本文的其余部分的结构如下。 在第2节，我们讨论了相关文献。 在第3节，我们概述了中的完整系统流程。视觉和预积分IMU测量的预处理步骤在第4节。 在第5节，我们讨论估计器初始化过程。在第6节 一个紧密耦合，自校准，非线性优化的单目VIO。 在第7、8节中提出了紧耦合重定位和全局姿态图优化。实施细节和实验结果见第9节。最后，本文在第10节将做出结论，讨论未来可能的研究方向。
2. 相关工作
基于单眼视觉的状态估计/测距/ SLAM的学术研究是广泛的。 值得注意的方法包括PTAM [1]，SVO [2]，LSD-SLAM [3]，DSO [5]和ORB-SLAM [4]。 很明显，任何进行全面相关审查的尝试都是不完整的。 然而，在本节中，我们跳过了关于仅视觉方法的讨论，并且只关注单目视觉惯性状态估计的最相关结果。
处理视觉和惯性测量的最简单方法是松耦合的传感器融合[11]，[12]，其中IMU被视为一个独立的模块，以协助从运动的视觉结构获得的仅视觉姿势估计。 融合通常通过扩展卡尔曼滤波器（EKF）来完成，其中IMU用于状态迭代，并且仅视觉姿势用于更新。 此外，紧耦合的可视化算法要么基于EKF [13] - [15]，要么基于图优化[7]，[8]，[16]，[17]，其中相机和IMU测量结果是从原始测量水平获得。 一种流行的基于EKF的VIO方法是MSCKF [13]，[14]。 MSCKF在状态向量中维护多个先前的相机姿势，并且使用跨多个相机视图的相同特征的视觉测量来形成多约束更新。 SR-ISWF [18]，[19]是MSCKF的扩展。它使用squareroot形式[20]来实现单精度表示并避免差的数值属性。 该方法使用逆滤波器进行迭代重新线性化，使其等价于基于优化的算法。 批量图优化或束调整技术维持并优化所有测量以获得最佳状态估计。 为了实现恒定的处理时间，流行的基于图形的VIO方法[8]，[16]，[17]通常通过边缘化过去的状态和测量来优化最近状态的有界大小的滑动窗口。 由于迭代求解非线性系统的高计算需求，很少基于图形可以在资源受限的平台上实现实时性能，例如移动电话。
对于视觉测量处理，可以根据视觉残差模型的定义将算法分类为直接或间接方法。 直接方法[2]，[3]，[21]最小化光度误差，而间接方法[8]，[14]，[16]最小化几何位移。 直接方法由于其小的吸引区域而需要良好的初始猜测，而间接方法在提取和匹配特征上消耗额外的计算资源。 由于其成熟度和稳健性，间接方法在实际工程部署中更常见。 然而，直接方法更容易扩展用于稠密建图，因为它们直接在像素级操作。
实际上，IMU通常以比摄像机高得多的频率获取数据。 已经提出了不同的方法来处理高频IMU测量。 最直接的方法是在基于EKF的方法中使用IMU进行状态迭代[11]，[13]。 在图优化公式中，开发了一种称为IMU预积分的有效技术，以避免重复的IMU重新积分。该技术首先在[22]（预积分）中引入，其使用欧拉角来参数化旋转误差。 我们以前的工作[7]开发了一种用于IMU预整合的流形上旋转公式。 这项工作使用连续时间IMU错误状态动态推导出协方差迭代。 然而，IMU偏差被忽略了。 通过增加后路IMU偏差校正，[23]进一步改善了预积分理论。
准确的初始值对于引导任何单目VINS至关重要。 在[8]，[24]中提出了一种利用短期IMU预积分的相对旋转的线性估计初始化方法。 然而，该方法不对陀螺仪偏差建模，并且不能在原始投影方程中模拟传感器噪声。 在实际应用程序中，当视觉特征远离传感器套件时，这会导致初始化不可靠。 [25]中介绍了单眼视觉惯性初始化问题的封闭形式解决方案。后来，[26]提出了通过增加陀螺仪偏差校准来扩展这种封闭形式的解决方案。 这些方法无法建模惯性积分中的不确定性，因为它们依赖于长时间内IMU测量的双重积分。 在[27]中，提出了一种基于SVO的重新初始化和故障恢复算法[2]。 这是一种基于松散耦合的融合框架的实用方法。 然而，需要额外的向下的距离传感器来恢复度量标度。 在[17]中引入了一种建立在流行的ORB-SLAM [4]之上的初始化算法。 在给定来自ORBSLAM的一组关键帧的情况下，针对视觉惯性全BA计算尺度，重力方向，速度和IMU偏差的初始估计。 但是，据报道，缩放收敛所需的时间可能超过10秒。 这可能会给机器人导航任务带来问题，这些任务需要在开始时进行比例估算。
VIO方法，无论它们所依赖的基础数学公式如何，都会受到全局转换(global translation)和定位的长期漂移的影响。 为此，循环闭合对于长期操作起着重要作用。 ORBSLAM [4]能够关闭循环并重用地图，利用了Bag-of-World[6]。 在循环检测之后进行7自由度 [28]（位置，方向和比例）姿势图优化。 相比之下，对于单目VINS，由于增加了IMU，漂移仅发生在4自由度（3D平移）和围绕重力方向（偏航角度）的旋转中。 因此，在本文中，我们选择在最小4自由度中使用循环约束来优化姿势图。
3. 概述
所提出的单目视觉惯性状态估计器的结构如图2所示。系统从测量数据的预处理开始（第4节），其中提取和跟踪特征点，并且预积分两个连续帧之间的IMU测量值。 初始化过程（第5节）提供所有必要的值，包括姿势，速度，重力矢量，陀螺仪偏差和3D特征点位置，用于引导随后的基于非线性优化的VIO。具有重新定位（第7节）模块的VIO（第6节）紧耦合了预积分的IMU测量值，观察得特征点以及来自闭环的重新检测特征。 最后，姿势图优化模块（第8节）采用几何验证的重定位结果，并执行全局优化以消除漂移。 VIO，重定位(relocalization)和姿势图优化(pose graph optimization)模块设置在多线程中同时运行。 每个模块都有不同的运行速度和实时保证，以确保始终可靠运行。
我们现在定义我们在整篇论文中使用的符号和坐标系定义。 我们认为(·)w是世界坐标系。 重力方向与世界坐标系的z轴对齐。(·)b是body坐标系，我们将其定义为与IMU坐标系相同。(·)c是相机坐标系。我们使用旋转矩阵R和Hamilton四元数q来表示旋转。 我们主要在状态向量中使用四元数，但旋转矩阵也用于方便地旋转3D向量。qbw，pbw是从body坐标系到世界坐标系的旋转和平移。bk是拍摄第k张图像时的body坐标系(body frame)，ck是拍摄第k张图像时的相机坐标系(camera frame)，⊕代表四元数乘法，gw=[0,0,g]T是世界坐标系中的重力向量，最后，我们用(^)表示噪声测量和某一数量的估计。
图2
4. 测量数据预处理(Measurement Preprocessing)
本节介绍惯性和单目视觉测量的预处理步骤。 对于视觉测量，我们跟踪连续帧之间的特征点并检测最新帧中的新特征点。 对于IMU测量，我们在两个连续帧之间预积分。 请注意，我们使用的低成本IMU的测量值受偏置和噪声的影响。 因此，我们特别在IMU预积分过程中考虑偏差。
A. 视觉处理前端(Vision Processing Front-end)
对于每张新图像，现有特征点由KLT稀疏光流算法[29]跟踪。 同时，检测新的角点特征[30](Shi-Tomasi)以维持每个图像中的特征的最小数量（100-300）。 检测器通过设置两个相邻特征之间的最小像素间隔来实施均匀的特征分布。 2D特征首先不失真，然后在通过outlier rejection后投射到单位球面。 使用基本矩阵模型的RANSAC进行outlier rejection[31]。
在此步骤中也选择了关键帧。 我们有两个关键帧选择标准。第一个是平均视差。 如果跟踪要素的平均视差在当前帧和最新关键帧之间超出某个阈值，我们将帧视为新的关键帧。 请注意，不仅平移而且旋转都会导致视差。 但是，在仅旋转运动中不能对特征进行三角测量。为了避免这种情况，我们在计算视差时使用陀螺仪测量的短期积分来补偿旋转。 请注意，此旋转补偿仅用于关键帧选择，不参与VINS公式中的旋转计算。 为此，即使陀螺仪包含大噪声或偏置，也只会导致关键帧选择结果不理想，并且不会直接影响估计质量。另一个标准是跟踪质量。 如果跟踪要素的数量低于某个阈值，我们会将此帧视为新的关键帧。 该标准是为了避免完全丢失特征轨迹。
B. IMU预积分(IMU Pre-integration)
IMU预积分首先在[22]中提出，它参数化了欧拉角的旋转误差。 在我们以前的工作中开发了用于IMU预积分的流形上旋转公式[7]。 这项工作使用连续时间IMU误差状态动态推导出协方差迭代。然而，IMU偏差被忽略了。 通过增加后路IMU偏差校正，[23]进一步改善了预积分理论。 在本文中，我们通过结合IMU偏差校正扩展了我们之前的工作[7]中提出的IMU预积分。（这段话没啥用）
来自IMU的原始陀螺和加速度计数据ω和a(有半角符号)，又下述给出：
在body坐标系中IMU的测量结合了用于抵抗重力和平台动力学的力，并受到加速度偏差ba，陀螺仪偏差bw和加性噪声的影响。 我们假设加速度和陀螺仪测量中的加性噪声是高斯噪声，na~N(0,σa2) ，nw~N(0,σw2)。加速度偏差和陀螺仪偏差被建模为倒数为高斯的随机噪声
给定对应于图像bk和bk + 1的两个时刻，位置，速度和方向状态可以由世界坐标系下[tk,tk+1]世界内的惯性测量来迭代(?propagated)。
可以看出，IMU状态迭代需要帧bk的旋转，位置和速度。 当这些起始状态发生变化时，我们需要重新迭代IMU测量值。 特别是在基于优化的算法中，每次调整姿势时，我们都需要在它们之间重新迭代IMU测量值。 这种迭代策略在计算上要求很高。 为避免重新迭代，我们采用预积分算法。（不懂为什么）在将参考坐标系从世界帧坐标系为局部坐标系bk之后，我们只能预积分与线性加速度a^ 和角速度ω^相关的部分，如下所示：
可以看出，通过将bk作为参考帧，可以仅通过IMU测量获得预积分项(6)。αbk+1bk，βbk+1bk，γbk+1bk仅与IMU偏差有关，而与bk和bk+1中的其他状态无关。当偏差的估计发生变化时，如果变化很小，我们会根据偏差调整αbk+1bk，βbk+1bk，γbk+1bk的一阶近似值，否则我们会进行重新迭代。 该策略为基于优化的算法节省了大量计算资源，因为我们不需要重复迭代IMU测量。对于离散时间实现，可以应用不同的数值积分方法，例如Euler，中点，RK4积分。 这里选择Euler法来演示易于理解的过程（我们在实现代码中使用中点法）。
一开始αbkbk，βbkbk为0，γbkbk是单位四元数。(6)中的α，β，γ的平均值如下逐步迭代。 注意加性噪声项na，nw是未知的，并且在实现中被视为0。由（^）标记预积分项的估计值：
公式6展开
i是对应于[tk，tk+1]内的IMU测量的离散时刻。 t是两个IMU测量i和i+1之间的时间间隔。然后我们处理协方差迭代。 由于四维旋转四元数γtbk过度参数化，我们将其误差项定义为围绕其平均值的扰动：
其中δθtbk是三维小扰动。我们可以导出(6)的误差项的连续时间线性化（另一篇文献中定义为残差）：
（感觉公式可能有问题）Pbk+1bk可以通过初始协方差Pbkbk = 0的一阶离散时间协方差更新递归计算：
其中Q是噪声的对角协方差矩阵。
同时，相对于δzbkbk的δzbk+1bk的一阶雅可比矩阵Jbk+1也可以用初始雅可比Jbk = I递归计算，
使用这个递归公式，我们得到协方差矩阵Pbk+1bk ，Jbk+1 ，关于偏差的一阶近似αbk+1bk，βbk+1bk，γbk+1bk可写为：
式(12)中许多J是Jbk+1 的字块。当偏差的估计稍微改变时，我们使用(12)来近似地校正预积分结果而不是重新迭代。现在我们能够用相应的协方差Pbk+1bk 写下IMU测量模型
5. 估计器初始化
单目紧耦合视觉惯性测距法是一种高度非线性系统。 由于无法从单目摄像机直接得到尺度，因此如果没有良好的初始值，很难直接融合这两个测量值。 人们可以假设一个固定的初始条件来启动单目VINS估计器。 然而，这种假设是不合适的，因为在实际应用中经常遇到运动初始化。 当IMU测量被大偏差破坏时，情况变得目更加复杂。 事实上，初始化通常是单VINS最脆弱的一步。 需要一个鲁棒的初始化过程来确保系统的适用性。
我们采用松耦合传感器融合方法来获得初始值。 我们发现仅视觉SLAM或Motion from Motion（SfM）具有良好的初始化特性。在大多数情况下，仅视觉系统可以通过相对运动方法的导出初始值来自我引导，例如八点[32]或五点[33]算法，或估计H矩阵。 通过将度量IMU预积分与仅视觉SfM结果对齐，我们可以粗略地恢复比例，重力，速度甚至偏差。 这足以引导非线性单目VINS估计器，如图4所示。
图4
与在初始化阶段同时估计陀螺仪和加速度计偏差的[17]相比，我们选择忽略初始步骤中的加速度计偏差项。 加速度计偏差与重力耦合，由于较大的重力矢量幅度，并且在初始化阶段期间相对较短，因此难以观察到这些偏差项。 我们之前的工作[34]中提供了加速度计偏差校准的详细分析。
A. Sliding Window Vision-Only SfM
初始化过程从仅视觉SfM开始，以估计相机比例位置(up-to-scale pose)和特征点位置。
我们维护滑动窗口中的图图像以限制计算复杂性。 首先，我们检查最新帧和所有先前帧之间的特征点对应关系。 如果我们能够在最新帧和滑动窗口中的任何其他帧之间找到稳定的特征跟踪（超过30个跟踪特征）和足够的视差（超过20个旋转补偿像素）。 我们使用五点算法[33]恢复这两帧之间的相对旋转和无尺度平移。（五点法，可能可调参数）否则，我们在窗口中保留最新帧并等待新帧。 如果五点算法成功，我们任意设置尺度并三角测量在这两个帧中观察到的所有特征。 基于这些三角测量特征，执行透视n点（PnP）方法[35]以估计窗口中所有其他帧的姿势。 最后，应用全局完整束调整[36]以最小化所有特征观察的总重投影误差。 由于我们还没有关于世界坐标系的任何知识，我们将第一个相机坐标系(·)c0设置为SfM的参考坐标系。所有帧图像的位置(pckco,qckco )是参考c0坐标系的，假设我们有一个关于相机和IMU的外部参数(pcb,qcb)，我们可以将相机坐标系下的位置转换到body(IMU)坐标系中
其中s缩放参数对齐位置。 解决此缩放参数是实现成功初始化的关键。
B. 视觉-惯性融合
1)陀螺仪偏差校准：在窗口中考虑两个连续帧bk和bk+1，在视觉SfM中我们已经获得了旋转qbkco和平移pbkco，还从IMU预积分获得了相对约束条件γbk+1bk，我们根据陀螺仪偏差线性化IMU预积分项，并最小化以下成本函数：
其中B代表滑动窗口中的所有帧。我们使用在第四节-B中导出的偏置雅可比行列式对陀螺仪偏差γbk+1bk进行一阶近似，通过这种方式，我们得到了陀螺仪偏差bw的初始校准。然后，我们使用新的陀螺仪偏差重新迭代所有IMU预积分项αbk+1bk，βbk+1bk，γbk+1bk。2)速度、重力矢量和度量尺度初始化:
在陀螺仪偏差初始化之后，我们继续初始化导航的其他基本状态，即速度，重力矢量和标准尺度：
其中vbkbk是在第k帧时body坐标系下的速度，gc0是c0坐标系中的重力矢量，s是尺度。在窗口中考虑两个连续帧bk和bk + 1，公式(5)可以写成：
我们可以将(14)和(17)组合成以下线性测量模型：
3)重力细化(?Gravity Refinement):
可以从先前线性初始化步骤通过约束幅度来细化获得的重力矢量。 在大多数情况下，重力矢量的大小是已知的。 这导致重力矢量仅剩余2个自由度。 因此，我们在其切线空间上用两个变量重新参数化重力。我们的参数化将重力矢量表示为g ^ g + w1b1 + w2b2，其中g是已知的重力大小，^ g是表示重力方向的单位矢量。 b1和b2是跨越切平面的两个正交基，如图5所示。w1，w2分别是朝向b1和b2的相应位移。 我们可以使用算法1通过交叉乘积运算找到一组b1，b2。然后我们用g ^ g + w1b1 + w2b2代替(17)中的g ，并将w1和w2与其他状态变量一起求解。 该过程迭代直到^ g收敛。
4)完成初始化：经过细化重力矢量后，我们可以通过把重力旋转至z轴获得世界坐标系和c0坐标系之间的旋转qc0w。然后我们将所有变量从参考坐标系(·)c0转移到世界坐标系(·)w。body坐标系的速度也将旋转到世界坐标系。 视觉SfM中的平移部分将缩放为公制单位。 此时，初始化过程完成，所有这些度量值将被馈送给紧密耦合的单目VIO。
6. 紧耦单目VIO
在估计器初始化之后，我们继续使用基于滑动窗口的紧耦合单目VIO，以实现高精度和鲁棒的状态估计。 滑动窗如图3所示。
图3
其中xk是在捕捉第k帧图像时IMU的状态(state)，它包括了IMU在世界坐标系下的位置，速度和方向，还包括扩了IMU在body坐标系下的加速度偏差和陀螺仪偏差。n是关键帧的总数，m是滑动窗口中的特征点数。λl是第l观察到的特征点的逆深度(inverse depth)。我们使用视觉惯性约束调整公式。 我们最小化先验之和以及所有残差的Mahalanobis范数，以获得最大后验估计：
这里用了Ceres Solver
C.视觉残差
与在广义图像平面上定义重投影误差的传统针孔相机模型相比，我们在单位球面上定义相机测量残差。 几乎所有类型的摄像机（包括广角，鱼眼或全向摄像机）的光学系统都可以建模为连接单位球面的单位光线。考虑第l个特征在第i帧图像中第一次被观测到，第j帧图像中该特征点的视觉残差被定义为：
图6
D. 边缘化(Marginalization)
为了限制基于优化的VIO的计算复杂性，将边缘化算法纳入其中。 我们有选择地从滑动窗口边缘化IMU状态xk和特征λl，同时将对应于边缘化状态的测量值转换为先验状态。
如图7所示，当倒数第二帧是一个关键帧时，它将停留在窗口中，并且最旧的帧被其边缘化并进行相应的测量。 否则，如果倒数第二帧是非关键帧，我们会抛出视觉测量并保持连接到此非关键帧的IMU测量。 我们不会将非关键帧的所有测量边缘化，以保持系统的稀疏性。我们的边缘化旨在将空间上分离的关键帧保持在窗口中。 这确保了特征三角测量的足够视差，并且最大化了在大激励下维持加速度计测量的可能性。
图7
边缘化是使用Schur补集[39]进行的。 我们基于边缘化来构建新的先验。 新的先验被添加到之前存在的先验中。我们注意到边缘化导致线性化点的早期修复，这可能导致次优的估计结果。 然而，由于小型漂移对VIO来说是可以接受的，我们认为边缘化造成的负面影响并不重要。
E. Motion-only Visual-Inertial Bundle Adjustment for Camera-Rate State Estimation
对于具有低计算能力的设备，例如移动电话，由于非线性优化的大量计算需求，紧耦合的目VIO无法实现相机速率输出。 为此，我们采用lightweight motion-only visual-inertial bundle adjustment提升到摄像机速率（30 Hz）。
motion-only visual-inertial bundle adjustment的成本函数与（22）中单目VIO的成本函数相同。 然而，我们只是优化固定数量的最新IMU状态的姿势和速度，而不是优化滑动窗口中的所有状态。 我们将特征的深度，外部参数，偏差和旧IMU状态视为我们不希望优化为常数值。 我们确实将所有视觉和惯性测量用于 motion-only bundle adjustmen。 这导致比单帧PnP方法更平滑的状态估计。 图8显示了所提出策略的一个例子。与完全紧密耦合的单眼VIO相比，它可以在最先进的嵌入式计算机上产生超过50ms的速度，the motion-only visual-inertial bundle adjustment需要大约5毫秒来计算。 这使得低延迟相机速率姿态估计能够对无人机和AR应用特别有益。
F. IMU Forward Propagation for IMU-Rate State Estimation
IMU测量的速度远高于视觉测量。 虽然我们的VIO频率受到图像捕获频率的限制，但我们仍然可以使用最新的IMU测量值直接迭代最新的VIO估计值，以实现IMU速率性能。 高频状态估计可以用作闭环闭合的状态反馈。 利用这种IMU速率状态估计的自主飞行实验在第9节D中给出。
G. 故障检测和恢复
虽然我们紧密耦合的单目VIO可以适应各种具有挑战性的环境和运动。 由于剧烈的光照变化或严重的剧烈运动，故障是不可避免的。 主动故障检测和恢复策略可以提高所提出系统的实用性。 故障检测是一个独立的模块，可以检测估算器的异常输出。 我们目前使用以下标准进行故障检测：
a.在最新帧中跟踪的特征数量小于某个阈值;
b. 最后两个估算器输出之间的位置或旋转的大的不连续;
c.偏差或外在参数估计的大变化;
一旦检测到故障，系统就会切换回初始化阶段。 单目VIO成功初始化后，将创建一个新的独立的位姿图片段。
7. 重定位(RELOCALIZATION)
我们的滑动窗口和边缘化方案限制了计算的复杂性，但它也引入了系统的累积漂移。 更具体地说，漂移发生在全局3D位置(x,y,z)和绕重力方向(yaw)的旋转。 为了消除漂移，提出了一种与单目VIO紧密耦合的重定位模块。
重定位过程从回环检测模块( loop detection module)开始，该模块识别已经访问过的位置。 然后建立循环闭包候选与当前帧之间的特征级连接。 这些特征对应关系紧密集成到单目VIO模块中，从而以最小的计算开销实现无漂移状态估计。 多个特征的多个观察直接用于重定位，从而导致更高的准确度和更好的状态估计平滑度。 重定位过程的图解说明如图9(a)所示。
A. 回环检测
我们利用DBoW2 [6]，这是一种先进的词袋位置识别方法，用于环路检测。 除了用于单目VIO的角点特征之外，还有Jane描述符[40]检测和描述了500多个角点。 附加的角点特征用于在环路检测上实现更好的召回率。 描述符被视为查询可视数据库的可视单词。 DBoW2在时间和几何一致性检查后返回回环候选闭包。 我们保留所有简要描述符以进行特征检索，但丢弃原始图像以减少内存消耗。 我们注意到我们的单目VIO能够显示滚动和俯仰角度。 因此，我们不需要依赖旋转不变特征，例如ORB SLAM [4]中使用的ORB特征。
B. 特征检索
当检测到回环时，通过检索特征对应关系建立本地滑动窗口和回环候选闭包之间的连接。 通过简要描述符匹配找到对应关系。 直接描述符匹配可能导致大量异常值。 为此，我们使用两步几何异常值抑制，如图10所示。
当超过某个阈值的内点时，我们将该候选者视为正确的环路检测并执行重定位。
C. 紧耦合重定位
重定位过程有效地将单目VIO（第六节）维持的当前滑动窗口与过去姿势的图形对齐。 在重定位期间，我们将所有回环闭包的姿势视为常量。 我们使用所有IMU测量，局部视觉测量测量以及从闭环中检索的特征来共同优化滑动窗口。 我们可以很容易地为回环闭合框架v中观察到的检索特征编写视觉测量模型，使其与VIO中的视觉测量相同，如(25，视觉残差)所示。
注意，虽然成本函数与(22)略有不同，但要解决的状态的维度保持不变，因为回环闭包的姿势被认为是常数。 当使用当前滑动窗口建立多个回环闭包时，我们同时使用来自所有帧的所有循环闭包特征对应进行优化。 这为重定位提供了多视图约束，从而产生更高的准确性和更好的平滑度。 请注意，过去姿势和回环闭包帧的全局优化发生在重定位之后，将在第8节中讨论。
8. 全局位姿图优化
重定位后，局部滑动窗口移动并与过去的姿势对齐。 利用重定位结果，开发了这个附加姿势图优化步骤，以确保将过去位姿集进入(registered)到全局一致的配置中。
由于我们的视觉惯性的建立使得滚动角和俯仰角完全可观察，因此累积的漂移仅发生在四个自由度（x，y，z和偏航角）中。 为此，我们忽略了无漂移滚动和俯仰状态的估计，并且仅执行4自由姿势图优化。
A. 在位姿图中加入关键帧
当关键帧从滑动窗口边缘化时，它将被添加到姿势图中。 此关键帧充当姿势图中的顶点(vertex)，并通过两种类型的边连接其他顶点：
(2)Loop Closure Edge：如果新边缘化的关键帧具有回环连接，它将通过姿势图中的回环闭合边缘与循环闭合frame连接。 类似地，回环闭包边仅包含4-DOF相对姿势变换，其定义与(27)相同。 使用重定位的结果获得回环闭包边缘的值。(看不懂)
整个图通过最小化以下成本函数来优化：
姿态图优化和重定位（第8节-C）在两个独立的线程中异步运行。 这使得能够立即使用最优化的姿势图，以便在可用时进行重定位。 类似地，即使当前姿势图优化尚未完成，仍然可以使用现有姿势图配置进行重定位。 该过程如图9(b)所示。
C. 位姿图管理(移除一些关键帧)
当行程距离增加时，姿势图的大小可能无限增长，从长远来看限制了系统的实时性能。 为此，我们实施了一个下采样过程，以将姿势图数据库维持在有限的大小。 将保留具有循环闭包约束的所有关键帧，而可以移除太靠近或具有与其邻居非常相似的取向的其他关键帧。 移除关键帧的概率与其邻居的空间密度成正比。
9.实验结果
10.结论和未来工作
在本文中，我们提出了一种强大而通用的单目视觉惯性估计器。 我们的方法具有最先进和新颖的IMU预积分，估计器初始化和故障恢复，在线外部校准，紧耦合视觉惯性测量，重定位和高效全局优化解决方案。 通过与最先进的开源实施和高度优化的行业解决方案进行比较，我们展示了卓越的性能。 我们开源PC和iOS实现，以造福社区。
尽管基于特征的VINS估计已经达到了实际使用的成熟度，但我们仍然看到了未来研究的许多方向。 单目VINS可能会根据运动和环境达到微弱可观察甚至退化的状态。 我们最感兴趣的是在线方法来评估单眼VINS的可观察性，以及在线生成运动计划以恢复可观察性。 另一个研究方向涉及单眼VINS在各种消费设备（如移动电话）上的大规模部署。该应用需要在线校准几乎所有传感器的内部和外部参数，以及校准质量的在线识别。 最后，我们有兴趣根据单目VINS的结果生成密集的地图。 我们在[44]中提出了应用于无人机导航的单目视觉 - 惯性稠密地图的第一个结果。 然而，仍需要进行广泛的研究以进一步提高系统的准确性和稳健性。
网友评论