基于BoW模型的增强RGB-D SLAM算法

荆树旭，卢鹏宇，翟晓惠，高 涛

(长安大学 信息工程学院，陕西 西安 710064)

0 引 言

随着智能机器人技术的不断发展，研究者们希望机器人可以像人类一样，通过眼睛去观察和理解周围的世界，完成自身的定位和对周围环境的建模。这个问题被称为即时定位与地图构建(simultaneous localization and mapping，SLAM)[1-4]。视觉SLAM[5]是指通过视觉传感器的方式完成即时定位与地图构建。相比使用激光雷达来获取周围环境信息，视觉传感器具有体积小，重量轻，功耗小，价格便宜等优点[4]。2010年，微软推出了可以同时获取彩色图和深度图的相机Kinect。由于其低廉的价格，使得RGB-D相机在视觉SLAM领域得到了广泛的应用[6-8]。

目前，国内外主流的方法是基于特征点法和图优化法的RGB-D SLAM。2010年，Henry等人[9]最早提出了一种基于深度相机的SLAM算法，该算法首先提取SIFT特征点，然后采用最近迭代点(iterative closest point，ICP)算法求解两帧图像之间的最优变换，最后利用图优化工具TORO进行全局优化。2012年，Henry等人[10]提出了一种改进的算法，该算法利用重投影误差改进随机采样一致性(radom sample consensus，RANSAC)算法[11]的配准过程，使用Fast特征提取和Calonder特征描述符代替之前的SIFT，用稀疏光速平差法(sparse bundle adjustment，SBA)代替TORO进行优化。2011年，Newcombe等人[12]提出的KinectFusion是最早能实现实时三维重建的方案，可以在不需要RGB图只需要深度图的情况下就能实时地建立三维模型。2014年，Whelan等人[13]在KinectFusion的基础上增加了回环检测模块来增强鲁棒性。同年，Endres等人[14]测评了SIFT[15]、SURF[16]和ORB[17]这3种方法在RGB-D SLAM中的表现，在后端采用通用图优化工具g2o进行全局优化，最后采用了八叉树地图OctoMap进行地图的构建。2017年，Mur-Artal R等人[18]提出了ORB-SLAM2，这是一个基于单目、双目和RGB-D相机的完整的SLAM系统。2018年，张震等人[19]利用ORB特征和词袋(bag of words，BoW)模型增强了RGB-D SLAM算法的鲁棒性。

上述算法存在的部分问题：采用暴力匹配算法对特征点进行匹配，需要每个特征与其他所有的特征进行匹配，匹配范围大，耗时严重。RANSAC处理数据时具有随机性的特点，忽略了待匹配点质量的差别。BoW模型不考虑图像中视觉单词的位置信息，会导致回环检测的精度不足。针对上述问题，该文利用BoW模型结合型树层次结构，将特征点划分为特征字典某一特定层的节点，只有属于同一父节点的特征才是潜在的正确匹配，从而缩小匹配范围，提高算法的实时性。利用顺序抽样一致性(progressive sample consensus，PROSAC)算法[20]对图像匹配误差进行剔除并且求解相机位姿，利用视觉词典选出一系列闭环候选帧，对闭环候选帧进行严格的筛选，从而筛选出真正的闭环帧，提高算法的鲁棒性。

1 基于BoW模型的增强RGB-D SLAM算法

算法的整体流程如图1所示。在前端，采取ORB算法提取图像的特征点，基于BoW模型改进匹配，在保证匹配效果的同时，减少匹配的时间，提高算法的实时性，结合PROSAC算法和透视n点(PnP)算法计算相机的初始位姿变换，利用bundle adjustment(BA)进行非线性优化，得到更精确的相机位姿。通过关键帧筛选方案判断该帧是否成为关键帧。在后端，通过BoW模型[21]对关键帧序列中的每一帧与当前关键帧图像进行相似度得分计算，得分较高的列为闭环候选帧，利用结构一致性对候选帧序列进行回环判定，筛选出真正的闭环帧，将闭环帧作为新的约束加入到位姿图中，增强算法的鲁棒性。利用通用图优化工具g2o对位姿图进行全局优化，得到全局一致的位姿。最后利用全局优化后的位姿和对应的点云数据，采用贪心三角化算法重建出网格地图。

