一种基于结构特征的单目视觉惯性里程计

闫莉萍, 李程伟, 徐柏凯, 夏元清, 肖 波

(1. 北京理工大学自动化学院, 北京 100081; 2. 北京邮电大学人工智能学院, 北京 100876)

0 引 言

同时定位与建图(simultaneous localization and mapping,SLAM)技术发展到如今已有几十年的历史,目前以激光雷达作为主传感器的SLAM技术比较稳定、可靠,仍然是主流的技术方案。最近几年,随着计算机视觉技术的快速发展,SLAM技术越来越多地被应用于家用机器人、无人机、无人驾驶、虚拟现实等领域,基于视觉的SLAM(visual SLAM,VSLAM)技术逐渐开始崭露头角[1]。不过,相比于激光SLAM,VSLAM存在着鲁棒性不足、精度较差等问题,因此将轮速计、惯性测量单元(inertial measurement unit,IMU)、全球导航卫星系统(global navigation satellite system,GNSS)与卫星图像引入到VSLAM系统成为了一个研究热点[2-3]。在此过程中,IMU以其低廉的价格、较轻的重量以及与相机互补的特性得到了广泛关注[4-5]。

相较于单一的视觉里程计(visual odometry,VO),视觉惯性里程计(visual-inertial odometry,VIO)的精度与鲁棒性取得了显著的提升。然而在一些低纹理的人造场景中,传统的仅采用点特征作为视觉约束的VIO系统仍然不能满足要求。与此同时,低纹理场景往往是人造场景,这意味着该类场景一般富含类似家具、墙壁、电器等结构化物体。结构化物体一般包含许多线段、平面等高级几何特征,这些高级几何特征能够提供更强的几何约束,且对光照变化不敏感。因此,近年来,学者们提出了一系列组合点特征与线面等高级几何特征的SLAM系统。比如,Zuo等[6]提出了一个基于点线特征、可以在各种场景下鲁棒运行的双目视觉SLAM系统,He等[7]提出了一个以点线特征为路标的紧耦合单目VIO,赵良玉等[8]提出了一种基于点线特征并融合IMU的双目视觉的惯性SLAM系统。面特征也在近年获得了广泛的应用[9-11]。高级几何特征的引入有效地提升了SLAM系统的鲁棒性和精度,然而相对于只拥有三自由度的点特征,线面特征自由度更高,更难被初始化。此外,线面特征还存在不稳定、容易被遮挡、提取时间长等问题,且与点特征相同,线面特征只能提供局部的约束,这将造成累计误差的存在。

与此同时,低纹理人造场景,如办公室、工厂及地下车库等具有比较规则的结构特征,满足曼哈顿世界由3个相互正交的主导方向组成的假设,由这3个相互正交的主导方向组成的结构特征可作为一个全局路标,以提供全局约束。与此同时,与每个主导方向平行的线段特征在图像平面中交于消失点(vanishing point,VP),因此可以通过对线段特征采用VP提取算法对结构特征进行提取。结构特征的引入有效地消除了旋转累计误差,该方法已经被研究者应用在单目[12]、双目[13]及RGBD(red-green-blue-depth)相机[14]中。

本文提出了一种高效的VP提取方法,在此基础上将其引入到一个基于优化的紧耦合单目VIO中。在提升了VIO系统的精度与鲁棒性的同时保持了系统的实时性。本文的主要贡献如下:(1) 提出了一种快速且准确的VP提取算法;(2) 提出了一种初始化全局结构特征的方法;(3) 提出了一种点特征、线段特征和结构特征紧耦合的单目VIO。

1 相关工作

1.1 视觉惯性SLAM

视觉和IMU定位方案存在一定的互补性:IMU适合计算短时间、快速的运动;而视觉适合计算长时间、慢速的运动。因此,可以利用视觉信息来估计IMU的零偏;同时利用IMU信息为相机提供快速运动时的初始定位。VIO可以被划分为松耦合和紧耦合两类。松耦合[15-16]将IMU定位与视觉的位姿直接进行融合。紧耦合[17-18]融合过程则会影响视觉和IMU的参数(如IMU的零偏和视觉的尺度)。目前更常用的方案为紧耦合,因为紧耦合可以一次性建模所有的运动和测量信息,更容易达到最优的状态估计。视觉融合IMU问题可以分为基于滤波[19-20]和基于优化[21-22]两大类。在计算资源受限、待估量比较简单的情况下,以扩展卡尔曼滤波为代表的滤波方法更加有效,在计算资源充足的情况下,基于优化的方法精度更高。

