改进位姿估计环节的ORB-SLAM稠密建图算法

刘畅, 党淑雯, 陈丽

(上海工程技术大学航空运输学院, 上海 201600)

同时定位与地图构建(simultaneous localization and mapping, SLAM)[1-3]是指在未知环境下机器人根据传感器采集的数据实时构建目标环境地图的同时进行定位的新技术[4]。视觉SLAM(visual SALM,VSLAM)采用视觉传感器为数据源,成本比雷达等其他SLAM技术更低,是当前SLAM研究的热点方向之一[5]。

现有研究中,Davison[6]率先设计出使用单目相机的实时单目VSLAM系统;Georg等[7]提出了并行追踪与建图(parallel tracking and mapping, PTAM)方法,其实现了并行化跟踪与建图过程和非线性优化,对传统滤波器(如卡尔曼滤波[8]、粒子滤波[9]和组合滤波[10])作为后端的算法方案进行了改善。文献[11]研究了以PTAM作为基础的支持单目、双目、RGBD三种相机的SLAM系统,而ORB-SLAM2[12]是一种使用ORB(oriented fast and rotated brief)贯穿所有阶段的轻量级ORB-SLAM[13]定位方法,其将整个SLAM过程划分为三个线程(定位、局部建图和回环检测),但由于特征追踪存在不断积累的误差,使得后面帧和实际的位姿之间的距离不断增加,进而降低系统整体精度。在SLAM与曼哈顿主方向计算的应用方面,Zhou等[14]利用连续帧间的曼哈顿主方向信息来匹配相对位姿并增量式地构建地图,有效利用室内场景下的结构特点辅助视觉里程计应用,揭示了曼哈顿世界(Manhattan word,MW)假说对室内SLAM及定位的辅助价值。Yunus等[15]提出的ManhattanSLAM是一种在MW和非MW环境中提供鲁棒跟踪的新RGB-D SLAM系统,利用室内场景中的结构信息,在CPU上进行精确跟踪和高效密集映射。

因此,现从提高位姿估计精度和生成易进行二次加工的地图这两个角度出发,在位姿估计阶段提出一种融合迭代最近点拟合(iterative closest point, ICP)算法和MW假说的改进位姿估计方法并在建图方面加入稠密建图线程,从而实现精准定位及稠密建图。此外,利用慕尼黑工业大学(technische universität münchen, TUM)数据集进行仿真实验,并通过与ORB-SLAM2进行对比,验证所提方法对提高位姿估计精度和稠密建图的有效性。

1 系统概述

ORB-SLAM2系统包括追踪线程、局部建图线程和回环检测线程,能够实现地图重用、回环检测以及重新定位等。本方法在ORB-SLAM2的基础上进行改进,并增加了稠密建图线程,整体框架如图1所示。

BA(bundle adjustment)为光束法平差图1 算法流程图Fig.1 Algorithm flowchart

1.1 追踪线程

在跟踪线程,由于本方法在ORB-SLAM2的框架下二次开发,因此仍采用恒速运动模型计算初始位姿,之后再通过特征匹配和结构约束完成位姿优化。首先通过ORB特征点法、最小显著性差异(least-significant difference, LSD)算法[16]和聚集层次聚类(agglomerative hierarchical clustering, AHC)方法进行点、线、面特征的提取,其中点、线特征跟上一帧匹配,面特征在全局地图中匹配。然后采用基于surfel的稠密建图策略将图像划分为非平面与平面区域,其中非平面采用ICP算法计算位姿,平面区域则通过面与面之间的正交关系确定曼哈顿世界从而使用不同的位姿估计策略,曼哈顿世界场景通过位姿解耦实现基于曼哈顿帧观测的无漂移旋转估计,而曼哈顿世界场景下的平移以及非曼哈顿世界场景位姿采用追踪到的点、线、面特征进行估计和优化。最后判断新关键帧是否插入。

1.1.1 特征检测和匹配

(1)点特征提取与匹配。点特征选择与传统ORB-SLAM2算法相同的ORB特征。ORB特征基于FAST关键点检测器和BRIEF描述器,使用P=(X,Y,Z)表示三维坐标,而二维坐标用Pobs=(u,v)表示。通过将三维点投射到图像上并利用各自描述符之间的汉明距离找到最接近的观测值来确定匹配度。