图1 算法流程

1.1 ORB特征提取与基于BoW的图像特征匹配

1.1.1 ORB特征提取

该文选用ORB特征算法进行特征提取。ORB算法采用改进的FAST角点检测方法获取特征点，采用BRIEF特征描述子生成特征描述符。与SIFT和SURF相比，ORB算法在速度上拥有绝对的优势，不需要使用GPU加速，适合在嵌入式设备或缺少GPU运算能力的设备上使用。

1.1.2 基于BoW的图像特征匹配

当两幅图像进行匹配时，如果采用暴力匹配法，将每幅内的每一个特征与另一幅图像内的每一个特征进行匹配，其时间复杂度为O(N2)。而BoW的正向索引存储了图像特征描述子和它们在字典树中关联的节点，这种方式可以极大地缩小图像特征的匹配范围，加快匹配速度。算法步骤如下：

(1)对任意两幅图像I1和I2，提取它们的ORB特征，得到它们的特征点和描述子，即(keypoints1i，descriptors1i)和(keypoints2i，descriptors2i)。其中keypoints1i表示I1的特征点，descriptos1i表示与keypoints1i对应的特征描述子；keypoints2i表示I2的特征点，descriptos2i表示与keypoints2i对应的特征描述子。

(2)对descriptors1i和descriptors2i求其对应的BoW特征向量(node1i，f1ij)和(node2i，f2ij)。其中node1i和node2i表示第l层的节点，f1ij表示以node1i为父节点的特征，f2ij表示以node2i为父节点的特征。

(3)当node1i=node2i时，计算f1ij和f2ij对应的描述子descripto1ij与descriptor2ij之间的汉明距离Dist，设置最小值为bestDist1，次小值为bestDist2。

设定阈值H和h，若满足：

bestDist1

(1)

bestDist1

(2)

则认为该匹配是正确的匹配，存入Matches。

最终可得到鲁棒性强的匹配Matches，Matches被用来进行下一步的图像配准和位姿估计。

1.2 位姿求解与优化

完成两幅图像的匹配后，可以进行相机位姿的求解。该文使用PROSAC算法与PnP算法结合计算相机的初始位姿，同时认为最邻近匹配的汉明距离越小，匹配点的质量越高。得到相机的初始位姿后，采用BA对初始位姿进行非线性优化，得到精确的位姿，BA问题即求解最小化重投影误差，如式(3)所示。

(3)

其中，(R,t)表示相机位姿，Xi表示空间点的坐标，xi表示该点在像素内的坐标，Φ表示相机内参，ρ表示Huber核函数。

BA将在g2o中采用Levenberg-Marquadt实现。位姿求解与优化的具体算法如下：

(1)对Matches中的匹配点按照bestDist1的值从小到大进行排序。

(2)在排序后的点中选取前n个点作为高质量匹配点作为RANSAC算法和PnP算法的输入，求解出相机位姿的变换矩阵(R,t)和内点数Inliers。

(3)将(R,t)代入式(3)求得优化后的相机位姿(R,t)*。

1.3 关键帧选择

该文使用关键帧进行定位和建图，通过计算帧间相对运动距离来判断该帧是否为关键帧。帧间相对运动距离的计算公式如下：

D=t+min(2π-R,R)

(4)

其中，(R,t)表示相机位姿。

对当前帧Framecurr，判断该帧是否为关键帧的方法如下所示：

(1)判断Inliersi>Inliersmin，如果该式成立，则进行下一步判断，否则舍弃该帧。

(2)将(R,t)*作为初值输入式(4)，求得帧间相对运动距离D。

(3)设定阈值Dmin和Dmax，若Dmin

该文引入了冗余关键帧删除机制，有助于地图的构建和提高SLAM算法的鲁棒性。对当前关键帧KeyFramecurr，如果KeyFramecurr上超过90%以上的特征点可以被至少其他3个关键帧检测到时，认为KeyFramecurr为冗余关键帧，将其从关键帧序列里删除。

1.4 回环检测

该文通过BoW模型对关键帧序列进行回环检测，首先计算待检索图像和数据库图像的BoW向量，计算方式如下：

(5)