视觉惯性SLAM通常通过优化视觉重投影残差与IMU测量残差组成的代价函数来获得状态的最优估计。然而,由于IMU的采样频率高,无法将数据都放到状态变量中。为了简化计算,将两帧图像间的IMU测量值进行积分,得到一个两帧图像之间的IMU约束。标准的IMU积分方法与IMU初始状态紧密相关,当初始状态发生改变时,需要重新对所有的IMU测量值进行积分。针对该问题,提出了预积分理论[23]。

1.2 提取VP

VP提取算法主要可分为4类,第1类为穷举法[24-25],此类方法基于VP的正交性穷举出所有的候补VP,并通过线段特征构建极坐标网格对候补VP进行查询验证,最终得到最优的VP估计。第2类为EM法[26],即在E-step中根据当前估计的消失点,找到最佳线段分类,在M-step中基于分类的线段,对消失点重新进行估计,然后反复迭代执行 E-step和 M-step,直至收敛。第3类则基于随机抽样一致(random sample consensus,RANSAC)算法。该类方法通常首先定义一个最小解集(minimum sample size,MSS),然后应用 RANSAC迭代生成候补消失点,直至从中选择出一个最好的解。目前,研究者已提出了1-line[27]、3-line[28]、4-line[29]等多种 MSS 模型。这些模型很好地满足了消失点的正交约束,同时在效率和精度之间取得很好的平衡。第4类则是基于优化的方法[30],该方法旨在寻求VP估计这一数学问题的全局最优解。

1.3 结构SLAM

人造环境中包含着丰富的结构信息,例如平行性、正交性和共面性。这些结构信息能够为SLAM系统提供有效的几何约束,使之获得更加准确和鲁棒的位姿估计。在此基础上,一系列的结构SLAM系统被提出。基于扩展卡尔曼滤波器(extended Kalman filter, EKF) 框架,Camposeco等[12]将VP融入到了一个紧密耦合的VIO以提高定位精度,StructSLAM[31]则引入了与曼哈顿世界主导方向平行的结构线段特征,Zou等[32]采用了由多个曼哈顿世界模型组成的亚特兰大世界模型,使VIO系统对于复杂的人造环境更加鲁棒。基于点线(point and line,PL)-SLAM[33],文献[30]根据VP所蕴含的正交性、平行性和共面性[34]等几何约束,提出了一种后端优化策略,有效地减少了累计误差的存在,提高了PL-SLAM的精度。Kim等[35]则将旋转与平移解耦,首先于RGBD图像上提取结构特征,然后通过追踪结构特征获得相机的旋转量,最后根据追踪到的点特征及旋转量获得相机的平移。

2 框架概述

(1)

图1展示了本文所提出的VIO系统的基本框架。它以图像数据及IMU数据为输入,输出每帧图像对应的位姿估计。其中前端模块负责对点、线与结构特征进行提取和匹配,同时对IMU测量数据进行预积分。后端则通过非线性优化的方法对视觉特征重投影误差与IMU预积分误差组成的代价函数进行优化。接下来,将详细说明系统的每个模块。

图1 基于结构特征的VIO框架Fig.1 VIO framework based on structural features

3 前端

前端负责处理来自传感器的数据,同时决定当前图像帧是否为关键帧。在此过程中,IMU测量值被用来对当前帧的位姿进行一个初始估计,同时通过预积分操作获得当前帧与上一帧的预积分约束,具体细节将于第3.1节中进行阐述。针对图像序列,当新的一帧图像到来时,系统便开始并行地提取点特征和线段特征。其中点特征由KLT (Kanade Lucas Tomasi)稀疏光流法[36]进行跟踪和匹配, 线段特征由边缘绘制直线(edge drawing lines, EDLines)[37]算法进行提取。EDLines算法是一种高效的线段提取算法,其速度是传统线段特征提取(line segment detector, LSD)算法的5～10倍,线段特征的匹配则通过线段带描述子(line band descriptor, LBD)算子+几何验证的方法。LBD算子是一个256维的向量,它能够描述线段特征周围环境的信息。两个相匹配的线段应该符合如下3个准则:(1) 两条线段特征的LBD描述子之间的距离小于某个阈值;(2) 两条线段特征中点之间的距离应小于某个给定值d;(3) 两条线段特征的角度差应小于阈值θ。