(2)线特征提取与匹配。改进算法使用LSD检测器和LBD(line banddiscriptor)描述符检测和描述线特征。三维线和二维观测端点都使用(pl,start,pl,end)表示,同时观测的规范化二维线函数表达式为

(1)

为了确定三维线和二维观测之间的匹配,三维线的两个端点被单独投影,并使用LBD描述符进行匹配。

(3)面特征提取与匹配。面特征使用AHC方法提取,该方法为每个平面提供平面系数(n,d)和点云中的支持点。针对位姿优化,使用平面的最小表示法表示为

(2)

式(2)中:q(π)为用于优化的平面π的最小化参数;Φ、Ψ为平面法线的方位角和仰角。平面匹配是通过比较法线间的角度和平面的点面距离来确定的。

(3)

SVD(RCiMk)=UDVT

(4)

(5)

式中:U、V为正交矩阵;D为奇异值组成的对角矩阵。

此外,建立曼哈顿地图G存储检测到的曼哈顿帧,并且地图G与当前帧Fi检测到的曼哈顿帧通过匹配估计无漂移旋转矩阵,而Fi与具有相同曼哈顿帧的帧Fj的匹配通过检查组成平面与地图平面之间的匹配关系实现。

1.1.2 改进位姿估计方法

针对实现相邻两帧图像间的位姿估计,设计了融合ICP算法和曼哈顿世界假说的改进位姿估计方法。由于在非平面区域求得3D点的相机坐标即可将3D-2D点转换为3D-3D对应点,因此采用3D-3D匹配点对的ICP优化。而关于平面区域,利用室内的曼哈顿结构约束实现无漂移旋转估计,借助恰当的曼哈顿帧检测方法生成的曼哈顿地图来追踪曼哈顿帧的观测值,且在不满足曼哈顿世界假设的环境中实现曼哈顿帧和特征追踪之间位姿估计的无缝切换。

1)基于ICP算法的位姿估计方法[17]

ICP算法采用迭代优化方式,不断调整相机的位姿,从而获得最小匹配点间的累积距离,并计算出相机运动的最优旋转矩阵R和平移矩阵t。点对距离平方和minF(R,t)数学模型为

(6)

式(6)中:Pi(i=1,2,…,m)为理论点集;Qi(i=1,2,…,m)为实测点集,步骤如下。

步骤1在Qi寻找Pi对应距离最小点点集Pik(k为迭代次数),即

min=‖Qik-Pik‖

(7)

步骤2根据式(1)采用奇异值分解(SVD)方法计算R和t。

(1)计算Qi、Pi的质心Qc、Pc:

(8)

(2)Pi、Qi去中心化获得新点集Pn、Qn:

(9)

(3)计算矩阵H:

(10)

(4) SVD分解:

H=UΣVT

(11)

式(11)中:U、V为正交矩阵;Σ为对角矩阵。

(5)计算变换旋转矩阵R0和平移矩阵t0:

(12)

步骤3Pi旋转平移变换:

(13)

步骤4计算平均距离dk+1

(14)

步骤5重复步骤1进行迭代,直到dk+1比τ小或次数不再低于预定阈值。

2)基于曼哈顿世界假说的位姿估计方法

基于曼哈顿世界假说的室内SLAM算法[18-19]通过检测曼哈顿帧Mk确定当前场景是否为曼哈顿世界场景,不同场景使用不同的位姿估计方法。

相机位姿ξCW由世界坐标W到摄像机坐标C的旋转矩阵RCW和平移向量tCW组成。如果曼哈顿世界假说未成立,即没有检测到曼哈顿帧,则通过跟踪特征估计完整的6D位姿。在曼哈顿世界场景,旋转和平移分别被解耦合估计。

(1)非曼哈顿世界场景下的估计。在非曼哈顿世界场景中,6D相机位姿可以跟踪点、线和平面估计。将检测特征与其对应匹配3D特征之间的重投影误差ep、el和eπ定义为

(15)

此外每个观测平面的平行平面和垂直平面匹配,并作为结构约束eπ‖和eπ⊥被添加到总能量函数:

(16)

式(16)中:nC和nW为观测平面法线和匹配平面坐标;R⊥是90°旋转矩阵;qn(·)为用于优化的平面的最小化参数,qn(π)=(Φ,Ψ)。假设一个高斯噪声模型并结合所有误差得到最终能量函数,利用列文伯格-马夸尔特(Levenberg-Marquardt, L-M)算法进行优化得到最佳位姿。