其中，ni和mi分别表示待检索图像和数据库图像中单词i的数目，ωi表示单词i的权重。然后计算待检索图像和数据库图像之间的相似性分数，计算方式如下：

(6)

利用式(6)可以计算当前帧Framecurr与旧的关键帧KeyFramei之间的相似度。通过建立关键帧的倒排索引，记录包含特定单词的关键帧。对于新的一帧图像，首先计算该帧图像包含的单词表，对于每一个单词，通过倒排索引查找具有该单词的所有关键帧，同时记录每个关键帧与当前帧的共同单词个数。对那些共同单词个数超过N的关键帧，计算其与当前帧之间的相似性得分Score。设定阈值sTH，若相似度分数Scorei满足：

Scorei>sTH×Scoremax

(7)

则将存储到候选闭环帧序列。

若闭环候选帧序列不为空，对采用1.1.2节的方法得到匹配点对Matches，之后利用RANSAC算法计算出基础矩阵F和内点数inlier，利用F验证对极几何约束[22]，如果满足几何约束关系并且inlier大于阈值fTH，则认为构成回环。

1.5 全局优化与地图构建

该文通过位姿图进行全局优化，将每个关键帧的相机位姿作为节点，将两个位姿的变换关系T作为边构建位姿图。这样对于两个有边连接的节点，会存在一个误差：

ei,j=xi-T×xj

(8)

其中，xi,xj表示i,j节点的估计值。将所有的顶点和边考虑进来，总的误差函数为：

(9)

其中，Ωi,j表示边的信息矩阵。这样整个问题被转换为一个最小二乘问题。该文采用g2o中的Levenberg-Marquadt法进行求解。

在相机运动过程中，可能会出现运动丢失的情况。为此，该文加入了重定位功能：当连续M帧图像都出现失配的时候，采用回环检测的方法寻找关键帧序列中与当前帧最为相似的图像，之后通过1.2节的方法建立帧间约束并将当前帧设置为关键帧。

该文选用网格地图来进行稠密地图的构建，其构建算法如下所示：

(1)通过滤波对点云进行处理，减少点云数据容量，去除点云数据中的噪声、离群点。

(2)通过重采样对物体表面进行平滑处理和漏洞修复。

(3)计算点云法线，并将点云位姿、颜色、法线信息合并到一起，构建有向点云。

(4)使用贪心投影三角化算法对有向点云进行三角化，构建三角化网格地图。

2 实验结果与分析

为了验证算法的有效性，该文利用Sturm等人[23]提供的RGB-D基准包进行实验，该基准包中包含Kinect产生的彩色图像和深度图像序列，以及机器人的真实运动位姿。同时，文献[23]还提供了一个自动评估工具，该工具可以自动计算出真实轨迹与算法估计轨迹之间的均方根误差(RMSE)。该文将从准确性和实时性与文献[19]、文献[24]和文献[25]进行对比。算法的实验平台为：Intel i5 9300H处理器(2.4 GHz)，16 GB RAM，64位Ubuntu16.04操作系统。

2.1 特征匹配结果分析

对Fr1/xyz数据包相邻的两帧图像进行特征匹配。表1为基于BoW模型的特征匹配算法和暴力匹配算法关于Fr1/xyz数据包的特征匹配时间对比，图2所示为BoW匹配的结果。

表1 Fr1/xyz数据包特征匹配时间对比

由表1可知，在ORB算法提取的特征点数量为500，Fr1/xyz数据包共产生787次匹配的情况下，暴力匹配共使用了10.7 s的时间完成匹配，BoW匹配共使用了6.2 s的时间完成匹配，由此可见，BoW匹配可以极大地减少特征匹配的时间。由图2可知，BoW匹配的结果较好，正确匹配数量多。

图2 BoW匹配结果

2.2 定位误差对比

该文选取了Fr1数据包中的Fr1/xyz、Fr1/rpy、Fr1/desk等9个数据集进行实验。表2为文中算法与文献[19]、文献[24]和文献[25]的均方根误差对比。

表2 文中算法与文献[19]、文献[24]和文献[25]的均方根误差对比 m

