基于小滑窗优化的视觉惯性SLAM研究

李金春，张志呈，丁彬渊，曹雄，刘兴禹

（中国计量大学，浙江杭州，310018）

0 引言

港科技大学飞行机器人实验室（沈邵劫团队）在2018年开源了VINS-Mono算法。它使用一个单目相机+惯性测量单元实现了紧耦合的视觉和惯性联合状态估计。2019年该团队开源VINS-Fusion是VINS-Mono的扩展，它支持多种视觉惯性传感器类型，包括单目+IMU、双目+IMU、纯双目、双目+IMU+GPS四个模式。VINS-Fusion是一个完整的基于优化算法的SLAM系统，也是一个经典且适合学习的视觉惯性SLAM算法。

VINS-Mobile是在VINS-Mono基础上开发用于增强现实中定位功能并能在手机端运行的开源算法，并针对手机端算力不足进行了优化，并提出了一种轻量级的紧密耦合融合方法，将环路信息集成在滑动窗口单目VINS估计器中[4]。使用该方法可以提高VINS-Mobile的定位数据更新频率。

我们在项目中选择了双目+IMU这一模式进行研究。双目相机的距离估计是比较左右眼的图像获得的，并不依赖其他传感设备，所以它既可以应用在室内，亦可应用于室外[1]。相较于单目相机，双目相机可以做到更高的精度，同时避免尺度不确定性。另外与IMU模块结合不仅可以弥补纯视觉导航系统的缺陷，而且可以提高特征点的匹配速度并增强算法的鲁棒性[2]。

在项目中，由于设备运行的任务较多，加之控制成本，我们的设备存在性能不足情况，在了解到VINS-Mobile的轻量级优化方案后，我们尝试对VINS-Fusion进行改进，尽管相对位姿误差有所增加，但成功降低CPU使用率，我们主要做了以下工作：

在VINS-Fusion中实现了小滑窗PnP+IMU优化。

对改进后的VINS-Fusion使用EuRoc数据集进行测试。

1 VINS-Fusion的后端优化

VINS-Fusion可分为四个模块：初始化，前端数据处理，后端优化，回环检测。VINS-fusion的前端使用特征点法，用Harris提取特征点，利用LK光流法进行特征点追踪，同时对IMU数据进行IMU预积分。运行时前端获取的数据会传入后端优化模块，我们主要研究VINS-Fusion的后端优化模块，它主要包括非线性优化、边缘化两个操作。

1.1 非线性优化

VINS采用紧耦合方式对图像和IMU进行融合，紧耦合直接对测量数据进行融合具有精度高、复杂度高的特点。前端获得图像和IMU数据传递到后端作为状态变量。根据针孔相机模型建立具体参数化后的视觉观测模型，并通过 IMU 各变量的运动学模型推导预积分的计算方法，建立参数化后的 IMU 观测模型[6]，后端将状态变量都视作优化变量，对它们进行非线性优化，构建非线性最小二乘，并使用高斯-牛顿法优化，最后将优化的结果通过ROS的话题发布。

需要优化的状态变量包括滑动窗口内的n+1帧所有相机的状态xk（包括位置p、朝向q、速度v、加速度计bias 和陀螺仪bias）、相机到IMU的外参、m+1 个3D点的逆深度λ：

VINS-Fusion在进行非线性优化时使用惯性光束法平差公式，最小化所有测量残差的先验和马氏范数之和，以获得最大后验估计[5]，目标函数，式（1）：

式(1)中三个误差项分别是边缘化的先验信息、IMU的测量残差和视觉的重投影残差。

IMU测量残差是两帧数据之间的 PVQ和 bias变化量的差。视觉残差是重投影误差，对于第l个路标点P，将P从第一次观看到它的第i个相机坐标系，转换到当前的第j个相机坐标系下的像素坐标[2]这一重投影过程所带来的误差。程序中使用非线性优化库Ceres进行优化求解。

1.2 边缘化

在后端优化中，不仅需要优化状态变量，还需要将过去生成的位姿信息和即将产生的位姿信息相结合，对整个运动轨迹进行优化[2]。VINS-Fusion使用滑动窗口法实现，并增加了边缘化的操作。运行时边缘化掉滑动窗口中的最老帧或次新帧，从而达到不再计算这一帧的位姿或相关的路标点的目的，但又保留了其对窗口内其他帧的约束关系。这一过程使用Schur消元，如下所示：

