基于改进随机抽样一致算法的视觉SLAM

徐 岩，安卫凤

基于改进随机抽样一致算法的视觉SLAM

徐 岩，安卫凤

(天津大学电气自动化与信息工程学院，天津 300072)

同时定位与地图构建(SLAM)在智能驾驶和机器人技术中发挥着重要的作用．针对传统随机抽样一致(RANSAC)算法对噪声敏感的问题，提出了一种改进的RANSAC算法，命名为LORANSAC，简称LO*．该算法包含内点筛选和非线性优化两部分．首先，在传统RANSAC算法估计出较好的模型后，保存在这个模型下得到的内点，在这些内点中随机选出一个子集，以进一步缩小内点的选择范围，迭代地进行模型估计．然后，对估计的模型进行捆集调整，通过最小化误差优化模型．实验使用公开的TUM RGBD数据集和KITTI数据集中的共10个序列进行评估，每个序列至少存在一个闭环，数据集涵盖小型和大型、室内和室外环境．从定性角度验证该算法删除误匹配的特征点的有效性，从定量角度验证使用该算法的定位精度．实验结果显示，与传统的RANSAC算法相比，改进的算法可以提高SLAM的定位精度．此外，实验结果与4个流行的SLAM系统对比，精度平均最高提高60.82%，最低提高12.16%．实验结果证明，该方法可以有效提高SLAM的定位精度．

机器视觉；同时定位与地图构建；随机抽样一致

同时定位与地图构建(simultaneous localization and mapping，SLAM)旨在解决移动机器人的自我定位以及在未知环境中地图构建的问题．通过SLAM，移动机器人可以知道自己在哪里，周围环境如何，以及如何在下一步中自主行动．SLAM使用的主要传感器是激光雷达和相机．雷达SLAM具有高精度和高速度，但价格昂贵．相机具有很高的场景识别功能，可以在环境中获得比激光雷达更多的纹理信息，并且成本比雷达低．基于视觉传感器的SLAM称为视觉SLAM，根据匹配方法可分为直接方法[1-3]和特征方法[4-6]．

直接方法是直接根据图像中每个像素的强度恢复未知参数，例如位置和运动．Newcombe等[7]提出了DTAM，对每个像素的深度进行逆深度处理并且进行连续优化，建立了具有更多信息的稠密地图，并实现了稳定的位置跟踪．缺点是建图的速度较慢，不利于在移动终端运行．Engel等[8]提出的DSO是少数几个使用纯直接方法的SLAM 系统，建立了稀疏地图，实现了高速度建图，但是对光照的敏感度较高，鲁棒性不高．Forster等[9]提出的SVO使用半直接方法的单目SLAM系统，利用稀疏特征点图像块而不是整幅图像匹配估计位姿，使用光流跟踪优化位姿，实现了快速定位，但是在大的场景下，不能够保持高精度．

基于特征的方法使用特征点的匹配计算相机的位置和运动，如Davison等[10]提出的MonoSLAM和Mur-Artal等[4]提出的ORB-SLAM．MonoSLAM后端是基于滤波器优化的SLAM系统[11-13]，使用扩展卡尔曼滤波器(extended Kalman filter，EKF)优化位姿，滤波器的方法只和前几个有限时刻的状态有关，在一定程度上假设了马尔可夫性，不利于检测闭环．Klein等[14]提出的PTAM并行地执行跟踪和建图的两个线程，引入捆集调整(bundle adjustment，BA)[15]的优化方式取代了传统的滤波器优化的方式，使用了关键帧机制，实现了SLAM在小型设备上的实时性．但是，没有进行闭环检测以及闭环调整．ORB-SLAM借鉴PTAM的关键帧机制，并且加入了闭环检测线程，采用了具有旋转不变性的ORB特征，加快了特征的匹配速度，但是只适合单目相机．在文献[4]的基础上，Mur-Artal等[5]扩展了ORB-SLAM，提出了适用于单目、双目和深度相机的SLAM系统，即ORB-SLAM2，可以在局部优化和全局优化之后获得相机位姿的估计，并构建全局一致的稀疏地图．