其中,后两条准则是基于线段的几何特性给定的,图2展示了由这两条策略剔除的误匹配线段。其中相同颜色的线段代表一对匹配的线。

图2 误匹配线段Fig.2 Mismatched line segment

基于线段特征,对每帧图像的结构特征进行提取和匹配,其具体细节将于第4.2节中进行阐述。关键帧则根据以下3个标准决定[38]:(1) 当前帧和次新帧之间的视差大于某阈值;(2) 当前帧观测到了许多新的路标;(3) 当前帧没有追踪到足够的路标。

这3条准则保证了VIO系统能够鲁棒、实时地运行。

3.1 IMU预积分

IMU包含一个3轴加速度计和一个3轴陀螺仪,可用于测量其本身相对于惯性坐标系的加速度和角速度,其测量值受零偏和噪声影响,具体公式如下:

(2)

(3)

(4)

式中:⊗表示四元数的乘法操作。

(5)

为避免在初始时刻i的IMU状态发生变化时需对测量值重新积分,可将初始状态从积分中分解出来,即:

(6)

(7)

由于IMU测量值是离散且包含噪声的,本文采用中值积分的方法对k和k+1之间的IMU测量值进行积分,预积分测量如下:

(8)

(9)

3.2 结构特征的提取与初始化

结构特征为3个相互正交的主导方向组成且行列式为1的正交基,3个主导方向可通过对该帧图像上的线段特征进行消失点估计获得。本文采用了一种高效鲁棒的算法对结构特征进行提取与匹配,并在此基础上对全局结构特征进行了初始化。

3.2.1 结构特征提取算法

VIO系统初始化成功后,首先基于重力向量,采用穷举+随机采样一致或最小二乘法对图像的结构特征进行提取,主要分为如下3个步骤。

(1) 构建极坐标栅格

极坐标栅格基于等效球构建,如图3所示。

图3 等效球Fig.3 Equivalent sphere

等效球是一个球心位于相机焦点的单位球,其X轴和Y轴与图像平面的x轴和y轴平行,Z轴则从焦点指向像主点。给定像主点(x0,y0)T和焦距f,图像平面上任意一个像素点(x,y)T都能够通过如下公式获得其等效球坐标系下的坐标:

(10)

其经纬度坐标表示为

(11)

式中:φ∈[0,π/2],λ∈[0,2π]。

设定极坐标栅格G的大小为90×360,精度为1°。对i=1,2,3,…,90,j=1,2,3,…,360,有G(i,j)的初始值为0。

选取图像上任意一对线段特征l1,l2求其交点p,基于式(10)、式(11)求其对应的经纬度坐标(φ,λ),进而将其对应的栅格进行更新:

G(φdeg,λdeg)=G(φdeg,λdeg)+‖l1‖×‖l2‖×sin(2θ)

(12)

式中:‖l‖为对应线段长度,θ为两个线段的夹角,最后对得到的极坐标栅格应用3×3的高斯滤波器。极坐标栅格构建过程是记录每对线段对每个单元响应的过程。

(2) VP假设生成与验证

如图4(a)所示,将初始化得到的重力向量投影到图像平面上,获得第一个VP的初始估计VPinit,然后计算所有线段特征与该VP向量的夹角,夹角小于4°的线段属于该VP对应的结构线段聚类L1。

若采用穷举+随机采样一致的VP提取算法,假定图像中有N个线段,其中对应于灭点VP1,VP2,VP3的线段数量分别为n1,n2,n3,则对于任意两条线段,其对应于同一个灭点的概率P为

(13)

假设所有线段中异常线段的数量约占1/2,那么则有:

(14)

(15)

设置信系数η为0.999 9,则在该置信系数下,需要迭代的最少次数为

nit=ln(1-η)/ln(1-P)=105

(16)