其中Xb是需要保留部分，Xa是需要消去的项，整理得到式(4):

Sch ur消元将滑窗中边缘化的帧转化为滑窗内待优化变量的先验分布，既可以减少数据量，又不损失约束信息。整个过程如图 1 所示，图中Xp代表路标点，Xm代表相机位姿，当边缘化Xp1时，与Xp1相连的Xp2，Xm1，Xm2和Xm3之间就会建立新的约束[2]。

图1 边缘化过程[2]

通过边缘化这一操作，VINS的状态变量的数目不仅得到控制，而且它们的约束关系得到优化。

但运行时，后端优化的状态变量较多，求解Schur时的矩阵维数较高，频繁的非线性优化及边缘化操作对性能有一定的要求，当设备性能不足时会影响运行速度，过多地减少后端滑动窗口数量又会影响运行时的稳定性和精度。

2 小滑窗PnP+IMU优化

2.1 Vins_Mobile程序分析

为控制后端滑窗优化的计算量，通常是每隔几帧（比如3帧）选一帧送到后端处理。为计算被舍弃的帧的位姿，在VINS-Mobile版本中，加入一种轻量级的基于小滑窗的PnP+IMU联合优化方案[2]。VINS-Fusion在程序中选择每两帧送一帧数据至后端的方案，由于两帧之间的图像相似且相机帧率较高，对系统影响较小，但继续增加发送间隔会造成定位数据更新滞后。

VINS-Mobile则采用了一种轻量级优化方案，以10Hz的频率执行角点检测，以30Hz的频率执行特征跟踪[4]，但VINS-Mobile的论文中没有具体详细说明优化方案，我们阅读VINS-Mobile的源码后，在VINS-Fusion中移植和实现这一优化方案。

小滑窗PnP优化在VINS-Moblie源码中对应的代码主要位于vins_pnp.cpp文件中，通过vinsPnP类封装。不过在其他文件如feature_traker.cpp中也有一部分涉及小滑窗PnP的代码。

FeatureTracker::solveVinsPnP()寻找与最新帧中与滑窗中相同的路标点，计算其在归一化相机坐标系中的点，将结果通过feature_msg传递[2]。

FeatureTracker::readImade()读取图片，进行特征点检测和追踪，同时函数调用FeatureTracker::solveVinsPnP()；

vinsPnP::processImage()对小滑窗内的图片进行优化，该函数的一个参数就是feature_msg；

vinsPnP：：updateFeatures()更新小滑窗内与最新帧有共视关系的3D点坐标；

vinsPnP::solve_ceres()优化小滑窗内的PVQ，但不优化路标点；

vinsPnP::slideWindow()小滑窗PnP内的滑窗操作；

2.2 算法流程

小滑窗PnP优化运行时每间隔3帧选一帧送至后端且间隔的3帧只追踪特征点，从而减少后端和前端的计算量，但是增加小滑窗PnP优化模块，提高实时性。顾名思义也拥有一个滑窗用于储存前端获得的最新数据，并结合后端优化结果，使用IMU的帧间约束（帧与帧之间的IMU预积分）和每一帧的2D-3D观测（每一帧的 PnP）的视觉重投影误差约束，构成了PnP+IMU优化。

后端完成优化后，将优化好的路标点3D点坐标、相机的状态、加速度计bias 和陀螺仪bias传递给小滑窗PnP优化模块。小滑窗PnP优化模块更新与之有共视关系的3D点坐标、相同帧的位姿及其它参数，并将它们作为不优化的常量[4]。小滑窗PnP+IMU优化算法示意图如图2所示。

图2 小滑窗PnP+IMU优化算法示意图

计算没有后端优化结果的位姿时，将后端优化结果作为先验信息对小滑窗PnP优化模块内相机的位置、朝向、速度进行优化，不优化路标点。如图2中所示，小滑窗的第四帧（从左向右数）加入后端的优化结果，信赖并固定住第四帧的位置、朝向、速度，同时固定住此后小滑窗内的bias，并作为先验信息优化小滑窗内其他帧。

小滑窗PnP优化模块仅优化相机位姿不优化路标点化减少优化变量，从而降低计算量。另一方面没有对所有图片进行特征提取，也减少来自特征点检测中的部分计算量。

3 实现与测试

3.1 实现

我们仿照VINS-Mobile，将小滑窗PnP优化模块添加到视觉前端与后端优化之间。新的流程简图如图3所示。