由表2可以看出，文中算法在Fr1/xyz、Fr1/rpy、Fr1/desk等6个数据集中的精度均优于文献[19]、文献[24]和文献[25]中提出的算法。仅在Fr1/desk2数据集，文中算法略低于文献[19]和文献[25]，但优于文献[24]；在Fr1/360数据集，文中算法略低于文献[24]和文献[25]；在Fr1/plant数据集，文中算法略低于文献[25]，但优于文献[24]。基于相同数据的集性能比较，文献[24]的平均RMSE为0.096 8 m，文献[25]的平均RMSE为0.084 2 m，文中算法的平均RMSE为0.079 7 m，总体精度优于文献[24]和文献[25]。文献[19]在测试数据集的平均RMSE为0.072 8 m，文中算法对应的数据集的平均RMSE为0.066 5 m，总体精度优于文献[19]。根据以上统计结果，验证了文中算法的总体精度优于文献[19]、文献[24]和文献[25]，具有更好的准确性。

为了更加直观地显示文中算法的定位准确性，对估计轨迹与真实地面轨迹进行误差量化分析。图3是估计轨迹与真实轨迹的对比图，其中的(a)、(b)、(c)、(d)分别对应了表2的Fr1/xyz、Fr1/desk、Fr1/floor、Fr1/room。表3为误差量化数据，包括最大值max、最小值min、中位数median、均值mean、标准差std。

(a)Fr1/xyz数据集相机轨迹恢复结果 (b)Fr1/desk数据集相机轨迹恢复结果

表3 误差量化指标 m

从图3可以看出，估计轨迹能较好地贴合真实轨迹，全局一致性较好。从表3可以看出在Fr1数据包中，误差值与变化波动都较小，总体误差在可接受范围。

2.3 运行速度对比

由于文献[25]只进行了特征点的提取，没有进行特征描述符与特征匹配，所以文中只与文献[19]和文献[24]进行速度对比，对比结果如表4所示。

表4 文中算法与文献[19]和文献[24]算法运行速度对比 s

文献[24]处理一帧数据平均需要0.35 s，即平均每秒钟处理约3帧数据，文献[19]处理一帧数据平均需要0.11 s，即每秒钟处理约10帧数据，而文中算法处理一帧数据平均用时为0.04 s，即每秒钟处理约25帧数据，是文献[24]的8倍，文献[19]的2.5倍。由于Kinect的帧率为30 Hz，因此文中算法基本满足实时性要求。

2.4 地图构建评估

该文选用了Fr1/xyz、Fr1/desk和Fr1/floor数据包进行建图分析。图4的(a)、(b)和(c)分别表示使用Fr1/xyz、Fr1/desk和Fr1/floor数据包构建的三维网格地图。

(a)Fr1/xyz数据集重建结果

Fr1/xyz和Fr1/desk是办公室场景，为手持Kinect拍摄所得。由图4(a)和(b)可以看到，该场景内的办公桌、电脑、书籍和墙壁等都得到了很好的重建。图中的空洞部分是由于拍摄过程中没有扫描到那部分。Fr1/floor数据包由机器人携带Kinect拍摄所得。由图4(c)可以看到，整个场景得到了完整的重建。

3 结束语

该文提出了一种改进的RGB-D SLAM算法来进行即时定位与地图构建。在SLAM前端，该文采用ORB特征提取算法来进行特征点的提取，使其可以运行在嵌入式设备和缺乏GPU的设备上；采用BoW模型改进了匹配方法，极大地减少了匹配所需要的时间，提高了算法的实时性；利用PROSAC算法结合PnP算法计算相机位姿，提高了精度的同时也节省了时间，并且利用非线性优化进一步提高了位姿的精度；在传统的关键帧选取方法上加入了冗余关键帧删除机制，便于关键帧的管理和维护。在SLAM后端，该文采用了BoW模型进行闭环检测，利用结构一致性提高了闭环的准确率，增强了算法的鲁棒性；通过通用图优化工具g2o对位姿图进行优化，得到了全局一致的位姿。该文采用贪心三角化算法来构建三维网格地图。最后，该文选取TUM数据集里的Fr1数据包进行实验，并通过与文献[19]、文献[24]和文献[25]进行了实时性和准确性的对比，表明了该算法的优越性。在下一步的工作中，将会结合IMU(惯性测量单元)等高精度设备更进一步优化相机位姿，利用深度学习方法提取语义信息，构建语义SLAM。