(2)曼哈顿世界场景下的估计。在结构曼哈顿世界场景中,解耦位姿估计并使用新方法估计无漂移旋转矩阵,而特征跟踪用于平移向量估计。旋转矩阵估计需要首先检测场景中的所有曼哈顿帧,帧Fi中每个检测到的曼哈顿帧Mk使用相应旋转矩阵RCiMk表示,然后如果在地图中检测到Mk,则可从地图中得到相应首次观测到Mk的帧Fj。以Fj作为参考帧,可求得Fi和Fj间的相对旋转矩阵RWCi为

(17)

转置相对旋转矩阵即可获得目标旋转矩阵。此时若需要相应的平移向量确定完整的相机位姿,可以通过求解最小化误差函数获得。固定旋转矩阵只在优化过程中更新平移向量。

1.2 局部地图构建线程

在局部建图线程,将新的关键帧作为新节点加入共视图,并且更新与关键帧节点相连的边和关键帧的生长树,并放入地图。首先仅保留能够满足约束测试的地图节点,避免建图受到外点影响。然后对关键帧内ORB特征点进行正向景深,视差,反投影误差和尺度一致性进行检测,当检测无误时,产生新的地图点。此外每个特征反射的光束通过调整自身的空间位置和相机姿态汇聚到相机光心,即光束法平差[23](bundle adjustment, BA)BA过程的本质是优化模型,而局部BA会同时优化关键帧及地图点,并删除冗余关键帧。

1.3 回环检测线程

在闭环检测线程,首先计算添加该线程的候选关键帧与当前关键帧的相似度,若有回环则融合重复地图点。然后将新边嵌入共视图使闭环连通。最后将关键帧位姿与地图点坐标替换为优化的全局位姿与地图点。ORB-SLAM2作为常用的视觉SLAM系统之一,虽然性能强,但其只能构建三维稀疏特征地图,若使机器人能够完美完成导航、避障以及路径规划等任务,必须改进ORB-SLAM2系统使其能够实现三维稠密建图。

1.4 稠密建图线程

本文中改进的 ORB-SLAM2系统框架图如图1所示,图中虚线框内为ORB-SLAM2系统新增的稠密建图线程。

在跟踪线程选择符合关键帧要求的RGB图像,然后对其进行特征点二次采样,该采样仅进行ORB点特征提取而不匹配,以用于稠密建图。相机位姿的初始化由局部BA负责进行,并由回环检测和全局BA进行更新,从而实现更精准的点云拼接。稠密建图线程结构如图2所示。

图2 稠密建图线程结构图Fig.2 Dense mapping thread structure diagram

稠密建图线程的工作过程包括输入数据、位姿估计和建图三部分。首先输入建图所需的环境源数据,包括RGB图像、深度图和ORB 特征点;然后利用ORB-SLAM2为符合建图需求的关键帧提供位姿估计;最后接收图像数据和位姿信息,并完成三维点云拼接以及点云数据管理。

2 实验结果及算法对比分析

2.1 实验环境

为了验证所提出方法的有效性,本文的仿真实验采用TUM数据集中的四组RGB-D标准数据,包括两个细节较多的场景图像序列fr1-desk、fr1-xyz,两个大范围场景图像序列fr2-desk、fr3-long_office_household[19]。实验平台是搭载 Ubuntu20.04系统,i9-12900k处理器,32 G内存的台式电脑。

2.2 公开数据集实验研究

帧与帧之间位姿的准确性很大程度上决定了ORB-SLAM算法的精度,所以改进算法对位姿估计环节改进的有效性可以通过定位精度的提高来进行验证。本文中对选取的4个TUM数据集中不同场景下相机的估计轨迹与真实轨迹进行对比,比较结果如图3所示。

图3 真实轨迹与估计轨迹对比图Fig.3 Real and estimated trajectory contrast

从图3中可以看出实线与虚线有较高的重合度,说明本文方法在不同数据集上的大部分区域实线与虚线之间的差异都很小。而图3(b)中存在轨迹差异较大区域的原因大概率是由于采样相机运动速度较快,无法在图像中提取到足够的特征点,导致位姿进行优化计算时对应的代价函数值偏大,从而使得到的相机轨迹点偏差大,因此,后续研究的主要目标是解决较快运动条件下的相机优化问题,考虑采用稀疏匹配情况的解决办法,进而提高轨迹估算精度。

