基于改进关键帧选择的RGB-D SLAM算法

李 弋 星，刘 士 荣，仲 朝 亮，王 坚

(杭州电子科技大学 自动化学院， 浙江 杭州 310018 )

基于改进关键帧选择的RGB-D SLAM算法

李 弋 星，刘 士 荣*，仲 朝 亮，王 坚

(杭州电子科技大学 自动化学院， 浙江 杭州 310018 )

关键帧选择是提高视觉SLAM(simultaneous localization and mapping)算法精度及实时性的重要因素．关键帧常以图像的帧间相对运动距离为选择依据．该方法虽简单有效，但实时性、鲁棒性较差且容易产生大量冗余关键帧．针对上述问题，提出一种改进的关键帧选择算法．该算法整合了帧间相对运动距离、帧间特征点跟踪以及最小视觉变化来选择关键帧并删除冗余关键帧．基于该算法，结合具有较好方向和光照不变性的ORB(oriented FAST and rotated BRIEF)特征，实现了RGB-D SLAM算法．在RGB-D数据集上的实验表明，改进的关键帧选择算法能够更精准、及时地选择关键帧，并在减少RGB-D SLAM中冗余关键帧的同时提高算法的实时性、建图和定位精度．

关键帧选择；冗余关键帧删除；ORB特征；RGB-D SLAM

0 引 言

近年来，机器人三维同时定位与建图(3D SLAM)问题已经成为机器人领域研究的热点[1]．相较于二维SLAM，三维SLAM能够更好地利用环境的三维属性，描述出环境中物体的空间几何外形，对机器人在未知环境中的导航具有重要的意义[2]．得益于视觉传感器信号探测范围宽、信号获取内容全、价格低廉等特点，国内外学者对VSLAM[3](visual SLAM)进行了大量研究，包括LSD SLAM[4]、ORB-SLAM[5]、RGB-D SLAM[6]等．

TUM(Technische Universität München)计算机视觉组提出的LSD SLAM直接利用单目相机实现三维SLAM．不同于ORB-SLAM采用特征点进行帧间匹配，LSD SLAM采用直接法，根据灰度图强度(image intensities)进行帧间匹配和建图，实现了大规模环境下的准确定位与建图．ORB-SLAM利用ORB特征完成SLAM中的所有任务：跟踪、建图、重定位和环形闭合检测．在室内、室外以及大规模环境下，ORB-SLAM有较好的鲁棒性和实时性．与LSD SLAM使用单目相机通过三角测量方法恢复环境深度相比，RGB-D相机(如微软公司的Kinect)能够在捕获RGB图像的同时获得每一个像素的深度信息，很大程度上简化了利用三角测量方法获取深度信息的过程．基于上述优点，德国弗莱堡大学Endres等[6]提出了RGB-D SLAM算法，使用视觉传感器Kinect 作为唯一传感器对周围环境进行三维建图．实验证明，RGB-D SLAM算法能快速建图并具有很好的鲁棒性．

在VSLAM中，通常使用关键帧进行建图和定位，这样可以避免因逐帧插入导致系统实时性降低、计算代价增加、系统内存消耗过大的问题．迄今为止，根据帧间相对运动距离来选择关键帧是VSLAM最常用的方法[7]．该方法虽简单有效，但精度低、实时性差．同时该方法往往直接将已选择的关键帧全部储存[1,6]，导致VSLAM算法后期存在大量冗余关键帧．本文针对上述问题，首先提出一种结合帧间相对运动距离、特征点跟踪以及最小视觉变化的关键帧选择算法，以便更加及时、准确地插入关键帧．然后通过关键帧之间的图像可跟踪点进行二次判断，删除冗余关键帧．最后基于改进的关键帧选择算法，结合具有较好方向和光照不变性的ORB特征，实现RGB-D SLAM算法．

1 RGB-D SLAM算法框架

本文通过基于图(graph-based)的方法实现RGB-D SLAM算法．该算法主要由前端(front-end)、后端(back-end)、地图构建3部分组成，如图1所示．前端又叫位姿图构建；后端又叫图优化，用以优化全局位姿图.

图1 RGB-D SLAM的算法框架Fig.1 The overview of RGB-D SLAM algorithm