因此,在50%的线段异常比例以及0.999 9的置信系数下,至少需要105次迭代才能得到至少一个正确的VP1。

若L1包含的线段特征大于4时,还可通过构建最小二乘问题来对VPinit进行优化,获得第一个VP的假设VP1:

(17)

式中:S∈R3×n,n为L1包含的线段特征的个数;K为相机内参;ljstart,ljend分别对应第j条线段起点和终点的归一化坐标。然后,如图4(b)所示,基于消失点的正交性,给定第一个VP,即VP1=(X1,Y1,Z1)T后,第二个VP(VP2)必然位于其超圆上。因此,以精度1°对该超圆进行均匀采样获得VP2的360个假设。 其中第i个假设的经度λ=i×2π/360,纬度φ则通过如下两个约束求解:

图4 VP提取过程Fig.4 VP extraction process

(18)

X1×X2+Y1×Y2+Z1×Z2=0

(19)

最后,如图4(c)所示,VP3由VP1和VP2叉乘可得。

将步骤(2)中360个消失点假设VP1,VP2,VP3用极坐标表示(λ1,φ1),(λ2,φ2),(λ3,φ3),然后基于极坐标栅格查询每个VP假设的线段响应:

T=G(φ1,λ1)+G(φ2,λ2)+G(φ3,λ3)

(20)

线段响应最大的VP假设为最优估计,得到当前帧消失点估计后,对当前帧的线段特征进行分类。得到与VP1,VP2,VP3向量夹角小于4°的结构线段特征集合L1,L2,L3。

(3) 基于线段匹配关系计算VP

基于下一帧与当前帧线段匹配关系,获得与下一帧匹配成功的结构线段集合,并按照包含结构线段特征的数量对其由大到小进行排序,得到L1p,L2p,L3p。若L2p包含的结构线数量小于6,则继续采用式(1)、式(2)所述算法对下一帧的VP进行提取。否则,便可以基于匹配到的结构线段集合对下一帧的结构特征进行提取。即基于式(13)得到结构线段集合L1p,L2p所对应的消失点估计值VP1init,VP2init,然后将两者叉乘获得VP3init。最后,对VP1init,VP2init,VP3init进行施密特正交化,得到该帧的VP向量VP1,VP2,VP3,并在此基础上获得该帧的结构线段集合,用于对后续图像帧进行处理。

基于当前帧与次新帧结构线段的匹配数量分别采取穷举+RANSAC或最小二乘求解的方式对结构特征进行提取,有效地保证了在引入结构特征这一前提下VIO系统的实时性。图5(a)和图5(b)分别展示了两种方式提取的结构特征对应的结构线。

图5 不同算法提取的结构特征对应的结构线Fig.5 Structural lines corresponding to structural features extracted by different algorithms

3.2.2 初始化全局结构特征

VIO系统初始化成功后,将滑窗内的每帧图像对应的结构特征MFi转化到世界坐标系下。

(21)

(22)

距离小于0.1的结构特征为内点。若内点比例大于0.6,则对所有内点进行slerp插值得到全局结构特征MFW,否则,认定初始化失败,滑动窗口移动,重新按照上述流程对全局结构特征进行初始化。

4 后端

后端负责对前端获得的初始状态进行优化,在后端优化过程中,首先通过三角化获得点线特征于3D空间的初始值,然后对包含点线特征的重投影误差、IMU预积分误差、结构特征所重投影误差的代价函数进行优化,进而获得状态向量的估计。优化结束后,基于优化结果对异点进行剔除,其中包含逆深度为负的点特征,重投影误差过大的线特征和结构特征。

4.1 紧耦合非线性优化

当全局结构特征初始化成功后,时刻i滑动窗口的状态向量定义如下:

(23)

式中:xi包含滑窗第i帧图像对应的IMU平移、旋转、速度、加速度计零偏与角速度计零偏;λk为第k个路标点的逆深度;Ol为第l个路标线的正交表示;MFW为结构特征于世界坐标系下的矩阵表示。下标n,m,o则分别是IMU、路标点、路标线的起始索引;N为滑窗内关键帧数量;M,O分别为滑窗内路标点和路标线的数量。

通过优化如下代价函数对上述状态进行优化:

(24)