进一步比较本文方法和原始ORB-SLAM2算法在TUM数据集上的定位精度,定位精度指估计轨迹与真实轨迹的接近程度,SLAM算法常用的性能评估指标包括绝对轨迹误差(absolute trajectory error, ATE)和相对轨迹误差(relative pose error, RPE),其中绝对轨迹误差指的是相机真实位姿和估计位姿的差值,其与定位精度存在反比关系,可以采用该数值来评估定位精度,第n帧ATE模型为

(18)

式(18)中:Qn为真实轨迹;Pn为估计的轨迹;S为通过最小二乘法求得两者之间的变换矩阵。

ATE包含旋转误差和平移误差,ATE使用RMSE 统计,即

(19)

式(19)中:trans(Fn)为ATE中的平移部分,旋转误差求取公式同理,但利用平移误差对比即可。本文方法和原始ORB-SLAM2算法的相关性能指标如表1所示。

表1 改进算法与ORB-SLAM2 性能对比Table 1 Performance comparison between the algorithm in this paper and ORB-SLAM2

由表1可知,因为稠密建图线程的增加,改进算法会进行三维稠密地图场景的构建,以及实验计算平台的性能较弱的原因,改进算法的每帧耗时、合计耗时均相对于ORB-SLAM2算法较弱,并且每帧耗时的增加也会使得算法的定位精度受到一定的影响。按平均表现看,改进算法的每帧平均耗时为0.095 3 s,即达到每秒10.5帧,相较于ORB-SLAM2算法的每帧处理耗时增加了4.7%,合计处理耗时增加了10.7%。对于均方根误差RMSE,改进算法比ORB-SLAM2平均减少了0.24 cm,说明在上述4个数据集的仿真中,改进算法的平均定位精度相较ORB-SLAM2提高了7.17%。综上来看,本文所提出的改进算法能够达到机器人在定位与建图中对精度和速度的要求。

最后,在选取的4组RGB-D标准数据集上利用改进的相机位姿估计和优化方法进行场景的稠密建图测试,生成的三维稠密点云地图如图4所示。

图4 4组数据集下的稠密建图效果Fig.4 Effects of dense mapping in the four datasets

由图4可知,改进算法针对细节较多的图4(a)fr1-desk和图4(b)fr1-xyz等场景重建出的稠密点云地图不存在明显的漂移现象,且构建出的场景相对完整,较充分地表现出了细节方面;而大范围场景图4(c)fr2-desk和图4(d)fr3-long_office_household虽然因为受到数据含有的噪声点的影响,使桌面周围显得模糊不清,但是仔细观察后相关信息仍旧能够发现,验证了本文方法针对4组数据集的稠密点云地图构建结果可以相对完美地还原场景的结构信息,不会存在明显的漂移,这进一步体现了相机位姿估计的正确性,也验证了本文方法的有效性。

3 结论

为了提高ORB-SLAM2系统的定位精度以及解决无法生成稠密点云地图的问题,从提高位姿估计精度和生成一种易进行二次加工的地图这两个角度出发进行了改进,最终得到以下结论。

(1)本文提出融合ICP算法与曼哈顿世界假说的位姿估计方法,在完成点线面特征的提取与匹配后引入基于surfel的稠密建图策略将每幅图像划分为非平面和平面区域,然后非平面区域使用直接ICP算法计算位姿,平面区域使用基于曼哈顿世界假说的位姿估计方法,最终改进算法相对ORB-SLAM2均方根误差RMSE平均减少了0.24 cm,平均定位精度提高了7.17%,证明了位姿估计改进的有效性。

(2)根据新关键帧与相应位姿并利用新加入的稠密建图线程完成稠密点云地图的构建,解决了ORB-SLAM2系统无法生成稠密点云地图的问题,验证了改进算法稠密建图的可行性。

此外,改进方法虽然在4个数据集上均取得了很好的效果,但是仍旧需要进行进一步改进,首先采样相机在运动速度较快的情况下会导致位姿进行优化计算对应的代价函数值偏大,使得到的相机轨迹点偏差大。其次进行大范围场景的稠密点云地图构建时仍然存在漂移。因此,后续的研究需要解决这些不足,进一步改进和完善本文方法。