CN111882602B - 基于orb特征点和gms匹配过滤器的视觉里程计实现方法 - Google Patents

Abstract

本发明公开了一种基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，使用RGB‑D相机或双目相机采集视频流；对于每一帧图像，基于相机当前位姿，获取图像中的特征点(包括特征点对应角点的二维像素坐标、三维世界坐标和brief描述子)；对当前帧和上一帧之间的特征点结合GMS匹配过滤器进行匹配；根据对上一帧匹配到的特征点，求解PnP问题，得到当前帧的相机位姿；对最近的连续若干帧使用Bundle Adjustment同时优化相机位姿和特征点三维世界坐标；使用词袋模型计算当前帧的词袋向量，判定是否需要将当前帧保存为关键帧，或者是否出现回环；若出现回环，则使用快速回环优化对相关的关键帧进行优化。本发明在保证对相机重投影误差足够小的情况下，较传统奇异值分解的方法运算更简便，保证实时性。

Description

基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法

技术领域

本发明属于机器人自主导航领域，涉及一种基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法。

背景技术

里程计是一种通过传感器来推算被测物体位姿的技术。目前，各行各业对高精度、强鲁棒性的里程计的需求日益迫切，特别是在矿业、军事、勘探等领域中，人们试图使用各种里程计来实现没有地形特征、地标参照和第三方定位等情况下的环境信息采集和有效的位姿估计。视觉里程计是一种根据视觉信息实现位姿估计的算法。视觉里程计相比于目前常用的激光里程计、惯性导航里程计等具有更低的成本，但由于对应的实现方法不够成熟。在相对复杂的场景下，其鲁棒性、实时性和精度无法达到实现要求。目前常见的视觉里程计方案按所使用的传感器可分为单目视觉里程计、双目视觉里程计和RGB-D(彩色-深度)视觉里程计。其中，双目视觉和RGB-D能够更准确地测算出的深度信息，继而获得误差更小的位姿估计。