基于特征的SLAM对相机位姿估计的精度依赖于特征匹配的准确性．如果存在太多误匹配，计算出的相机位姿具有很大的偏差，这会使得累积误差增大．如何有效地删除误匹配并提高位姿估计的精度一直是SLAM关注的问题．SLAM使用传统的随机抽样一致(random sample consensus，RANSAC)算法来删除特征的误匹配．但是，传统的RANSAC算法参数没有经过细化处理，精度不高．Chum等[16]对RANSAC算法进行改进，每估计一次模型，进行一次优化，精度比传统RANSAC有所提高，满足定位的需求．但是，该算法耗时更长，不利于实现SLAM的实时性能．

本文针对传统的RANSAC算法删除特征误匹配的精度不够高，造成定位误差以及误差累积的问题，提出了一种改进的RANSAC算法，在用传统RANSAC算法迭代计算到内点后，将最小误差对应的内点保存下来，然后设定新的误差阈值，在内点中选择样本迭代计算模型．这样缩小了样本的选择范围，提高了内点被选中的概率．为了进一步精确位姿，该算法最后对得到的模型做一次BA优化．系统的前端利用该算法结合EPP[17]进行位姿估计，在闭环检测中，该方法结合Sim3[18]进行闭环检测和矫正.

1 传统的RANSAC算法

传统RANSAC算法可以在一组包含内点(inliers)和外点(outliers)的数据集中，通过迭代的方式计算数据的最佳模型．计算步骤如下．

步骤1 在数据集中随机选取(满足计算模型所有参数所需的最少的数据)个数据，计算出模型的参数．

步骤2 用得到的模型去估计所有的点，误差在设定的阈值以内的为内点，反之，为外点．

步骤3 重复步骤1、步骤2，迭代次数达到预设值后，保存最多的内点数对应的模型参数作为最终的结果．

RANSAC算法最少迭代次数满足

式中：表示计算模型参数所需的最少数据；P表示置信度，表示所选的个数据至少有一个是内点的概率；表示所选数据是外点的概率．

的解析式为

RANSAC假设数据中不存在噪声，计算的最优模型符合所有的内点，但是数据中存在噪声，噪声的随机性对模型的计算具有误导性，因此，RANSAC算法所需的迭代次数比理论次数大，得到的不是最优 模型．

2 改进的RANSAC算法

针对传统RANSAC算法存在对噪声敏感问题，本文提出一种改进的RANSAC算法——LORANSAC(locally optimized RANSAC，简称LO*)．LO*在传统的RANSAC的基础上进行了两项改进．第1项改进是增加筛选操作，保留最好的内点，滤除外点，用内点估计模型．第2项是在模型估计之后增加BA优化．用估计的模型计算误差，通过BA最小化误差，从而优化模型参数．下面介绍LO*的详细算法流程．

首先，计算所有匹配点的汉明距离，丢弃汉明距离大于阈值的匹配点，然后用汉明距离小于阈值的匹配点进行模型估计，具体流程参照模型估计.当RANSAC计算出最佳模型时，执行LO*迭代．算法的流程如图1所示．

图1 LO*算法流程

LORANSAC的具体步骤如下.

步骤1在满足汉明距离阈值的数据集中随机选取个数据，并用选取的数据估计模型．

步骤2 使用步骤1估计的模型计算所有数据点的误差，并保存误差低于阈值和期望概率的 内点．

步骤3 重复执行步骤1、步骤2，当前最佳模型出现时，更新最佳模型．

步骤4执行LO*迭代进一步筛选内点，估计模型．LO*迭代的细节如下．

(1) 采用新的阈值估计模型的内点，在内点中随机选取个数据再次估计模型．该阈值是先前阈值的倍．然后用最小二乘法最小化模型的误差，并将内点保存在新的内点集合in中，以进行进一步的选择．

(3)返回目前为止最好的模型．

步骤5对通过LO*得到的最佳模型执行BA优化，然后返回优化后的模型．

模型估计

1.for=1 todo

8.end if

9.end for

14.end if

15.end for

16.执行BA优化

LO*迭代

9.end if

10.end for

表1给出了模型估计和LO*迭代中使用的参数的含义．

表1 参数说明

Tab.1 Description of parameters

3 实验结果与对比分析