1.1 图构建

图构建主要包括特征检测与匹配、运动估计、关键帧选择与冗余关键帧删除以及环形闭合检测4部分．

常用的特征点包括SIFT[8]、SURF[9]、FAST[10]等．考虑到SLAM对实时性的要求，采用ORB[11]特征进行特征检测与提取．ORB特征由Rublee等[11]于2011年提出，其计算速率更快、精度更高且具有更好的鲁棒性(尺度和旋转不变性)．对于ORB特征，采用BruteForceMatcher结合汉明距离进行匹配．由于图像匹配后会包含大量误匹配对，采用随机采样一致性算法(RANSAC)[12]并利用RGB-D相机提供的深度信息对初匹配结果进行采样，从而剔除外点(outliers)、抑制噪声并利用正确的匹配对计算帧间匹配的内点(inliers)、旋转向量和平移向量；然后根据旋转向量和平移向量进行运动估计，计算出帧间相对运动距离；再结合特征点跟踪和最小视觉变化进行关键帧选择，之后通过关键帧之间的图像可跟踪点进行二次判断，删除冗余关键帧；最后，再次利用RANSAC算法进行局部环形闭合检测和全局环形闭合检测，用以检索当前场景是否重复出现，从而完成SLAM算法前端的所有环节．

1.2 图优化

完成图构建后，通过优化位姿图可以求解相机的最优位姿．常用的图优化方法包括非线性最小二乘法[13]、基于松弛的优化方法[14]、基于随机梯度下降法[15]以及流形优化方法[16]．非线性最小二乘法、基于松弛的优化方法和基于随机梯度下降法都是在欧氏空间中进行优化，但机器人的真实位姿变化却是在非欧氏空间中的．为了防止欧氏空间中奇异值的出现，使用四元数来表示机器人的位姿变化并在流形空间中进行优化．这样不仅克服了欧氏空间中产生奇异值的问题，还可取得更高的精度．本文具体采用g2o进行后端优化．g2o是Kümmerle等[16]提出能用于流形优化的开源工具，可以大大提高开发效率．

1.3 地图构建

三维环境地图有很多表示方法，本文采用3D点云地图和3D OctoMap[17]来建图．3D点云地图能够清晰直观地表示环境信息，但存在大量冗余点且系统内存占用较大，不适合在大规模环境以及终生SLAM(life-long SLAM)算法中使用．3D OctoMap克服了点云地图上述缺点，在表示地图的同时占用少量的内存．此外，3D OctoMap能够更加直观地表示环境中的占用、空闲和未知区域，更有利于机器人导航和路径规划．

2 关键帧选择算法

2.1 常用关键帧选择算法

当前端完成图像匹配后，利用RGB-D相机提供的各像素深度信息和RANSAC算法，可计算出帧间旋转向量r和平移向量t，从而求解帧间相对运动距离：

(1)

以D为判断依据，常用关键帧选择算法如下：

①若Dmin≤D≤Dmax，则Framecurr=Framekey；

②若D

③若D>Dmax，则Framecurr≠Framekey

其中，当前帧为Framecurr，关键帧为Framekey，两帧之间的最小运动距离为Dmin，两帧之间的最大运动距离为Dmax．条件②说明两帧之间运动距离较近，变化较小；条件③说明两帧之间运动距离较远，变化较大．以上两种情况均说明当前帧不是关键帧．

除此之外，还可以结合RANSAC算法中Inliers来选择关键帧．该算法虽然结合了D和Inliers来判断是否选择关键帧，但仍不能很好地满足SLAM算法对实时性、鲁棒性和精度的要求．在一些相机旋转或抖动的特殊情况下仍会出现判断错误甚至关键帧丢失的现象．对于终生SLAM算法来说，由于没有判定冗余关键帧，还会导致系统存储大量冗余的关键帧，使计算和存储负荷不断增大．

为了克服常用关键帧选择存在的不足，本文提出选择关键帧后，检测冗余关键帧，并将其删除．

2.2 改进关键帧选择算法

