[发明专利]基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法

权利要求书

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，k0，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的原理同时优化相机位姿和特征点三维世界坐标，0nk，若在步骤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，继续下一帧的计算，直到程序结束。