|期刊分享|SLAM|VINS-Mono:一种稳健的单目视觉惯性状态估计器 上

编者序：本文提出的紧耦合滑窗VINS状态估计算法，可在未知状态情况下稳健初始化，具有线外参标定、统一定义在球面上的重投影误差、闭环检测和4自由度位姿图优化的特点。本文是香港科技大学沈邵劼课题组的开源杰作，媲美state-of-the-art。将分为上、下两部分介绍，上部分侧重整体及实验，下部分侧重理论细节及应用。

推荐指数☆☆☆☆☆

一、引言

本文提出的视觉惯性系统结构框图如下所示。第一部分是测量处理前段，负责提取并跟踪每个新图像帧的特征，及帧间IMU数据预积分。第二部分是初始化过程，提供了激活非线性系统的必要初始值，如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。第三部分为非线性图优化，通过联合优化所有的视觉、惯性信息求解滑窗内的状态。另一部分运行另一个线程，负责闭环检测及姿态图优化。

请点击此处输入图片描述请点击此处输入图片描述

二、各部分概述

测量预处理：对于视觉测量，跟踪连续帧间的特征并检测最近一帧中的新特征；对于IMU测量，连续帧内进行预积分。注意IMU测量受偏差和噪声的影响，因此我们在预积分和优化部分考虑了偏差，这对低成本IMU非常必要。

初始化：单目紧耦合视觉惯性系统是高度非线性系统。由于单目相机不能直接观测到尺度，因此没有好的初始值情况下很难直接融合这两种传感器。一般的，在静止假设的前提下，认为开始几秒的IMU测量值平均是重力向量，IMU传播（propagation）认为是初始姿态。然而，当IMU测量值受非平凡（non-trivial）偏差影响或开始时存在加速运动情况时，这么认为是不合适的。为提高单目视觉惯性系统的成功率，需要稳健的初始化。我们采用松耦合传感器融合方法获得初始值。我们发现仅视觉SLAM或SfM初始化性能良好，大多数情况下可通过Eight-point、Five-point、homogeneous and fundamental等相对运动方法完成初始化。通过将IMU预积分结果与仅视觉对其，可以大体上获得尺度、重力、速度，甚至偏差，这对激活非线性系统是很有好处的，如下图所示。

请点击此处输入图片描述请点击此处输入图片描述

紧耦合非线性优化：状态初始化完成后，运行基于滑窗的非线性估计器进行高精度状态估计，使用Ceres Solver 求解该非线性优化问题。滑窗说明如下图所示。

闭环检测：我们在一个单独线程里进行闭环检测和维持一个仅姿态图。由于滑窗缺少绝对位置和偏航观测，当检测到闭环时，滑窗将通过重定位贴在姿态图上。关键帧从滑窗中边缘化后将被加入到姿态图，当新加入的关键帧包含闭环信息时执行姿态图优化。IMU测量提供了完整的俯仰角和滚动角，所以仅xyz和偏航角4个自由度上有累积漂移，为避免引入虚假信息，我们直接在这4个自由度上优化姿态图。

三、实验

3.1 数据集对比

算法评估使用的是EuRoc MAV数据集，该数据集利用无人机搭载的双目相机和IMU采集的，并通过VICON和Leica MS50采集ground truth。我们仅仅利用双目左边的相机数据。该数据集包括非平凡IMU偏出和光照变化，非常有代表性有挑战性。本实验选用MH_03_median和MH_05_difficult数据。

对比算法选的是目前state-of-the-art算法OKVIS，该算法是另一种基于优化的滑窗算法。与OKVIS算法相比，我们的算法在稳健初始化和闭环检测上更完整，在每个细节上都不太一样。为描述简单起见，记VINS为仅里程计，VINS_loop为含闭环检测姿态图优化的本文算法，OKVIS_mono和OKVIS_stereo分别为单目和双目OKVIS算法。为公平起见，我们丢掉开始的100个输出，取接下来的150个输出，计算该输出到groud truth的变换并对其，由于单目系统需要更长的时间收敛。

实验结果如下图所示。对平移误差来讲，VINS_loop误差最小。对旋转误差来讲，OKVIS更稳定些，在俯仰角和滚动角估计上更准确，可能的原因是本文算法为节约计算资源使用了IMU一阶近似去做预积分，或者OKVIS以更高的频率边缘化IMU约束。

另外，对于仅里程计，本文算法和OKVIS都很好，难分高下，且基于优化的紧耦合算法将来也并不能大幅度提高精度。

3.2 室内环境

在实验室手持设备来回走，经历了行人、黑暗、低特征区域、镜面反射等，代表了典型的日常生活。所用设备及实验结果如下图所示。

3.3 大规模环境

从室内环境转入室外环境：从实验室座位上出发走过实验室、下楼梯去室外到处走走，返回实验室并爬楼梯，回到座位上，整个轨迹10分钟700多米。不用闭环检测时，起点和终点误差距离是[-5.47,2.76,-0.29]，约0.88%；带闭环检测是误差距离为[-0.032，0.09，-0.07]，可忽略。轨迹及与Google earth对齐结果如下图所示。

大规模实验在HKUST校园进行，约710米长，240米宽，60米高度变化。整个轨迹持续1小时34分钟，共5.62km，可实验系统稳定性和持久性。设置关键帧数据库大小为2000，兼具是实现和足够的闭环信息。每当检测到闭环时，优化所有的姿态图。在主频为3.6Hz的Intel i7-4790 cpu上耗时统计及与Google地图比较结果如下图所示。

英文名称：VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

链接及源码：

https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/support_files/paper/tro_technical_report.pdf

死磕自律，遇见更好的自己；自斩双臂，方能长出强两翼。

败而不弃，潜龙勿用待时机；执着坚毅，飞升上神创奇迹！

关注该公众号，一起创造奇迹。