本文的实验环境是Intel i5处理器，2.6GHz CPU，4GB 内存，Ubuntu 16.04操作系统．在实验中使用了10个图像序列进行评估，其中fr1_desk、fr1_desk2、fr2_desk、fr2_xyz、fr3_office和fr3_nst 6个图像序列选自TUM RGB-D 数据集[19]，该数据集使用RGB-D相机采集的室内环境．另外4个序列(00、05、07和08)选自KITTI 数据集[20]，该数据集是使用装在汽车上的双目摄像机拍摄的城市和高速公路环境．在KITTI数据集中所选的4个序列，每个序列的轨迹至少有1个闭环．为了消除多线程不确定性的影响，对每个图像序列进行了6次实验，计算真实轨迹和估计轨迹之间的绝对平移的均方根误差(root mean square error，RMSE)．然后将6次实验的RMSE的平均值作为每个序列的最终结果．实验中使用的参数值如表2所示，汉明距离阈值采用经验值100．在统计学中，通常把置信率设为0.95，故本实验中的选取0.95．是经过多次实验、使得实验结果最优的值．counts是指最大迭代次数，根据实时性要求选取，本文设置为10．是指特征金字塔层数的尺度因子，是文献[16]建议的值．

表2 实验中使用的参数值

Tab.2 Values of parameters used in the experiments

为全面评估提出算法的性能，本文从定性和定量角度进行评估．在定性评估时，从LO*删除特征误匹配和LO*在SLAM中的定位精度两个方面进行评估．在定量评估时，对比使用RANSAC和LO*算法的定位精度，此外，与4个流行的SLAM系统进行对比，如ORB-SLAM2[5]、RGB-D SLAM[6]、ElasticFu-sion[21]和RGBDTAM[22]，其中RGB-D SLAM、ElasticFusion和 RGBDTAM的数据来自于作者发表的论文，ORB-SLAM2的数据与本文提出算法的数据在同一实验环境下取得．评价标准使用估计相机绝对平移的RMSE．

首先，为了测试LO*是否可以有效地删除误匹配，在TUM数据集的fr1_desk2序列中选取两张图片，提取图片的ORB特征进行匹配．未经过特征误匹配筛选的匹配图如图2(a)所示，可以看出，在删除误匹配之前，匹配的特征点分布在各个方向．图2(b)显示了使用RANSAC删除误匹配后的匹配图，可以看出，RANSAC算法可以删除大部分的误匹配，但是仍存在少量方向不一致的误匹配，而且，该算法删除了一些正确匹配．图2(c)显示了使用LO*删除误匹配后的匹配图，可以看出，匹配的特征点对集中在相同的方向上，减少了误匹配特征的数量，而且保留下来的正确匹配点的比例增加．

图2 匹配对比

其次，为了验证LO*在SLAM中的定位精度，本文分别选取TUM RGB-D数据集的4个序列fr1_ desk、fr2_ xyz、fr3_office和fr2_desk以及KITTI数据集的4个序列00、05、07和08进行实验．将LO*估计的轨迹和真实轨迹绘制在同一张图上，TUM数据集序列的轨迹图(主视图和俯视图)如图3～图6所示．可以看出，本文算法估计的轨迹非常接近真实轨迹，甚至部分轨迹和真实轨迹完全重合．fr3_office的轨迹的俯视图存在一个大的不规则的闭环，可以看出，本文算法估计的轨迹仍然接近真实轨迹．图3～图6中估计的轨迹存在抖动，主要是因为该序列来自手持RGB-D相机采集的数据．KITTI数据集的序列00、05、07和08的轨迹如图7所示．这4个序列采集的是室外的城市场景，环境是动态复杂的，存在大型的闭环．可以看出，本文算法对室外轨迹的估计接近真实轨迹，尤其是序列05，该轨迹存在多个拐角和闭环，轨迹长度长达千米，本文算法估计的轨迹仍然保持高精度．

此外，为了定量地对比LO*与RANSAC在SLAM中的定位精度，选取TUM RGB-D数据集的6组序列进行实验，同一个SLAM系统分别使用LO*与RANSAC删除特征的误匹配，对比指标利用真实轨迹与使用RANSAC算法和LO*估计的轨迹的RMSE表示．如表3所示．可以看出，对于这6个图像序列，LO*比RANSAC取得更高的定位精度，可以提高SLAM的定位精度．

图3 序列fr1_desk的估计轨迹与真实轨迹

图4 序列fr2_xyz的估计轨迹与真实轨迹

