快速提取特征点的视觉SLAM室内稠密建图研究

2023-12-12李兴州余国宽

重庆理工大学学报(自然科学) 2023年11期

李兴州,何锋,余国宽

(1.贵州大学机械工程学院, 贵阳 550025) (2贵州师范大学机械与电气工程学院, 贵阳 550025)

0 引言

在未知环境下机器人利用传感器检测周边数据,以完成对周围环境的定位与构建,称为即时定位与地图构建(simultaneous localization and mapping,SLAM)[1-3]。根据所用传感器类型不同,可分为视觉SLAM与激光SLAM,以成本低廉、建图色彩丰富等优点著称的视觉SLAM近几年得到了快速发展。

通常地,对帧序列图像特征点的处理是视觉SLAM定位与建图的第一步,包括特征点的提取与匹配[4]。特征点数量对视觉SLAM性能的影响至关重要,若特征点数量少,会导致周围环境特征较少,这将严重影响视觉SLAM定位与建图的准确性[5];而特征点数量过多,则可能出现提取和匹配时间长、特征点之间误匹配的问题,严重影响视觉SLAM的实时性和准确性。因此,特征点的提取与匹配一直是视觉SLAM的研究热点方向,目前主要可以分为下列3种方法:

1)基于光流法(optical flow)的特征提取与匹配。该方法是假设光度不变性来提取特征点,Lucas等[6-7]提出的光流法特征提取最具代表,该方法对关键点进行跟踪并优化,使图像中的2个对应位置的光度误差最小,以此估计相机的运动,然而,由于此研究的假设过于保守,实验结果对图像的光度变化比较敏感,从而导致建图与定位的精度差。

2)基于直接法的特征提取与匹配。该方法是基于光流法并对相机估计位姿进行优化。Forster等[8]提出的SVO法(semidirect visual odometry)是其中的代表,该方法利用估计好的位姿对过去已经收敛插入的地图点进行全局位姿优化,位姿的估计更加准确,然而,该方法单纯地使用图像的梯度求取位姿,由于图像的非凸性,使最后的结果容易陷入局部最优;同时该方法是基于光度不变性的假设完成的,因此,在实际中无法满足该假设。

3)基于ORB特征点的特征提取与匹配。该方法是通过FAST角点(features from accelerated segment test)和对其周围描述子进行快速匹配,提取特征点。Murartal等[9]提出的ORB-SLAM系列是其研究成果的代表,该方法利用对图像取不同响应值,得到FAST角点,然后利用BRIFE描述子(binary robust independent elementary features)轻量化地完成对关键点的信息记录,利用图像旋转不变性和BoW词袋(bag of words)[10]达到特征点快速提取与匹配。然而,ORB-SLAM在进行特征提取与匹配时,按照固定网格尺寸进行特征点搜索,当面对图像金字塔图层时再采用等比例缩放的方式,无法达到特征点最大化快速的提取。

因此采用ORB特征点的特征提取与匹配方法,结合自适应网格划分,以达到对特征点快速提取的目的,同时保证其定位与建图的精度要求,缩短特征点提取时间,最后在此自适应网格划分算法上添加RGB-D稠密建图线程,完成在室内的稠密建图。

1 ORB特征提取原理

ORB-SLAM算法[9]由提取特征点和计算描述子两部分组成,分别是FAST角点检测法和BRIEF二进制特征描述子法。

如图1所示,FAST角点的提取检测法是基于像素灰度差原理完成的。取一点像素点p为圆心,半径为R的圆上按照顺时针顺序地选择16个像素点,并计算p与这些像素点的像素灰度差。如果存在N个连续像素点与p的像素灰度值的差值都大于阈值t,那么就确定p为一个角点。N通常取9,阈值t取p点灰度值的20%[9]。

图1 FAST角点提取原理示意图

由于FAST检测法提取的角点会因为图像旋转和尺度不一致,导致后续特征点提取无法正确匹配,为了解决旋转不变性和尺度不变性的问题,ORB算法采用灰度质心法[11]和构建图像金字塔解决上述问题。

灰度质心法是通过计算图像的质心与形心之间的向量作为FAST特征点的主方向,其步骤为:

1) 定义特征点的局部图像块的矩为:

(1)

式中:p,q取0或1;I(x,y)表示在该点图像灰度值;mpq表示图像的矩。在半径为R的范围内,沿x,y坐标轴方向的图像矩分别为:

(2)

(3)

整个圆形范围内灰度值总和为:

(4)

2) 图像的质心C为:

(5)

θ=arctan2(cy,cx)=arctan2(m01,m10)

(6)

然后利用二进制描述子快速找到角点。

2 改进的自适应网格划分算法

在ORB-SLAM2中,采用一种名为Qtree_ORB的算法提取ORB特征点[12]。该算法分为以下4个步骤:① 构建图像金字塔,将其分成8个等比例的图层。② 将上述得到的图像金字塔等比例图层进行网格划分,把图像分成若干个网格,然后提取FAST角点。每次FAST角点提取都要满足大于初始阈值iniTHFAST的要求,如果在该阈值要求下没有提取到角点,则按照最小阈值minTHFAST的要求来提取。③ 角点提取使用四叉树算法均匀提取。首先,初始化节点以获取四叉树结构。然后划分节点区域,如果节点区域中没有角点,则删除节点;如果节点区域中只有1个角点,则节点不再划分;如果该节点区域中有多个角点,则按照上述过程继续划分,直到节点数达到期望的数量或无法再进行划分。④ 对于得到的节点集中保留 Harris 响应值最高节点作为特征点。

2.1 自适应网格划分

ORB-SLAM算法中是按照固定网格尺寸对图像进行网格划分,由于该网格划分方法无法适应不同的外部环境。因此,采用自适应网格划分。使用面积法根据每层图像的面积及特征点数量确定网格大小,以更好地适应不同的环境,具体公式如下:

(7)

式中:X,Y为每层图像的边界坐标;w为每层金字塔图像的宽;h为每层金字塔图像的高;N为每层金字塔图像所需的特征点数目。W为划分网格的尺寸,根据每层特征点的不同,实现自适应划分。

根据每层金字塔图像的高和宽的尺寸,计算出实际划分行与列的数量,如下:

(8)

式中:R为网格划分的行;C为网格划分的列。计算网格的实际高和宽并向上取整,如下式:

(9)

hcell为实际自适应划分网格高;wcell为实际自适应划分网格宽;ceil为向上取整函数。

自适应网格划分后,需要在每层金字塔图像上进行特征点的提取,每一层图像所提取的特征点数目为:

(10)

式中:Ni为图像金字塔每层需要提取的特征点数目;N为整个图像金字塔需要提取特征点总数目;s为每层图像金字塔之间的缩放系数;m为图像金字塔总层数,为8;i为第几层金字塔。

2.2 四叉树提取法

为了筛选角点并减少冗余,可以使用四叉树[13]方法。首先,将图像分割成四叉树结构并初始化节点进行分割,每个节点中选择Harris响应值最大的ORB特征点[14]。根据要求的特征点数目对节点进行分裂,每个节点分成4个子节点,直到某个节点不存在特征点或已达到保留数量要求时,停止分裂。如图2所示(b、c中,彩色圆点为图像特征点),a为四叉树原理图,b、c为仿真试验效果图,b、c对比可见,利用四叉树提取的特征点更加均匀,匹配效果显著。

图2 四叉树原理及仿真效果图

3 实验结果与分析

本研究的仿真基于Linux平台,其中系统版本为Ubuntu18.04,CPU为i5-12490F的处理器、内存为16 GB。为了减少实验随机性带来的误差,对实验中的每组数据集分别进行10次仿真测试,最终取平均值作为实验结果。实验1进行了6组实验,分别比对了ORB-SLAM2的MONO、RGB-D和改进算法的MONO和RGB-D在特征点提取时间上的表现。实验2分别验证了ORB-SLAM2的MONO、RGB-D和改进算法的MONO和RGB-D在数据集上评估算法的绝对轨迹误差(absolute trajectory error,ATE)。

3.1 平均每帧特征点提取时间测试