2.2.1 关键帧的选择 对D加以限制，能确保帧间运动距离在一定的范围内，对Inliers加以限制，能够确保相邻图像之间有足够的匹配精度．而特征点跟踪能够保证帧间的相关性，提高建图的一致性．本文提出最小视觉变化概念．最小视觉变化是指两帧图像之间存在足够小但又满足需求的运动变化，包括运动距离最小变化和帧间旋转角度最小变化．最小视觉变化能够提高帧间匹配精度，保证帧间相关性及建图的一致性．

为此，在常用关键帧选择算法中引入了特征点跟踪和最小视觉变化作为关键帧判断依据．该算法伪代码描述如下：

If (环形闭合检测空闲)or(距KeyframeL插入后又经过了z个帧)Then

If(Inliers>Inliersmin)Then

If(Dmin≤D≤Dmax)Then

If(Track(Framecurr-1,Framecurr)>α)Then

If(Match(KeyframeL,Framecurr)>β)Then

选择关键帧

EndIf

EndIf

EndIf

EndIf

EndIf

在上述算法中，Inliersmin为最少内点数，Framecurr为当前帧，Framecurr-1为上一帧，KeyframeL为最新插入的关键帧，α、β、z为可调参数．Track(Framecurr-1,Framecurr)用以计算Framecurr与Framecurr-1之间跟踪到的特征点个数，Match(KeyframeL,Framecurr)用以计算Framecurr与KeyframeL之间匹配的特征点个数占KeyframeL特征点个数的比例．

首先，判定是否插入关键帧，即在不进行环形闭合检测时，插入关键帧能够保证环形闭合检测的准确率、有效性．在距KeyframeL插入后检测到第z个非关键帧时插入关键帧，能够确保在建图空闲时插入，提高关键帧的检测效率．

其次，利用帧间相对运动距离D以及内点数Inliers对关键帧进行筛选．例如，算法语句If (Track(Framecurr-1,Framecurr)>α) Then用来对相邻帧间特征点进行跟踪．条件Track(Framecurr-1,Framecurr)>α说明帧间相关性强、匹配度高，将Framecurr作为候选关键帧，从而避免关键帧丢失．

最后，以最小视觉变化进行判定，检测Framecurr与KeyframeL之间跟踪到的特征点个数．当Match(KeyframeL,Framecurr)>β时，说明新插入的关键帧与当前帧之间有足够运动距离，帧间旋转角度满足要求且匹配度高，将Framecurr选定为关键帧，并在位姿图中插入该帧．

2.2.2 冗余关键帧的删除 大部分关键帧算法都不会检测冗余关键帧，但检测冗余关键帧并将其删除不仅能在很大程度上保证SLAM算法构建地图的一致性，还能减少关键帧的数量和占用的系统存储空间．这对于需要检测环境变化并对地图进行持续更新和维护的终生SLAM算法，具有重大意义．

本文通过比较相邻n个关键帧图像间的可跟踪特征点进行关键帧的二次判断．冗余关键帧具体检测方法如下：

①Track(Keyframecurr-1,Keyframecurr)>ε；

②重复上述比较方法；

③Track(Keyframecurr-n,Keyframecurr)>ε；

④如果以上判别式均成立，Keyframecurr为冗余关键帧，将其删除．

上述检测方法中，Keyframecurr-1,…,Keyframecurr-n为Keyframecurr之前的n个关键帧，Keyframecurr为当前关键帧，Track(Keyframecurr-n,Keyframecurr)用以计算Keyframecurr与Keyframecurr-n之间匹配的特征点个数．

当Keyframecurr上占比ε(ε>70%)的特征点能够在Keyframecurr-n上检测到时，说明Keyframecurr相对于之前的n个关键帧的运动尺度过小，Keyframecurr为冗余关键帧，删除Keyframecurr．这种方法本质上是关键帧的局部删除，由于是在已选择的关键帧中进行二次判断，在减少关键帧数量的同时提高了建图和定位精度．

3 实验研究

所有实验均在Ubuntu 12.04环境下完成，计算机CPU为2.0 GHz Intel i5,运行内存为3 GB．实验数据来自于RGB-D数据集——TUM RGB-D Benchmark[18]和NYU RGB-D Datasets[19]．其中，coffee来自于NYU RGB-D Datasets，fr1_xyz和long_office_household(loh)来自于TUM RGB-D Benchmark．实验分别比较了常用关键帧选择算法(仅以D、Inliers为关键帧选择依据)、改进关键帧选择算法(包含冗余关键帧)、改进关键帧选择算法(删除冗余关键帧)对RGB-D SLAM实时性、建图和定位精度以及关键帧数量的影响．