图5 序列fr3_office的估计轨迹与真实轨迹

图6 序列fr2_desk的估计轨迹和真实轨迹

图7 4个KITTI序列的估计轨迹和真实轨迹

最后，为了定量地对比本文提出的算法与4个流行的SLAM系统估计轨迹的精度，计算真实轨迹与用不同方法估计的轨迹之间的绝对平移的RMSE，并在表4中示出．“—”表示作者的文献中没有提供数据，“x”表示双目相机的数据集无法在基于深度相机的SLAM系统中运行．对于TUM RGB-D数据集，本文将提出的算法与4个流行的基于深度相机的SLAM系统进行对比．对于所有测试序列，本文提出的算法估计位姿的精度高于RGB-D SLAM[6]、ElasticFusion[21]和RGBDTAM[22]系统对相机位姿估计的精度．对于KITTI数据集，本文将提出的算法与ORB-SLAM2[5]进行对比，对于序列05和07，本文的算法比ORB-SLAM2估计的相机位姿精度高．对于序列08，本文的算法获得与ORB-SLAM2相同的结果．表4的最后一行表示10组数据精度提高的算术平均百分比，可以看出，本文的算法比ORB-SLAM2、RGB-D SLAM、ElasticFusion和RGBDTAM分别提高了12.16％、60.82％、42.32％和32.28％．总体来看，提出的LO*算法估计相机位姿的精度在不同数据集中和不同的SLAM系统对比均有不同程度的提高．

表3 使用RANSAC和LO*定位精度的对比

Tab.3 Comparison of positioning accuracy using RANSAC and LO*

表4 不同SLAM系统位姿估计均方根误差的对比

Tab.4 Comparison of the RMSE of pose estimation for different SLAM systems

4 结 语

本文提出了一种改进的RANSAC算法LORANSAC，用于基于ORB特征的SLAM系统．在传统RANSAC的基础上实现了对合格内点的进一步选择，并且在估计模型后增加一次非线性优化．实验结果表明，LO*能够有效地删除特征的误匹配，并且与其他4个流行的SLAM系统对比，本文提出的算法可以更精确地估计相机位姿．两种不同数据集的实验表明，本文提出的算法适用于室内和室外环境，并且具有较高的精度．

［1］ Silveira G，Malis E，Rives P. An efficient direct approach to visual SLAM[J]. IEEE Transactions on Robotics，2008，24(5)：969-979.

［2］ Cremers D. Direct methods for 3D reconstruction and visual SLAM[C]//International Conference on Machine Vision Applications. Nagoya，Japan，2017：8-12.