本次实验基于ORB-SLAM2的MONO、RGB-D,以及改进算法的MONO、RGB-D模式,针对TUM数据集的f1_xyz、f1_desk、f1_plant、f1_rpy、f1_room、f3_long_offices 6组数据,分别进行了10次仿真,并以平均每帧特征点提取时间为评估标准。结果如图3所示。

图3 平均每帧特征点提取时间曲线

实验结果表明,提出的自适应网格划分特征点提取算法在TUM不同数据集下,无论是在MONO还是RGB-D模式下,都能够显著降低特征点提取时间,平均节约时间达到8%～10%。

3.2 绝对轨迹误差测试

本实验中,使用ORB-SLAM2算法以及改进后的算法分别在TUM数据集上进行仿真测试。并以ATE作为仿真测试的评价标准(ATE是用于评估SLAM系统估计位姿精度的一种标准,它计算真实位姿与SLAM系统估计位姿之间的误差)。为了进行评估,需要先通过时间戳将真实值和估计值进行对齐,然后计算每对位姿之间的误差值[15]。最终,ATE以图表形式输出,以便更加直观地呈现评估结果。

1) 对其真实轨迹与估计轨迹,得到两者轨迹之间的最小二乘解变换矩阵S,绝对轨迹误差定义如下:

(11)

式中:Pi表示SLAM系统估计位姿;Qi表示轨迹真实位姿。

2) 整个时间戳范围内位姿的均方根误差RMSE为:

(12)

式中:trans为SLAM系统得到的平移误差。

在TUM数据集f1_xyz、f1_desk、f1_plant、f1_rpy、f1_room、f3_long_offices上分别对ORB-SLAM2和改进后的算法进行仿真,统计MONO和RGB-D两种模式下的绝对轨迹误差,如表1所示。

表1 绝对轨迹误差评估 mm

根据表1的实验结果显示,自适应网格划分特征点提取算法在绝对轨迹误差方面表现明显优于ORB-SLAM2算法。在6组不同的数据集中,无论是MONO,还是RGB-D模式下,该算法的精度均提高了5%以上。尤其在某些大型室内场景下,改进算法的精度提高幅度最高可达50%。以f1_plant和f1_fesk两组数据的MONO和RGB-D模式下的轨迹路线及x-y-z轴方向上的误差曲线为例,如图4、图5所示。在图中,groundtruth为真实轨迹,old_trajectory为ORB-SLAM2算法得到的轨迹路线,new_trajectory表示改进后的自适应网格划分ORB算法得到的轨迹路线。从图中可以看出,改进后的算法无论在轨迹匹配上还是在绝对误差方面的精度都要高于ORB-SLAM2算法。

4 稠密建图

为了生成稠密的点云地图,在上述自适应网格划分算法框架中,增加稠密点云构建线程。该线程使用RGB-D相机测量深度信息,与RGB图像进行匹配,利用前端估算的相机位姿计算出所有像素的空间坐标,并生成点云图。通过RGB图像信息上色[16],可以得到一个由离散的彩色点组成的地图。该线程会随着新增关键帧的不断加入,进行点云拼接和全局优化,实现稀疏到稠密的转变。具体流程如图6所示。遍历匹配好的RGB图像和深度图,提取出像素坐标(u,v)和深度值d,根据针孔相机成像模型:

(13)

式中:Z为深度值和实际空间距离的比例因子;K为相机内参矩阵;R、t为相机的旋转矩阵和平移矩阵;Pw为像素点在世界坐标系下的三维坐标。由于相机已进行标定,因此内参矩阵K为已知量,跟踪线程计算相机位姿,从而确定了R、t,因此可以确定RGB图像中每个像素的空间坐标。利用PCL点云库(point cloud library)来处理计算出的点云数据。点云Pi的坐标可由下式计算得出:

图6 稠密建图具体流程框图

(14)

式中, depthScale是深度值。

再利用RGB图像三通道的色彩信息给点云上色,然后,读取下一个关键帧进行点云拼接。以TUM下的f1_desk、f1_plant、f1_room、f3_long_office四组数据集为例进行室内稠密建图,效果如图7所示。由图7可知,该稠密建图效果显著,室内物体轮廓清晰,颜色比对正确,因此,在原有基础上添加稠密建图线程可以满足室内稠密建图的需求。