[发明专利]一种基于GMS特征匹配及滑动窗口位姿图优化的视觉里程计

权利要求书

1.一种基于GMS特征匹配及滑动窗口位姿图优化的RGB-D视觉里程计，其特征在于，包括以下步骤：

S1.通过RGB-D相机读取第一帧RGB图像作为参考帧，读取第一帧深度图像作为参考帧的深度信息，对参考帧提取特征点并计算ORB特征描述子；

S2.读取下一帧RGB图像作为当前帧，读取下一帧深度图像作为当前帧的深度信息，对当前帧提取特征点并计算ORB特征描述子；

S3.对从参考帧与当前帧提取的特征点进行初步的特征匹配；

S4.对步骤S3得到的特征匹配点对通过GMS算法剔除错误匹配；

S5.经过步骤S4得到参考帧与当前帧的二维-二维匹配点对，将参考帧中经过步骤S4筛选的特征点利用相机投影模型以及参考帧的深度信息投影到三维空间中，得到特征点的三维空间坐标，由此将二维-二维匹配点对转换成三维-二维匹配点对；

S6.最小化重投影误差，通过迭代优化得到初步的帧间相对位姿；

S7.最小化亮度误差，将步骤S6得到的初步帧间相对位姿作为迭代优化的初始值，得到经过第二次优化的帧间相对位姿；

S8.滑动窗口位姿图优化，选取当前帧及其前面的帧的位姿作为窗口中要进行迭代优化的位姿；采用Graph图来表示位姿优化问题，其中顶点为每帧的位姿，边为两帧之间的相对位姿，误差的计算公式如下：

其中，T_ij表示第j帧到第i帧的相对运动，T_i,T_j分别表示第i帧和第j帧的位姿；将窗口外的图像帧的位姿保留在Graph图中，但在进行迭代时，对这些窗口外的图像帧的位姿进行边缘化，不进行更新；

S9.将当前帧作为参考帧，当前帧的深度信息作为参考帧的深度信息，返回步骤S2。

2.根据权利要求1所述的一种基于GMS特征匹配及滑动窗口位姿图优化的RGB-D视觉里程计，其特征在于，步骤S1中提取的特征点为图像中快速段测试特征FAST角点的像素点位置；将角点的亮度值与其周围128个像素点的亮度进行比较，比角点亮记为1，反之记为0，最后产生一个128维的二进制向量作为该角点的特征描述子。

3.根据权利要求2所述的一种基于GMS特征匹配及滑动窗口位姿图优化的RGB-D视觉里程计，其特征在于，步骤S3具体包括：用汉明距离作为两个特征点相似度的度量，将参考帧上的每一个特征点与当前帧上的所有特征点逐一计算汉明距离，选取汉明距离最小的特征点作为匹配点，产生一对匹配点对。

4.根据权利要求3所述的一种基于GMS特征匹配及滑动窗口位姿图优化的RGB-D视觉里程计，其特征在于，GMS算法基于运动的平滑性提出了一种假设：第一帧图像上的一个特征点P₁在第二帧图像上的匹配点为P₂，若该匹配为正确匹配，则以P₁为中心的3*3网格中的特征点的匹配点都大概率地落在第二帧图像以P₂为中心的3*3网格中；基于该假设通过对两帧图像进行网格划分并对对应网格区域内的匹配点数进行统计，若匹配点数大于阈值T₀，则认为特征匹配点对为正确匹配点对，反之为错误匹配点对，其中T₀的计算公式如下：

其中，n为每个网格中的特征点的平均个数；α为可自定义的参数。

5.根据权利要求4所述的一种基于GMS特征匹配及滑动窗口位姿图优化的RGB-D视觉里程计，其特征在于，所述的相机投影模型的计算公式为：

P＝dK^-1p

其中，p为特征点的像素坐标，K为相机内参，d为特征点的深度，P为特征点三维空间坐标。

6.根据权利要求4所述的一种基于GMS特征匹配及滑动窗口位姿图优化的RGB-D视觉里程计，其特征在于，所述的最小化重投影误差的目标函数为：

ε^*＝argmin_ε|(π(T(P；ε))-p)|²

其中，ε为待估计的参考帧与当前帧之间的相对位姿，T表示参考帧到当前帧的位姿变换，π表示相机的投影模型，即将三维空间投影到图像上的变换；p为特征点的像素坐标；P为特征点三维空间坐标。