3.1 RGB-D SLAM实时性分析

实时性对任何SLAM来说都是重要的性能评价指标．表1比较了ORB、SIFT和SURF特征在同一数据集下的实时性和匹配精度．从表1可知，ORB特征检测时间约为SIFT和SURF特征的1/10，且匹配时间远远少于SIFT和SURF特征．同时，ORB特征的匹配精度为SIFT和SURF特征的2倍以上．显然ORB特征在检测和匹配速率上都比其他特征更快且精度更高．

表1 ORB、SIFT和SURF特征实时性和匹配精度比较Tab.1 Comparison of ORB, SIFT and SURF features in terms of real-time and matching precision

在使用ORB特征的基础上，3种关键帧选择算法对RGB-D SLAM实时性的影响结果如表2所示．从表2可知，无论是否删除冗余关键帧，改进的关键帧选择算法都可以减少系统运作时间，提高实时性．特别是在场景loh中，由于数据较多，基于删除冗余关键帧的改进算法的实时性尤为显著．

表2 RGB-D SLAM中3种关键帧选择算法实时性比较Tab.2 Comparison of three key-frame selection algorithms in terms of real-time in RGB-D SLAM

3.2 RGB-D SLAM建图和定位精度分析

图2为3种关键帧选择算法构建的3D点云地图．显然，图2(a)、(b)中存在大量漂移点，场景中房屋、桌子和人的轮廓都不够清晰．由于场景中存在动态行人，前两种关键帧选择算法都不能很好地构建人所经过区域的局部地图．与图2(a)、(b)相比，图2(c)中的漂移点和冗余点较少，房屋、桌子和人的轮廓很清晰．并且人在移动过程中未被遮挡时的图像也可以清晰显示．由此可见，删除冗余关键帧改进算法构建的3D点云地图比其他两种算法精度更高．

(a) 关键帧选择算法

(b) 包含冗余关键帧改进算

(c) 删除冗余关键帧改进算法

图2 3种关键帧选择算法构建的3D点云地图
Fig.2 3D pointcloud map constructed by three key-frame selection algorithms

图3(a)为TUM计算机视觉组利用运动视觉捕捉系统记录的摄像头在该场景下真实且连续的轨迹，图3(b)、(c)、(d)分别为fr1_xyz场景下常用关键帧选择算法、删除冗余关键帧改进算法以及包含冗余关键帧改进算法计算的轨迹．各关键帧选择算法生成轨迹是非连续的，且与真实运动轨迹(ground truth)间存在旋转和平移关系，所以并未将所有轨迹统一到一个坐标系中．可以看出，图3(c)中删除冗余关键帧改进算法对应的运动轨迹奇点较少，与真实轨迹更加一致．

(a) 轨迹1

(b) 轨迹2

(c) 轨迹3

(d) 轨迹4

图3 真实运动轨迹与3种关键帧选择算法的实际运动轨迹
Fig.3 Ground truth and actual trajectory of three key-frame selection algorithms

对于定位精度[18]，采用实际运动轨迹与真实运动轨迹间的均方根误差(root mean square error,Erms)进行比较．Erms越小，精度越高，计算式如下：

(2)

表3比较了3种关键帧选择算法对应的Erms．从表3可知，在不同实验数据下，改进关键帧选择算法(删除冗余关键帧)的Erms最小，约为常用关键帧选择算法的1/2，从而表明该算法的定位精度最高．

3.3 RGB-D SLAM关键帧数量分析

本文通过对相邻两关键帧(n=2)进行二次判断来删除冗余关键帧．图4比较了不同数据集下3种关键帧选择算法计算的关键帧数量N．很显然，改进关键帧选择算法(删除冗余关键帧)的关键帧数量远远小于其他两种算法．当RGB-D SLAM算法在大规模环境loh中使用时，这种差异尤为明显，其计算的关键帧数量约为常用关键帧选择算法的一半．

