港科大VINS-MONO入门（一）：框架入门及源码解析

一、VINS介绍

VINS-Mono是HKUST的Shen Shaojie团队开源的一套Visual-Inertial融合定位算法. 介绍见(https://github.com/HKUST-Aerial-Robotics/VINS-Mono), 论文里面也有.

此外,他们还开源了ios版本(https://github.com/HKUST-Aerial-Robotics/VINS-Mobile), 有兴趣可以去下载源码跑一跑, 好像已经有国外团队基于这套开源代码开发了一些AR的App
--------------------- 

1.github地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git

2.演示视频地址：https://www.youtube.com/watch?v=I4txdvGhT6I

二、源码解析参考博客

VINS-Mono主要包含两个节点: 前端节点feature_tracker_node和后端节点estimator_node. 前端节点处理Measurement Preprocessing中的Feature Detection and Tracking, 其他几个部分(IMU preintegration, initialization, LocalBA, Loop closure)都是在estimator_node中处理. 

feature_tracker_node中就一个主线程, 接收图像信息,调用img_callback()函数进行处理.

estimator_node节点中开了3个子线程, 分别是:process,loop_detection和pose_graph. 其中 process()线程处理VIO后端, 包括(IMU preintegration, initialization, LocalBA)loop_detection()线程处理闭环检测pose_graph()线程分别处理全局优化。如果不需要做loop closure, 可以把LOOP_CLOUSRE置0, 则后面两个线程不会创建.

https://blog.csdn.net/q597967420/article/details/76099409

https://blog.csdn.net/mcw1234/article/details/83039506

三、跑通VINS的demo

1.终端一

运行feature_tracker节点和estimator节点, 订阅图像和IMU数据, 发布位姿, 3D特征点等消息给RVIZ显示

roslaunch vins_estimator euroc.launch

2.终端二

roslaunch vins_estimator vins_rviz.launch

3.终端三

rosbag play YOUR_PATH_TO_DATASET/

三、论文细读

       1.引言

SLAM发展趋势，近年来的发展趋势为用低成本惯性测量单元(IMU)辅助单目视觉系统。这种单目视觉-惯性系统(VINS)的主要优点是具有可观测的度量尺度，以及翻滚角(roll)和俯仰角(pitch)。这让需要有尺度的状态估计的导航任务成为可能。此外，对IMU测量值的积分可以显著提高运动跟踪性能，弥补光照变化、缺少纹理的区域或运动模糊的视觉轨迹损失的差距。事实上，单目VINS不仅广泛应用于移动机器人、无人机和移动设备上，还是满足充分自我感知和环境感知的最小传感器。

        对于单目VINS，众所周知，需要加速度激励以测量尺度。这意味着单目VIN估计器不能从静止状态启动，而是从未知的移动状态发动。同时要认识到视觉惯性系统高度非线性的事实，在估计器初始化方面还有重大挑战。两个传感器的存在也使得摄像机-IMU的外部校准至关重要。最后，为了消除在可接受的处理窗口内的长期漂移，提出了一个完整的系统，包括视觉惯性里程计、回环检测、重定位和全局优化。

~~本文略去一二，感兴趣请参见原文~~

        原文的解决方案的核心是一个鲁棒的基于紧耦合的滑动窗非线性优化的单目视觉惯性里程计(VIO)。本文主要贡献如下：

• 1、一个鲁棒的初始化过程，它能够从未知的初始状态引导系统。
• 2、一个紧耦合、基于优化的单目视觉惯性里程计，具有相机-IMU外部校准和IMU偏置估计。
• 3、在线回环检测与紧耦合重定位。
• 4、四自由度全局位姿图优化。
• 5、用于无人机导航、大规模定位和移动AR应用的实时性能演示。
• 6、完全集成于ros的pc版本以及可在iphone 6或更高版本上运行的IOS版本的开源代码。

2.相关工作

值得注意的方法包括PTAM[1]、SVO[2]、LSD-SLAM[3]、DSO[5]和ORB-SLAM[4]。

处理视觉和惯性测量的最简单的方法是松耦合的传感器融合[11][12]，其中IMU被视为一个独立的模块，用于辅助运动的视觉结构(sfm)获得的纯视觉位姿估计。融合通常由扩展卡尔曼滤波(EKF)完成，其中IMU用于状态传播，而视觉位姿用于更新。（即紧耦合VIO）

进一步说，紧耦合视觉惯性算法要么基于EKF[13]-[15]，要么基于图优化[7][8][16][17]，其中相机和IMU测量值是从原始测量水平联合优化的。

一种流行的基于EKF的VIO方法是MSCKF[13][14]。MSCKF在状态向量中维护以前的几个摄像机位姿，并使用多个摄像机视图中相同特征的视觉测量来形成多约束更新。

SR-ISWF[18][19]是MSCKF的扩展。它采用squareroot形式[20]实现单精度表示，避免了较差的数值性质。该方法采用逆滤波器进行迭代再线性化，使其与基于优化的算法相当。

批量图优化或集束调整技术（BA）维护和优化所有测量值以获得最优状态估计。为了达到恒定的处理时间，流行的基于图的VIO方法[8][16][17]通常采用边缘化过去的状态和测量来优化最近状态的有界滑动窗口。由于对非线性系统迭代求解的计算要求很高，很少有基于图的非线性系统能够在资源受限的平台（如手机上）实现实时性能。

在实践中，IMU通常以比摄像机更高的速率获取数据。不同的方法被提出来处理高速率的IMU测量值。最简单的方法是在基于EKF的方法中使用IMU进行状态传播[11][13]。在图优化公式中，为了避免重复的IMU重复积分，提出了一种有效的方法，即IMU预积分(IMU pre-integration)。

IMU与积分用欧拉角来参数化旋转误差。在我们先前的工作中[7]，我们提出了一种流形上的IMU预积分旋转公式，利用连续IMU误差状态动力学推导了协方差传递方程。然而IMU偏置被忽略了。在[23]中通过增加后验IMU偏置校正，进一步改进了预积分理论。

词袋模型（Bag of words,BOW）和词向量是自然语言处理和文本分析中的两种最常用模型。

IMU预积分理论太过详细庞杂，此处略去，以下给个本文对它的总结：

VIO方法，不管它们所依赖的基本数学公式，在全局的平移和旋转中长期受到漂移的影响。为此，回环检测在长期操作中起着重要的作用。ORB-SLAM[4] 利用了词袋模型能够闭合回环并重用地图。回环检测之后进行7自由度（位置、方向和尺度）的位姿图优化。相对于单目VINS，由于IMU的加入，漂移只发生在4自由度，即三维平移，和围绕重力方向的旋转（偏航角）。因此，本文选择在最小四自由度设定下，优化具有回环约束的位姿图。

3.概述

提出的单目视觉惯性状态估计器的结构如下图所示。

• 系统从测量预处理(IV)开始，在其中提取和跟踪特征，对两个连续帧间的IMU测量值进行预积分。
• 初始化过程(V)提供了所有必要的值，包括姿态、速度、重力向量、陀螺仪偏置和三维特征位置，用于引导随后的基于非线性优化的VIO。
• VIO(VI)与重定位(VII)模块紧密地融合了预先积分的IMU测量、特征观测和回环重新检测到的特征。最后，位姿图优化模块(VIII)接受几何验证的重定位结果，并进行全局优化以消除漂移。
• VIO、重新定位和位姿图优化模块在多线程设置中同时运行。每个模块有不同的运行速度和实时保证，以确保在任何时候可靠运行。

接下来为论文中的符号与坐标系定义。

w是世界坐标系(world frame)。重力方向与世界坐标系z轴对齐。

b是本体坐标系(body frame)，我们把它定义为与IMU坐标系相同。

c是相机坐标系(camera frame)。我们同时使用旋转矩阵R和Hamilton四元数q来表示旋转。我们主要在状态向量中使用四元数，也用旋转矩阵来表示三维向量的旋转。

ck​表示获取第k个图像时的相机坐标系。⊗表示两个四元数之间的乘法运算。gw是世界坐标系上的重力向量。最后，我们将(^)表示为某一具体量的噪声测量值或估计值。

4.测量预处理

对于视觉测量，我们跟踪连续帧之间的特征，并在最新帧中检测新特征。对于IMU测量，我们在两个连续帧之间做预积分。

请注意，我们使用的低成本IMU的测量值受到偏置和噪声的影响。因此，我们在IMU预积分过程中特别考虑偏置。

A.视觉处理前端

对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪[29]。同时，检测新的角点特征[30]以保证每个图像特征的最小数目(100-300)。该检测器通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布。二维特征首先是不失真的，然后在通过外点剔除后投影到一个单位球面上。利用基本矩阵模型的RANSAC算法进行外点剔除。

关键帧选择

我们有两个关键帧选择标准。

第一是与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。

请注意，不仅平移，旋转也会产生视差。然而，特征点无法在纯旋转运动中三角化。为了避免这种情况，在计算视差时我们使用陀螺仪测量值的短时积分来补偿旋转。请注意，此旋转补偿仅用于关键帧选择，而不涉及VINS公式中的旋转计算。为此，即使陀螺仪含有较大的噪声或存在偏置，也只会导致次优的关键帧选择结果，不会直接影响估计质量。

另一个标准是跟踪质量。如果跟踪的特征数量低于某一阈值，我们将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。

B.IMU预积分

~~~详细理论推导太多，楼主SLAM小白尚未吃透，以后再更~~~

5.估计器初始化（Estimator Initialization）

单目紧耦合VIO是一个高度非线性的系统。由于单目相机无法直接观测到尺度，因此，如果没有良好的初始值，很难直接将这两种测量结果融合在一起。

可以假设一个静止的初始条件来启动单目VINS估计器。然而，这种假设是不合适的，因为在实际应用中经常会遇到运动下的初始化。当IMU测量结果被大偏置破坏时，情况就变得更加复杂了。事实上，初始化通常是单目VINS最脆弱的步骤。需要一个鲁棒的初始化过程以确保系统的适用性。

原文采用松耦合的传感器融合方法得到初始值。我们发现纯视觉SLAM，或从运动中恢复结构(SfM)，具有良好的初始化性质。在大多数情况下，纯视觉系统可以通过从相对运动方法（如八点法[32]或五点法[33]或估计单应性矩阵）中导出初始值来引导自己。通过对齐IMU预积分与纯视觉SfM结果，我们可以粗略地恢复尺度、重力、速度，甚至偏置。这足以引导非线性单目VINS估计器。其流程如下图所示：

A.滑动窗口（Sliding Window）纯视觉SfM

B.视觉惯性校准

6.紧耦合单目VIO

在估计器初始化后，原文采用基于滑动窗口的紧耦合单目VIO进行高精度和鲁棒的状态估计。下图显示了滑动窗口的图示。

A.公式

B.IMU测量误差

C.视觉测量残差

D.边缘化

E. 摄像机速率状态估计的纯运动视觉惯性BA

F. IMU前向传递以达到IMU速率状态估计

G. 故障检测与恢复

~~~暂且先略过~~~

7.重定位

A.回环检测

B. 特征恢复

C. 紧耦合重定位

~~略~~

8.全局位姿图优化

A. 在位姿图中添加关键帧

B. 4自由度位姿图优化

C. 位姿图管理

~~略~~

9.实验结果

A. 数据集比较

B. 室内实验

C. 大范围环境

~~~略~~~

10.结论和未来工作

本文提出了一种鲁棒、通用的单目视觉惯性估计器。我们的方法在IMU预积分，估计器初始化和故障恢复，在线外部校准，紧耦合视觉惯性里程计，重定位和有效的全局优化上，具有最先进的和新颖的解决方案。

我们通过与最先进的开源实现和高度优化的行业解决方案进行比较，显示出更好的性能。我们开源了PC和iOS的实现，以造福社会。

研究展望

虽然基于特征的VINS估计器已经达到了实际使用的成熟程度，我们仍然看到了未来研究的许多方向。单目VINS可能会根据运动和环境而达到较难观测甚至退化的状态。我们最感兴趣的是在线方法来评估单目VINS的可观测性，以及在线生成运动计划来恢复可观测性。另一个研究方向是在大量消费设备上大规模部署单目VINS，例如移动电话。这一应用要求在线校准几乎所有传感器的内参和外参，以及在线鉴定校准质量。最后，我们感兴趣的是制作由单目VINS给出的稠密地图。我们在[44]中首次给出了用于无人机导航的单目视觉-惯性稠密地图的结果。然而，仍需进行广泛的研究以进一步提高系统的精度和鲁棒性。

 

 

 

 

本文参考

【1】VINS论文翻译