图3 加入小滑窗PnP+IMU优化后的程序流程图

由于VINS-Fusion的默认检测的特征点数为150个远多于VINS-Mobile的60个，我们将小滑窗PnP优化模块的窗口大小设置为6，并每4帧送一帧至后端，后端滑动窗口的大小为10。VINS-Fusion的运行结果由小滑窗PnP优化模块发布，后端优化模块向其更新优化数据，作为小滑窗的先验信息。

初始化与原VINS-fusion一致，系统方式以每两帧送一帧至后端，避免受到小滑窗PnP优化模块的影响造成初始化时间增加和发生初始化异常。

其中，优化结果的更新通过全局变量在小滑窗PnP优化模块与后端优化中传递，并使用互斥锁维护数据的安全。尽管后端优化和小滑窗PnP优化模块所需的图像与IMU数据有重叠，但各自需要的时刻不一致且二者数据重叠部分较少（如图2所示），并且小滑窗PnP模块的部分数据需要后端优化结果作为更新，所以我们将两者所需的图像和IMU数据单独维护。两者的数据单独维护，虽然会增加内存的使用，但小滑窗内的数据较少，并且两者在运行时更加独立、数据更安全，编写多线程及后期维护时更加容易，运行时，数据访问不易冲突尽量避免 “死锁”的发生。

3.2 测试平台

测试的处理器为i5-8265U 1.60GHz，内存8GB。数据集为EuRoc中的MH_01～MH_05。配置文件euroc_stereo_i imu_config.ymal的相关参数为默认参数。

由于使用小滑窗优化后，定位数据发布频率与数据集中图像发布率相同为20Hz，所以我们增加一组将图像数据全部送入后端的测试作为相同频率的对照。测试均未开启回环检测。

3.3 测试结果

我们分别对小滑窗PnP优化模块、默认每两帧送一帧（10Hz）、全部送入后端(20Hz)三种方案运行时间内的内存使用率和CPU使用率进行的测试。数据通过编写Linux的Bash脚本，调用指令“top -n 400 0.5|grep vins_node ＞CpuMem.csv”获取，再通过WPS整理数据。测试过程中，多种因素如CPU运行产生温度、数据集的时间长度及难易等使数据有所浮动。数据为多次测量平均值，如表1所示。

表1 三者的CPU和内存使用率

我们使用轨迹评估工具evo对三者的绝对位姿误差（APE）和相对位姿误差（RPE）进行了评估，以标准差作为评估的指标，如表2所示。

表2 三者的APE和RPE指标

3.4 测试结果分析

尽管CPU的使用率有所浮动，但总体上使用小滑窗PnP优化模块可以使用更少的CPU来获得较高频率的定位数据输出，相应的内存有所增加，是一种以空间换取CPU性能的策略。

同时绝对位姿误差增加较少，对整条轨迹的全局一致性影响较弱，但反映局部准确性的相对位姿误差明显增加，这是因为小滑窗PnP优化模块使用后端数据优化局部，整体位姿受影响较小，但局部有一定程度的影响。虽然局部位姿有所影响，但其计算得到的轨迹与真实轨迹的误差可以接受，如图4所示。

图4 MH_03数据集的轨迹叠加图

尽管改进后降低对设备算力的消耗，但对设备性能仍有要求，我们在jetson nano上运行GPU版的VINS-Fusion,并且使用小滑窗PnP+IMU联合优化方案，效果并不理想。

4 结束语

仅凭一个模块想同时提高性能和精度是很难的事，有失有得是常有的。这是我们几人第一次参与此类项目，除了学到了不少知识，还认识到细节对结果不少的影响，实际运行时还需要考虑设备的参数如图像与IMU数据的时间戳对齐，不然程序容易发生异常。不过这些还只算刚刚入门，定有不少不足之处，今后我们会不断改进，为将来参与真正的科研做好准备。

同时也要感谢崔华坤老师的《vins论文推导及代码解析》给我们提供很多思路，对我们研究和学习VINS有很大的帮助。

当然，我们也能从测试中看出VINS的前端消耗量较多的算力，尽管VINS使用光流法实现特征点的匹配，但特征点的提取仍占据大多数的计算时间。最后我们用《视觉SLAM十四讲》中 的“如何提高特征提取、匹配算法的速度，将是特征点方法的一个重要的主题[1]”结束本文。