本文采用LM(levenberg-marquardt)算法对该优化问题进行求解,即通过如下公式迭代更新初始值来获得最佳估计:

(25)

式中:⊕操作符用于表示更新操作。

4.2 IMU测量误差

(26)

误差对应的雅克比矩阵[34]为

(27)

(28)

(29)

(30)

4.3 点特征测量误差

(31)

式中:

(32)

(33)

(34)

(35)

4.4 线特征测量误差

给定一个普吕克坐标描述的3D线段L=[n,d]T,其中n∈R3为线段与世界坐标系原点组成平面的法向量,d∈R3为线段的方向向量,其投影到相机平面的投影方程为

(36)

(37)

采用普吕克坐标描述的线段特征具有6个自由度,是一种过参数的表述形式,无法直接在后端优化过程中使用。因此,本文在后端优化中采用了具有4个自由度的正交形式描述线段特征[39]。普吕克坐标与正交形式的转换公式如下:

(38)

式中:U为一旋转矩阵,具有3个自由度,W则只有一个自由度。3D线段L在相机i归一化平面上的重投影误差定义为

(39)

d(s,l)为投影线段两个端点与观测线段的距离:

(40)

误差对应的雅克比矩阵为

(41)

(42)

式中:ui为U的第i列,w1=cosφ,w2=sinφ,K为内参矩阵,同时有:

(43)

(44)

4.5 结构特征测量误差

由于结构特征是一个由3个相互正交的主导方向组成且行列式为1的正交基,因此它可以用来表示一个旋转量。当全局结构特征初始化成功后,通过引入绝对旋转残差项,可有效减少旋转累计误差。绝对旋转残差定义如下:

(45)

其中MFci为结构特征于第i帧图像的观测。误差对应的雅克比矩阵为

(46)

对于四元数q=[qw,qx,qyqz]T有:

(47)

(48)

5 实验结果

在公开数据集EuRoc[40]上对本文所提系统的精度和实时性进行了测试。该数据集由无人机采集,记录了传感器在一个工厂和两个不同房间内采集到的数据,其中包含了双目图像、IMU测量,以及通过光学运动捕捉系统获得的真实值。本文采用了左眼相机拍摄的图像和IMU测量,所有的实验都运行在一台处理器为i5-9400(2.90 GHz)、内存为8GB、系统为ROS Kinetic的电脑上。

5.1 精度评估

由于管道、机械、家具等物体的存在,EuRoc数据集拥有丰富的结构线段,基于这些结构线段特征提取的结构特征,可有效减少旋转的累积误差。图5、图6分别展示了在工厂和房间内提取的结构特征。

图6 V1_01_easy序列某帧图像提取的结构特征Fig.6 Sequence V1_01_easy structural features extracted from a frame image

本节将本文所提快速曼哈顿框架VIO(fast Manhattan frame VIO,FMF-VIO)与PL-VIO[5]和没有回环检测的单目视觉惯性状态估计器(monocular visual-inertial system, VINS-Mono)[38]、开放式视觉惯性系统(open visual-inertial system, OpenVINS)[41]进行了对比;同时为了证明本文所提VP提取算法的有效性,实验还和采用文献[24]提出的VP检测算法的VIO系统的曼哈顿框架(Manhattan frame, MF)VIO进行了对比。其中PL-VIO和VINS-Mono均采用作者提供的默认参数。实验结果如图7、图8和表1所示。

图7 MH_02_easy数据集上PL-VIO与FMF-VIO的误差Fig.7 Error between MF-VIO and FMF-VIO on dataset MH_02_easy

图8 MH_04_difficult数据集上MF-VIO与FMF-VIO的误差Fig.8 Error between MF-VIO and FMF-VIO on dataset MH_04_difficult

表1 平移和旋转的均方根误差Table 1 Root mean square error of piston and rotation