在现有的视觉SLAM方案中，PTAM作为一个早期的较为成功的单目视觉里程计框架，提供了一个较为完整的通用框架。PTAM创造性的将跟踪和建图过程放在两个线程里面并行的进行，这大大提升了程序的运行效率。同时，它使用了非线性优化作为后端，使视觉SLAM系统在相同的计算量的前提相比于卡尔曼滤波的视觉SLAM系统有更高的精度。但是因为存在场景小，缺乏全局重定位功能，导致实用性较差。J.Engle提出了LSD-SLAM算法(Jakob Engel,Thomas Sch,Daniel Cremers.LSD-SLAM:large-scale direct monocularSLAM[C]//ECCV.2014.)作为一种单目视觉里程计，使用直接法进行半稠密地图的重建。它仅依靠CPU就实现了半稠密场景的重建，在它之前很少有方案能够只用CPU就重建出稠密的地图。另外，LSD-SLAM的关键帧之间的约束使用了相似变换群和李代数表达出尺度，在后端优化的时候可以将不同尺度的场景考虑进来，减小了尺度的漂移现象。但是该方案对于相机内参误差和图像曝光率非常敏感，且在相机快速运动时容易跟踪中断。ORB-SLAM2是一种基于RGB-D传感器的视觉里程计方案，最早由Raul Mur-Artal(Mur-Artal R,Montiel JMM,Tardos J D.ORB-SLAM:aVersatile andAccurate Monocular SLAM System[J].IEEETransactions on Robotics,2015,31(5):1147-1163.)等人提出。它非常完善，精度很不错，并且代码清晰易读，简单易用，便于二次开发，使得ORB-SLAM成为视觉SLAM系统的开发模板。相比如其它视觉SLAM系统，ORB-SLAM有如下优点：首先它既支持单目相机，同时也支持双目和RGBD相机，具有良好的泛用性。他们在KITTI、EuRoC等公开数据集上的测试结果尚可，精度较高，但在实际应用过程中体现出的综合性能较差，难以克服复杂场景下的干扰问题，所谓的“实时性”也是基于性能较强的计算机的基础上而言的，并且ORB(一种快速特征点提取和描述的算法)特征点的检测比较耗费计算量，容易跟丢等。

发明内容

本发明的目的在于提供一种基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法。

实现本发明目的的技术解决方案为：一种基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，使用RGB-D相机或双目相机采集视频流，对于每一帧图像，基于相机当前位姿，获取图像中特征点对应角点的二维像素坐标、三维世界坐标和brief描述子；对当前帧和上一帧之间的特征点结合GMS匹配过滤器进行匹配；根据对上一帧匹配到的特征点，求解PnP问题，得到当前帧的相机位姿；对最近的连续若干帧使用BundleAdjustment同时优化相机位姿和特征点三维世界坐标；使用词袋模型计算当前帧的词袋向量，判定是否需要将当前帧保存为关键帧，或者是否出现回环；若出现回环，则使用快速回环优化对相关的关键帧进行优化。

本发明与现有技术相比，其显著优点：(1)在位姿估计的过程中应用了特征点法，并使用了ORB作为特征点提取和描述子计算的方法。该算法由FAST算法改进而来，在使用FAST提取出特征点、获得特征点的坐标之后，给其定义一个特征点方向，以此来实现特征点的旋转不变性；最后使用Brief描述子对特征点进行描述，对于不同图像上的特征点，可以依据Brief描述子对不同的特征点进行匹配，来判定他们是不是世界中的同一点，通过两点的Brief描述子的Hamming距离来度量他们的相似度，继而判定他们是否匹配、是否对应世界中的同一点。因此，对于一个ORB特征点，它有一个二维图像上坐标、一个Brief描述子、和一个世界坐标。(2)在进行特征点匹配的时候，常常会出现特征点匹配错误的情况出现，有些情况下，两个特征点的描述子Hamming距离很小，但实际上并不是对应世界中的同一点。GMS(基于网格的运动统计算法)是一种将运动平滑度封装为区域中一定数量的匹配项的统计似然性的简单方法。GMS可以将高匹配数转换为高匹配质量。这提供了实时、超鲁棒的通信系统。对具有低纹理，模糊和宽基线的视频进行的评估表明，GMS始终优于其他实时匹配器，并且可以通过更复杂，更慢的技术来实现奇偶校验。(3)步骤5和步骤9中的ORB特征点匹配过程中采用了GMS匹配过滤器，该过滤器基于网格的运动统计，将运动平滑度作为一个统计量，进行局部区域的匹配，同时剔除错误的匹配；对于两帧图像当中的特征点，先使用brute-force方法进行暴力匹配，并计算匹配点对之间的Hamming距离，然后使用GMS匹配过滤器剔除错误匹配，最后设置一定的阈值来剔除Hamming距离过大的匹配点对。在保证程序运算速度的同时提高了特征匹配的精度。(4)步骤16中使用了一种快速回环优化的方法；对于一段连续的若干个关键帧，固定首尾两个关键帧的相机位姿和特征点世界坐标，实现快速的回环优化。(5)步骤3、步骤11和步骤15中求解ORB特征点的三维世界坐标，对于双目相机，提出一种快速的三角测量方法，此方法在保证对相机重投影误差足够小的情况下，较传统奇异值分解的方法运算更简便，保证实时性。

下面结合附图对本发明作进一步说明。

附图说明

图1为本发明基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法的流程图。

图2为当前帧F_k匹配到某关键帧时各关键帧对应相机位姿的示意图。

图3为加权回环优化后F_k和相关关键帧对应相机位姿的示意图。

具体实施方式

本发明基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，使用RGB-D相机或双目相机采集视频流，对于每一帧图像，基于相机当前位姿，获取图像中特征点对应角点的二维像素坐标、三维世界坐标和brief描述子；对当前帧和上一帧之间的特征点结合GMS匹配过滤器进行匹配；根据对上一帧匹配到的特征点，求解PnP问题，得到当前帧的相机位姿；对最近的连续若干帧使用BundleAdjustment同时优化相机位姿和特征点三维世界坐标；使用词袋模型计算当前帧的词袋向量，判定是否需要将当前帧保存为关键帧，或者是否出现回环；若出现回环，则使用快速回环优化对相关的关键帧进行优化。

结合图1，本发明基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，使用RGB-D相机或双目相机，具体步骤如下：

步骤1，视觉里程计初始化，获取RGB-D(彩色-深度)相机或双目相机视频流的当前时刻的帧F₀，对于RGB-D相机，包括一幅RGB(彩色)二维图像和一幅相对应的深度图像；对于双目相机，包括左相机和右相机的RGB二维图像。

步骤2，在F₀图像中提取ORB特征点，对于RGB-D相机，在RGB二维图像中寻找ORB特征点，获取图像中ORB特征点的像素坐标，计算ORB特征点的Brief描述子；对于双目相机，获取左相机图像中ORB特征点的像素坐标，计算左相机图像中ORB特征点的Brief描述子，使用光流法寻找右相机图像中与左相机图像中ORB特征点匹配的特征点。

步骤3，假设帧F₀对应的相机位姿为{R₀＝[1,0,0；0,1,0；0,0,1],t₀＝[0；0；0]}，根据这一位姿计算F₀中ORB特征点的三维世界坐标。

步骤4，将F₀保存为关键帧，保存其位姿和ORB特征点信息。

步骤5，获取RGB-D相机或双目相机视频流的当前时刻的帧F_k(k>0，k为整数)。

步骤6，在F_k(k>0)图像中提取ORB特征点。对于RGB-D相机，在RGB二维图像中寻找ORB特征点，获取图像中ORB特征点的像素坐标，计算ORB特征点的Brief描述子；对于双目相机，获取左相机图像中ORB特征点的像素坐标，计算左相机图像中ORB特征点的Brief描述子，使用光流法寻找右相机图像中与左相机图像中ORB特征点匹配的特征点。

步骤7，基于词袋模型，计算F_k的RGB二维图像的词袋向量V_k。对于一个n维的词袋向量V_k＝{word₁,word₂,…,word_n}中的任意一个单词word_i有word_i＝value_i*weight_i，其中单词的权重来源于图片，是变化的。分别计算V_k和所有关键帧的词袋向量之间的相似度η(V_k,V_i)。若V_k与某关键帧的词袋向量的相似度大于某个阈值(最大值为1，表示V_k与该关键帧的词袋向量一模一样；一般取0.3以上)，则判断与该关键帧匹配，则说明出现回环，跳到步骤13，否则继续步骤8。

步骤8，基于步骤7计算出的F_k的词袋向量V_k，如果它和所有关键帧的词袋向量之间的相似度η(V_k,V_i)均小于一定阈值(为区分各关键帧，避免错误匹配，一般取0.1～0.2)，则判定当前帧F_k为新的关键帧。

步骤9，通过计算当前帧F_k的所有ORB特征点描述子与上一帧F_k-1中的ORB特征点描述子的Hamming距离，对两帧中的ORB特征点做匹配，并使用GMS匹配过滤器剔除错误的匹配，对于RGB-D相机，匹配F_k和F_k-1的RGB图像中的ORB特征点；对于双目相机，匹配F_k和F_k-1的左相机图像中的ORB特征点。

步骤10，根据当前帧F_k与上一帧F_k-1匹配的ORB特征点的三维世界坐标，使用PnP算法计算当前帧对应的相机位姿{R_k,t_k}。

步骤11，基于当前帧F_k的相机位姿{R_k,t_k}，计算当前帧中新的ORB特征点的三维世界坐标；

步骤12，对当前帧F_k到F_k-n+1(0<n<k)的连续n帧根据BundleAdjustment(光束平差法)的原理同时优化相机位姿和特征点三维世界坐标，若在步骤8中判定当前帧能够作为关键帧，则将当前帧保存为关键帧，保存其位姿和ORB特征点信息，返回步骤5。

步骤13，通过计算当前帧F_k的所有ORB特征点描述子与匹配到的关键帧中的ORB特征点描述子的Hamming距离，对两帧中的ORB特征点做匹配，并使用GMS匹配过滤器并结合特定的阈值剔除错误的匹配，对于RGB-D相机，匹配F_k和匹配到的关键帧的RGB图像中的ORB特征点；对于双目相机，匹配F_k和匹配到的关键帧的左相机图像中的ORB特征点。

步骤14，根据当前帧与步骤7匹配到的关键帧的三维ORB特征点的世界坐标，使用PnP算法计算当前帧对应的相机位姿{R_k,t_k}。

步骤15，基于当前帧F_k的相机位姿{R_k,t_k}，计算当前帧中被提取但没有和F_k-1中的特征点匹配的ORB特征点的三维世界坐标。

步骤16，使用加权回环优化算法对相关联的关键帧位姿进行优化，从而实现当前帧的定位和位姿计算。并且，返回步骤5，继续下一帧的计算，直到程序结束。

当不需要相机位姿计算时，可以中断上述任意步骤，直接退出该步骤中的处理程序。

所述步骤9和步骤13中的ORB特征点匹配过程中采用了GMS匹配过滤器，该过滤器基于网格的运动统计，将运动平滑度作为一个统计量，进行局部区域的匹配，同时剔除被错误匹配的特征点；对于两帧图像当中的特征点，先使用brute-force方法进行暴力匹配，并计算匹配点对之间的Hamming距离，然后使用GMS匹配过滤器剔除错误匹配，最后设置一定的阈值(根据实际对特征点匹配数量调整，为经验值，一般选取25-30)来剔除Hamming距离过大的匹配点对。在保证程序运算速度的同时提高了特征匹配的精度。

所述步骤3、步骤11和步骤15中求解ORB特征点的三维世界坐标，对于RGB-D相机方法如下：

先从深度图像中获得对应特征点的深度信息d，已知ORB特征点对于相机的投影齐次坐标为p＝[u v 1]^T、相机的位姿是{R，t}，则该特征点的三维世界坐标为P＝R·[d·ud·v d]^T+t。

所述步骤3、步骤11和步骤15中求解ORB特征点的三维世界坐标，对于双目相机方法如下：

假设当前双目相机左相机位姿为{R，t}，右相机相对左相机的位姿为{R_relative，t_relative}，其中R、R_relative为旋转矩阵，t、t_relative为平移矩阵。假设特征点的相对左相机的深度为s₁、相对右相机的深度为s₂，已知左相机中特征点的投影齐次坐标为x₁＝[u₁ v₁ 1]^T，右相机中在步骤2或步骤6中通过光流法获得的与左相机特征点匹配的特征点的投影齐次坐标为x₂＝[u₂ v₂ 1]^T，则有等式s₁x₁＝s₂R_relativex₂+t_relative，(R_relativex₂)×s₁x₁＝s₂(R_relativex₂)×R_relativex₂+(R_relativex₂)×t_relative＝(R_relativex₂)×t_relative，则有

则该特征点的三维世界坐标为P＝R·[s₁·u₁ s₁·v₁ d]^T+t，此方法在保证对右相机重投影误差足够小的情况下，较传统奇异值分解的方法运算更简便，保证实时性。

所述步骤16中使用加权回环优化的具体过程为：假如当前帧F_k匹配到关键帧，说明出现回环(相机位姿很接近该关键帧的位置，并且当前帧的词袋向量和该关键帧的词袋向量相似度足够大)。如图2所示，在当前帧F_k之前，已保存了15个关键帧(图2中举例说明)，当F_k和关键帧F_key0相匹配，则说明从F_key0到F_key14再到F_k构成一个回环，但由于之前开环状态下的视觉里程计由于各种随机误差和系统误差造成了位姿估计的偏差，这时假设F_key0和F_k的位姿估计是准确的，使用回环优化来调整F_key1到F_key14的相机位姿，使得这些关键帧的位姿的某种误差最小，具体方法如下：

假设F_keyi对应的相机位姿为

T_keyi和T_keyj的变换矩阵为

回环优化后产生误差

则有优化问题：

其中标量k_ij表示每个误差项的权重，若F_keyi到F_keyj之间(包括F_keyj对用的帧)的所有c帧F_n到F_n+c-1的相机位姿分别由k_n、k_n+1……k_n+c-1个匹配特征点通过PnP算法得出，则k_ij＝min{k_N,k_n+1…k_n+c-1}，此方法能够在不增加计算成本的情况下提升回环优化结果的精度，该优化问题可通过Gauss-Newton、Levenberg-Marquardt等梯度下降方法求解，优化的结果如图3所示。

Claims (5)

1.一种基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，其特征在于使用RGB-D相机或双目相机采集视频流，对于每一帧图像，基于相机当前位姿，获取图像中特征点对应角点的二维像素坐标、三维世界坐标和brief描述子；对当前帧和上一帧之间的特征点结合GMS匹配过滤器进行匹配；根据对上一帧匹配到的特征点，求解PnP问题，得到当前帧的相机位姿；对最近的连续若干帧使用Bundle Adjustment同时优化相机位姿和特征点三维世界坐标；使用词袋模型计算当前帧的词袋向量，判定是否需要将当前帧保存为关键帧，或者是否出现回环；若出现回环，则使用快速回环优化对相关的关键帧进行优化；

具体步骤如下：

步骤1，视觉里程计初始化，获取RGB-D相机或双目相机视频流的当前时刻的帧F₀，对于RGB-D相机，包括一幅RGB二维图像和一幅相对应的深度图像；对于双目相机，包括左相机和右相机的RGB二维图像；

步骤2，在F₀图像中提取ORB特征点，对于RGB-D相机，在RGB二维图像中寻找ORB特征点，获取图像中ORB特征点的像素坐标，计算ORB特征点的Brief描述子；对于双目相机，获取左相机图像中ORB特征点的像素坐标，计算左相机图像中ORB特征点的Brief描述子，使用光流法寻找右相机图像中与左相机图像中ORB特征点匹配的特征点；

步骤3，假设帧F₀对应的相机位姿为{R₀＝[1,0,0；0,1,0；0,0,1],t₀＝[0；0；0]}，根据这一位姿计算F₀中ORB特征点的三维世界坐标；

步骤4，将F₀保存为关键帧，保存其位姿和ORB特征点信息；

步骤5，获取RGB-D相机或双目相机视频流的当前时刻的帧F_k，k>0，k为整数；

步骤6，在F_k图像中提取ORB特征点，对于RGB-D相机，在RGB二维图像中寻找ORB特征点，获取图像中ORB特征点的像素坐标，计算ORB特征点的Brief描述子；对于双目相机，获取左相机图像中ORB特征点的像素坐标，计算左相机图像中ORB特征点的Brief描述子，使用光流法寻找右相机图像中与左相机图像中ORB特征点匹配的特征点；

步骤7，基于词袋模型，计算F_k的RGB二维图像的词袋向量V_k，对于一个n维的词袋向量V_k＝{word₁,word₂,…,word_n}中的任意一个单词word_i有word_i＝value_i*weight_i，分别计算V_k和所有关键帧的词袋向量之间的相似度η(V_k,V_i)；若V_k与某关键帧的相似度大于某个阈值，则判断与该关键帧匹配，则说明出现回环，跳到步骤13，否则继续步骤8；

步骤8，基于步骤7计算出的F_k的词袋向量V_k，如果它和所有关键帧的词袋向量之间的相似度η(V_k,V_i)均小于一定阈值，则判定当前帧F_k为新的关键帧；

步骤9，通过计算当前帧F_k的所有ORB特征点描述子与上一帧F_k-1中的ORB特征点描述子的Hamming距离，对两帧中的ORB特征点做匹配，并使用GMS匹配过滤器剔除错误的匹配，对于RGB-D相机，匹配F_k和F_k-1的RGB图像中的ORB特征点；对于双目相机，匹配F_k和F_k-1的左相机图像中的ORB特征点；

步骤10，根据当前帧F_k与上一帧F_k-1匹配的ORB特征点的三维世界坐标，使用PnP算法计算当前帧对应的相机位姿{R_k,t_k}；

步骤11，基于当前帧F_k的相机位姿{R_k,t_k}，计算当前帧中新的ORB特征点的三维世界坐标；

步骤12，对当前帧F_k到F_k-n+1的连续n帧根据Bundle Adjustment的原理同时优化相机位姿和特征点三维世界坐标，0<n<k，若在步骤8中判定当前帧能够作为关键帧，则将当前帧保存为关键帧，保存其位姿和ORB特征点信息，返回步骤5；

步骤13，通过计算当前帧F_k的所有ORB特征点描述子与匹配到的关键帧中的ORB特征点描述子的Hamming距离，对两帧中的ORB特征点做匹配，并使用GMS匹配过滤器并结合特定的阈值剔除错误的匹配，对于RGB-D相机，匹配F_k和匹配到的关键帧的RGB图像中的ORB特征点；对于双目相机，匹配F_k和匹配到的关键帧的左相机图像中的ORB特征点；

步骤14，根据当前帧与步骤7匹配到的关键帧的三维ORB特征点的世界坐标，使用PnP算法计算当前帧对应的相机位姿{R_k,t_k}；

步骤15，基于当前帧F_k的相机位姿{R_k,t_k}，计算当前帧中被提取但没有和F_k-1中的特征点匹配的ORB特征点的三维世界坐标；

步骤16，使用加权回环优化算法对相关联的关键帧位姿进行优化，从而实现当前帧的定位和位姿计算，返回步骤5，继续下一帧的计算，直到程序结束。

2.根据权利要求1所述的基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，其特征在于：

所述步骤9和步骤13中的ORB特征点匹配过程中采用了GMS匹配过滤器，该过滤器基于网格的运动统计，将运动平滑度作为一个统计量，进行局部区域的匹配，同时剔除被错误匹配的特征点；对于两帧图像当中的特征点，先使用brute-force方法进行暴力匹配，并计算匹配点对之间的Hamming距离，然后使用GMS匹配过滤器剔除错误匹配，最后设置一定的阈值来剔除Hamming距离过大的匹配点对。

3.根据权利要求1所述的基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，其特征在于：

所述步骤3、步骤11和步骤15中求解ORB特征点的三维世界坐标，对于RGB-D相机方法如下：

先从深度图像中获得对应特征点的深度信息d，已知ORB特征点对于相机的投影齐次坐标为p＝[u v 1]^T、相机的位姿是{R，t}，则该特征点的三维世界坐标为P＝R·[d·u d·vd]^T+t。

4.根据权利要求1所述的基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，其特征在于：

所述步骤3、步骤11和步骤15中求解ORB特征点的三维世界坐标，对于双目相机方法如下：

假设当前双目相机左相机位姿为{R，t}，右相机相对左相机的位姿为{R_relative，t_relative}，其中R、R_relative为旋转矩阵，t、t_relative为平移矩阵，假设特征点的相对左相机的深度为s₁、相对右相机的深度为s₂，已知左相机中特征点的投影齐次坐标为x₁＝[u₁ v₁ 1]^T，右相机中在步骤2或步骤6中通过光流法获得的与左相机特征点匹配的特征点的投影齐次坐标为x₂＝[u₂ v₂ 1]^T，则有等式s₁x₁＝s₂R_relativex₂+t_relative，(R_relativex₂)×s₁x₁＝s₂(R_relativex₂)×R_relativex₂+(R_relativex₂)×t_relative＝(R_relativex₂)×t_relative，则有

则该特征点的三维世界坐标为P＝R·[s₁·u₁ s₁·v₁ d]^T+t。

5.根据权利要求1所述的基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，其特征在于：

所述步骤16中使用加权回环优化的具体过程为：假如当前帧F_k匹配到关键帧，说明出现回环，使用回环优化来调整F_key1到F_key14的相机位姿，使得这些关键帧的位姿的误差最小，具体方法如下：

假设F_keyi对应的相机位姿为

T_keyi和T_keyj的变换矩阵为

回环优化后产生误差

则有优化问题：

其中标量k_ij表示每个误差项的权重，若F_keyi到F_keyj之间的所有c帧F_n到F_n+c-1的相机位姿分别由k_n、k_n+1……k_n+c-1个匹配特征点通过PnP算法得出，则k_ij＝min{k_n,k_n+1…k_n+c-1}。

Images