表3 RGB-D SLAM中3种关键帧选择算法定位精度的比较Tab.3 Comparison of localization accuracy of three key-frame selection algorithms in RGB-D SLAM

图4 不同数据集下3种关键帧选择算法计算的关键帧数量
Fig.4 The number of key-frame calculated by three key-frame selection algorithms under different datasets

3.4 实验结果分析

从上述实验中可以发现，引入删除冗余关键帧方法的改进关键帧选择算法在实时性、建图和定位精度方面性能较其他两种算法更好．在加入 “当前环形闭合检测空闲或距KeyframeL插入后又经过了z个帧”条件且未删除冗余关键帧时，改进算法中关键帧数量略多于常用关键帧选择算法中的数量．但是在加入删除冗余关键帧的方法后，改进算法中的关键帧的数量就会有所下降．特别是在大规模环境中，基于删除冗余关键帧的改进算法中的关键帧数量更是大幅下降．

实验过程中，当可调参数设置不准确时会出现地图重影、定位有奇异值等现象．对于不同的数据集，文中提到的这些参数最优取值范围往往需要通过大量的实验反复测试才能确定．

一般情况下，Inliersmin、Dmin和Dmax可以通过相邻两帧图像之间的匹配情况以及算法实际所需精度来确定：Inliersmin越大，Dmin和Dmax的范围越小，关键帧的选取条件就越严格，在一定程度上会提高定位和建图的精度但也会增加系统的计算代价．

α表示Framecurr与Framecurr-1之间跟踪到的特征点个数，α越大表明可跟踪的特征点个数越多，两帧图像匹配度越高．由于在Track(Framecurr-1,Framecurr) 之前已经使用常用关键帧选择算法进行了一次关键帧的筛选且还需为后续环节留有足够的候选关键帧，所以此时α在50～100为宜．β表示Framecurr与KeyframeL之间匹配的特征点个数所占KeyframeL特征点个数的比例．β越大表明Framecurr与KeyframeL之间的相关性越高．为了防止关键帧丢失以及满足最小视觉变化原则，β在85%～95%为宜．对于α、β，均不能因精度要求而选择过大，这样不仅会导致候选关键帧数量不足，还会增加计算量，降低算法实时性．设参数d表示帧间间隔空隙．d过小会频繁插入关键帧，在增加计算负荷的同时降低建图的精度甚至出现定位和环形闭合检测错误的现象；d过大则不能充分利用系统资源合理进行计算．一般情况下，d在20～30为宜．设参数k表示关键帧删除时需检测的关键帧个数，该参数可随着环境规模的增加适当增加．考虑到整个算法的实时性，k在2～4即可满足删除冗余关键帧的需求．

4 结 语

本文提出的改进关键帧选择算法不仅在一定程度上解决了常用关键帧选择算法鲁棒性较差、精度较低的问题，还通过删除冗余关键帧的方法减少了关键帧数量．整体上，本文提出的关键帧选择算法在及时、准确地插入关键帧的同时，提高了RGB-D SLAM算法的实时性、建图和定位精度．

[1]GARCIA-FIDALGO E, ORTIZ A. Vision-based topological mapping and localization methods:A survey [J]. Robotics and Autonomous Systems, 2015, 64:1-20.

[2]CARLONE L, TRON R, DANIILIDIS K,etal. Initialization techniques for 3D SLAM:A survey on rotation estimation and its use in pose graph optimization [J]. Proceedings - IEEE International Conference on Robotics and Automation, 2015, 2015-June:4597-4604.

[3]FUENTES-PACHECO J, RUIZ-ASCENCIO J, RENDN-MANCHA J M. Visual simultaneous localization and mapping:a survey [J]. Artificial Intelligence Review, 2012, 43(1):55-81.

[4]ENGEL J, SCHÖPS T, CREMERS D. LSD-SLAM:Large-scale direct monocular SLAM [J]. Lecture Notes in Computer Science, 2014, 8690 LNCS(part 2):834-849.