表1列出了EuRoc数据集下各种系统的平移和旋转的均方根误差,并加粗和标记了每个序列效果最好的两个结果。由于OpenVINS在部分数据集下会出现跟踪失败的现象,故在表1中用斜杠表示。可以看出,与不包含线段特征与结构特征的OpenVINS[41]相比,本文提出的FMF-VIO系统在精度相近的情况下具有更好的鲁棒性。与包含线段特征但不包含结构特征的PL-VIO相比,FMF-VIO在大多数序列上的旋转精度和平移精度均有所提升,只有在MH_01_easy数据集上的平移精度有所下降,这是由记录该序列时无人机存在长时间的缓慢移动进而造成结构特征测量无法有效利用造成的。与同样利用了结构特征的MF-VIO系统相比,本文提出的FMF-VIO系统与之达到了相似的精度,但显著提高了系统的实时性(关于实时性的具体分析见下一小节)。

图7直观展示了PL-VIO和FMF-VIO两种算法在MH_02_easy序列下旋转与平移的误差情况,其中红色曲线代表FMF-VIO算法,蓝色曲线代表PL-VIO算法。从图7可以看出,本文提出的FMF-VIO算法具有更小的平移和旋转误差。这与表1得出的结论一致。

图8展示了MF-VIO和FMF-VIO在MH_04_difficult数据集下的旋转误差和平移误差,MF-VIO和FMF-VIO分别采用了不同结构特征提取算法。其中红色和蓝色曲线分别表示FMF-VIO和MF-VIO算法的误差情况。从图8可以看出,本文提出的FMF-VIO系统得到的旋转误差和平移误差与MF-VIO得到的误差相当,这也与表1中的数据一致。

综上所述,本节的实验结果表明,本文提出的FMF-VIO的精度明显优于PL-VIO,与OpenVINS和MF-VIO相当,且比OpenVINS具有更好的鲁棒性。

5.2 实时性评估

SLAM是一种对实时性要求很高的系统,结构特征的引入在提升系统旋转精度的同时,也对系统的实时性产生了影响。本节将在EuRoc数据集上对本文提出的基于重力向量和最小二乘法的曼哈顿框架下的VP提取算法(VP extraction algorithm based on gravity vector and least squares in the Manhattan frame,G2MF)与仅基于重力向量的曼哈顿框架下的VP提取算法(VP extraction algorithm based on gravity vector in the Manhattan frame,GMF)和文献[24]提出的基于曼哈顿框架的VP提取算法(VP extraction algorithm based on Manhattan frame,MF)的实时性进行了对比分析。实验结果如表2所示。表2列出了本文算法和对比算法在每一个序列中对每帧图像提取结构特征的平均耗时。

表2 VP提取算法的实时性展示Table 2 Real time display of VP extraction algorithm ms

由表2可以看出,本文提出的算法G2MF处理单帧图像的平均耗时要远远小于文献[24]所提MF算法。与GMF算法相比,本文所提出的算法实时性也有所提升。

本文还在 MH_05_difficult数据集上评估了运行PL-VIO与FMF-VIO的平均执行时间,该数据集是一个典型的人造场景的图像序列,具有丰富的结构线段且相机运动较为剧烈。表3展示了两个系统在不同单元的执行时间。由表3可以看出,通过将LSD算法替换为EDLines,同时采用更为鲁棒、高效的结构特征提取算法,FMF-VIO在引入结构特征的前提下,实时性依然得到了有效的提升。在Euroc数据集上的实时运行结果显示,FMF-VIO的速度是PL-VIO的两倍。

表3 PL-VIO 与FMF-VIO的不同单元每帧平均执行时间Table 3 Average execution time per frame of PL-VIO and FMF-VIO different units ms

综上所述,本节的实验结果表明,本文提出的FMF-VIO系统在实时性方面,均明显优于PL-VIO和MF-VIO。

综合第5.1节和第5.2节两部分的实验结果可知,本文提出的FMF-VIO方法是有效的。

6 结束语

为了提升VIO的精度,本文于线段中提取结构特征,并基于结构特征的正交特性将其引入到VIO中,有效地提升了VIO的精度。与此同时,为了保证VIO系统的实时性,本文还将线段提取算法由LSD替换为EDLines,并基于重力向量与结构线段的相邻帧匹配关系提出了一种鲁棒且高效的结构特征提取算法。在EuRoc数据集上对本文提出的VIO进行了测试,结果表明本文提出的系统在保持高精度的同时,可以实时地运行。

在未来,将尝试通过神经网络引入语义特征,进一步提高SLAM的鲁棒性和潜在应用价值。