［3］ Engel J，Stückler J，Cremers D. CPA-SLAM：Consistent plane-model alignment for direct RGB-D SLAM[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg，Germany，2015：1285-1291.

［4］ Mur-Artal R，Montiel J M M，Tard́os J D. ORB-SLAM：A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics，2015，31(5)：1147-1163.

［5］ Mur-Artal R，Tard́os J D. ORB-SLAM2：An open-source SLAM system for monocular，stereo and RGB-D cameras[J]. IEEE Transactions on Robotics，2017，33(5)：1255-1262.

［6］ Endres F，Hess J，Sturm J，et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics，2014，30(1)：177-187.

［7］ Newcombe R A，Lovegrove S J，Davison A J. DTAM：Dense tracking and mapping in real-time[C]// IEEE International Conference on Computer Vision. Barcelona，Spain，2011：2320-2327.

［8］ Engel J，Koltun V，Cremers D. Direct sparse odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40(3)：611-625.

［9］ Forster C，Pizzoli M，Scaramuzza D. SVO：Fast semi-direct monocular visual odometry[C]// International Conference on Robotics and Automation. Hong Kong，China，2014：15-22.

［10］ Davison A J，Reid I D，Molton N D，et al. MonoSLAM：Real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2007，29(6)：1052-1067.

［11］ Montemerlo M，Thrun S，Koller D，et al. Fastslam：A factored solution to the simultaneous localization and mapping problem[C]// AAAI National Conference on Artificial Intelligence. Edmonton，Alberta，Canada，2002：593-598.

［12］ Bailey T，Nieto J，Guivant J，et al. Consistency of the EKF-SLAM algorithm[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. Beijing，China，2006：3562-3568.

［13］ Reuter S，Vo Ba-Tuong，Vo Ba-Ngu，et al. The labeled multi-bernoulli filter[J]. IEEE Transactions on Signal Processing，2014，62(12)：3246-3260.

［14］ Klein G，Murray D. Parallel tracking and mapping for small AR workspaces[C]// IEEE ACM International Symposium on Mixed and Augmented Reality. Nara，Japan，2007：225-234.

［15］ Triggs B，Zisserman A，Szeliski R. Proceedings of International Workshop on Vision Algorithms：Theory and Practice[M]. Heidelberg：Springer-Verlag，2000：298-372.

［16］ Chum O，Matas J，Kittler J. Locally optimized RANSAC[C]// 25th DAGM Symposium. Berlin，Germany，2003：236-243.

［17］ Lepetit V，Moreno-Noguer F，Fua P. EPP：An accurate()solution to the PP problem[J]. International Journal of Computer Vision，2009，81(2)：155-166.

［18］ Strasdat，H，Montiel J M M，Davison A J. Scale drift-awarelarge scale monocular SLAM[C]// Robotics：Science and Systems Conference. Zaragoza，Spain，2010：1-8.

［19］ Sturm J，Engelhard N，Endres F，et al. A benchmark for the evaluation of RGB-D SLAM systems[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura，Portugal，2012：573-580.

［20］ Geiger A，Lenz P，Stiller C，et al. Vision meets robotics：The KITTI dataset[J]. International Journal of Robotics Research，2013，32(11)：1231-1237.

［21］ Whelan T，Salas-Moreno R F，Glocker B，et al. ElasticFusion：Real-time dense SLAM and light source estimation[J]. International Journal of Robotics Research，2016，35(14)：1697-1716.

［22］ Concha A，Civera J. RGBDTAM：A cost-effective and accurate RGB-D tracking and mapping system[EB/OL]. https：//arxiv.org/pdf/1703.00754.pdf，2017-03-02.

Visual SLAM Based on the Improved RANSAC Algorithm

Xu Yan，An Weifeng

(School of Electrical and Information Engineering，Tianjin University，Tianjin 300072，China)

Simultaneous localization and mapping(SLAM)has been playing an important role in intelligent driving and robotics．To address the noise sensitivity problem of the traditional random sample consensus (RANSAC) algorithm，an improved RANSAC algorithm called locally optimized RANSAC(LORANSAC，abbreviated as LO*)is proposed．The algorithm comprises two parts：inners screening and nonlinear optimization．First，after the traditional RANSAC algorithm estimates a good model，the inners produced by this model are saved．Then，a subset is randomly selected among these inners to further narrow the selection range of the inners and iteratively perform model estimation．Finally，the estimated model is bundledand optimized by minimizing the error．The proposed algorithm is evaluated using 10 public sequences from the TUM RGBD and KITTI datasets，with at least one closed loop for each sequence，which cover both small-and large-scale indoor and outdoor environments．Experiments were conducted to qualitatively validate the algorithm in deleting the incorrect matches of feature points and quantitatively verify the positioning accuracy of the algorithm．The experiment results show that in comparison with the traditional RANSAC algorithm，the improved algorithm can enhance the positioning accuracy of SLAM．In addition，in comparison with the average accuracy of the four well-known SLAM systems，the average accuracy of the improved algorithm is increased by a maximum of 60.82% and a minimum of 12.16%．The experiment results show that the proposed method can effectively improve the positioning accuracy of SLAM．

machine vision；simultaneous localization and mapping；random sample consensus

TP242.6

A

0493-2137(2020)10-1069-08

10.11784/tdxbz201908024

2019-08-13；

2019-10-28.

徐 岩（1977—  ），女，博士，副教授，xuyan@tju.edu.cn.

安卫凤，awf_joker@tju.edu.cn.

国家自然科学基金资助项目(61372145)；青海省基础研究资助项目(2017-ZJ-753)；天津大学自主创新基金资助项目(2015XZC-0005).

Supported by the National Natural Science Foundation of China(No.61372145)，the Fundamental Research Project of Qinghai Province，China(No.2017-ZJ-753)，the Independent Innovation Fund of Tianjin University(No.2015XZC-0005).

(责任编辑：王晓燕)