[5]MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM:a versatile and accurate monocular SLAM system [J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.

[6]ENDRES F, HESS J, STURM J,etal. 3D mapping with an RGB-D camera [J]. IEEE Transactions on Robotics, 2014, 30(1):177-187.

[7]KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces [C] // 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality, ISMAR. Piscataway:IEEE Computer Society, 2007:4538852.

[8]LOWE D G. Distinctive image features from scale-invariant keypoints [J]. International Journal of Computer Vision, 2004, 60(2):91-110.

[9]BAY H, TUYTELAARS T, VAN GOOL L. SURF:Speeded up robust features [J]. Lecture Notes in Computer Science, 2006, 3951 LNCS(1 of 4):404-417.

[10]ROSTEN E, DRUMMOND T. Machine learning for high-speed corner detection [J]. Lecture Notes in Computer Science, 2006, 3951 LNCS(1 of 4):430-443.

[11]RUBLEE E, RABAUD V, KONOLIGE K,etal. ORB:An efficient alternative to SIFT or SURF [C] // 2011 International Conference on Computer Vision, ICCV 2011. Piscataway:IEEE, 2011:2564-2571.

[12]FISCHLER M A, BOLLES R C. Random sample consensus:a paradigm for model fitting with applications to image analysis and automated cartography [J]. Communications of the ACM, 1981, 24(6):381-395.

[13]DELLAERT F, KAESS M. Square root SAM:Simultaneous localization and mapping via square root information smoothing [J]. International Journal of Robotics Research, 2006, 25(12):1181-1203.

[14]FRESE U, LARSSON P, DUCKETT T. A multilevel relaxation algorithm for simultaneous localization and mapping [J]. IEEE Transactions on Robotics, 2005, 21(2):196-207.

[15]GRISETTI G, STACHNISS C, GRZONKA S,etal. A tree parameterization for efficiently computing maximum likelihood maps using gradient descent [C]// Robotics:Science and Systems. Cambridge:MIT Press Journals, 2008:65-72.

[16]KÜMMERLE R, GRISETTI G, STRASDAT H,etal. G2o:A general framework for graph optimization [C] // 2011 IEEE International Conference on Robotics and Automation, ICRA 2011. Piscataway:IEEE, 2011:3607-3613.

[17]HORNUNG A, WURM K M, BENNEWITZ M,etal. OctoMap:An efficient probabilistic 3D mapping framework based on octrees [J]. Autonomous Robots, 2013, 34(3):189-206.

[18]STURM J, ENGELHARD N, ENDRES F,etal. A benchmark for the evaluation of RGB-D SLAM systems [C] // 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2012. Piscataway:IEEE, 2012:573-580.

[19]SILBERMAN N, FERGUS R. Indoor scene segmentation using a structured light sensor [C] // 2011 IEEE International Conference on Computer Vision Workshops, ICCV Workshops 2011. Piscataway:IEEE, 2011:601-608.

RGB-D SLAM algorithm based on improved key-frame selection

LI Yixing,LIU Shirong*,ZHONG Chaoliang,WANG Jian

(School of Automation, Hangzhou Dianzi University, Hangzhou 310018, China )

The key-frame selection is an important factor to improve the accuracy and real-time of visual SLAM algorithm. The key-frame selection is often based on the relative motion distance of the images. This method is simple and effective, but with poor real-time and robustness, and is easy to generate a large number of redundant key-frames. Aiming at the above problems, an improved key-frame selection algorithm is proposed. The algorithm incorporates the distance of relative motion of the frames, the feature points tracking and the minimum visual change to select a key-frame and delete the redundant key-frames. Based on this algorithm, combined the ORB (oriented FAST and rotated BRIEF) features with better direction and illumination invariant, the RGB-D SLAM algorithm is implemented. Experiments on RGB-D datasets indicate that the improved key-frame selection algorithm can be more accurate, timely in selecting key-frames, and improve the real-time and the accuracy of mapping and localization while reduce the redundant key-frames.

key-frames selection; redundant key-frames deletion; ORB feature; RGB-D SLAM

1000-8608(2017)04-0411-07

2016-12-30；

2017-05-26．

国家自然科学基金资助项目(61175093).

李弋星(1992-)，女，硕士生，E-mail:yixing_li_323@163.com；刘士荣*(1952-)，男，教授，博士生导师，E-mail:liushirong@hdu.edu.cn.

TP242

A

10.7511/dllgxb201704012