一种基于GMS特征匹配及滑动窗口位姿图优化的RGB-D视觉里程计的制作方法

本发明属于计算机视觉领域，更具体地，涉及一种基于gms特征匹配及滑动窗口位姿图优化的rgb-d视觉里程计。

背景技术：

视觉里程计是指通过机器视觉技术，分析相关图像序列来实时估计机器人位姿，能克服传统里程计的不足，更加精确地进行定位，并且可以运行在gps(全球定位系统)无法覆盖的环境下，例如室内环境，星际探索等。视觉里程计在移动机器人定位和导航领域得到了广泛关注和应用。

目前，视觉里程计的两种主流方法分别是特征点法和直接法。特征点法主要通过三个步骤，即特征提取，特征匹配，最小化重投影误差(reprojectionerror),来估计图像帧之间的相对位姿。特征点法作为最早兴起的视觉里程计解决方案，一直被认为是视觉里程计的主流方法，这种方法运行稳定，对动态物体不敏感，是目前比较成熟的解决方案，但该方法也存在一定的问题。特征点法的特征点提取和匹配步骤存在比较耗时以及会有错误匹配的问题。当图像存在运动模糊，光照条件差，纹理大量重复或缺少纹理时，特征点法的准确性会受到较大的影响。直接法的原理基于一个称为亮度不变性的假设，该假设认为，两帧图像上对应的像素点应具有相同的亮度度值。基于这个假设，通过相机模型直接使用像素点的亮度值构造光度误差，并通过最小化光度误差(photometricerror)来估计帧间位姿。根据使用的像素点数量，直接法又可以分为稠密和半稠密两种，其中稠密的直接法会使用图像上的所有像素点来计算光度误差，因此计算量巨大。半稠密的直接法只使用有一定梯度信息的像素点来计算光度误差，这样的处理在保持相对位姿估计的准确性的同时也让直接法具备一定的实时性。直接法在相机相对运动较小时能够得到鲁棒且准确的位姿估计结果，同时，对于图像信息的充分利用，使得其在图像出现运动模糊，重复纹理以及纹理缺失的情况仍能保持较好的准确性。直接法的主要问题在于，亮度不变假设是一种较为强迫性的假设，即该假设在亮度差异较小的情况下能够认为是成立的，但在亮度差异较大的情况，该假设很有可能不成立，这种情况下直接法的视觉里程计的精确度会有较大程度的下降。

在使用特征点法或是直接法实现视觉里程计时，一般情况下都不会只单纯地对图像序列帧做两两之间的相对位姿估计，通常会通过一些技术手段的方式来减少累计误差。这些技术手段主要包括维护局部地图以及设计更加复杂光度误差计算方法。维护局部地图需要对地图进行插入新的地图点和删除旧地图点的操作，这两种操作对会增加计算量，降低视觉里程计的实时性。通过设计更加复杂的光度误差计算方法能够有效减少累计误差，但同时，在最小化光度误差时也需要更多的计算量，降低系统的实时性。

技术实现要素：

本发明为克服上述现有技术所述的至少一种缺陷，提供一种基于gms特征匹配及滑动窗口位姿图优化的rgb-d视觉里程计，采用gms(网格运动统计)算法进行误匹配剔除，在图像间相对运动较大且亮度变化较大时仍然能够筛选出足够数量的正确匹配点对，提高了系统的鲁棒性；采用滑动窗口位姿图优化技术减小位姿估计的累计误差，具有更高的实时性。

为解决上述技术问题，本发明采用的技术方案是：一种基于gms特征匹配及滑动窗口位姿图优化的rgb-d视觉里程计，包括以下步骤：

步骤1.通过rgb-d相机读取获取第一帧rgb图像作为参考帧，读取第一帧深度图像作为参考帧的深度信息，对参考帧提取特征点并计算orb特征描述子；提取的特征点为图像中fast(快速段测试特征)角点的像素点位置。orb特征通过将角点的亮度值与其周围128个像素点的亮度进行比较，比关键点亮记为1，反之记为0，最后产生一个128维的二进制向量作为该关键点的特征描述子。

步骤2.读取下一帧rgb图像作为当前帧，读取下一帧深度图像作为当前帧的深度信息，对当前帧进行提取特征点并计算orb特征描述子。

步骤3.对参考帧与当前帧提取的像素点进行初步的特征匹配；用汉明距离作为两个特征点相似度的度量，将参考帧上的每一个特征点与当前帧上的所有特征点逐一计算汉明距离，选取汉明距离最小的特征点作为匹配点，产生一对匹配点对。

步骤4.对第二步得到的特征匹配点对通过gms(网格运动统计)算法剔除错误匹配；gms(网格运动统计)算法基于运动的平滑性提出了一种假设：第一帧图像上的一个特征点p1在第二帧图像上的匹配点为p2，若该匹配为正确匹配则以p1为中心的3*3网格中的特征点的匹配点都大概率地落在第二帧图像以p2为中心的3*3网格中。基于该假设通过对两帧图像进行网格划分并对对应网格区域内的匹配点数进行统计，若匹配点数大于阈值t，这认为该匹配对为正确匹配对，反之为错误匹配对，其中t的计算公式如下：

其中，n为每个网格中的特征点的平均个数；在本方案中取α值为6；基于gms(网格运动统计)算法的特征匹配作为本发明的关键技术之一，具有以下效果：

1.相较于一般的ransac(随机样本一致性算法)算法，gms(网格运动统计)在帧间运动相对较大以及帧间亮度变化较大的情况下仍能筛选出足够数量的正确匹配对，这在一定程度上保证了后续位姿计算的准确度；

2.该算法基于统计学理论对误匹配进行剔除，具有较高的实时性。

步骤5.经过步骤s3，可以得到参考帧与当前帧的二维-二维匹配点对，在这一步里，需要将参考帧中经过步骤s3筛选的特征点利用相机投影模型以及参考帧的深度信息投影到三维空间中，得到特征点的三维空间坐标，由此将二维-二维匹配点对转换成三维-二维匹配点对；相机投影模型的计算公式为：

p＝dk^-1p

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

步骤6.最小化重投影误差，最小化重投影误差的目标函数为：

ε^*＝argminε|(π(t(p；ε))-p)|²

其中，ε为待估计的参考帧与当前帧之间的相对位姿，t表示将参考帧到当前帧的位姿变换，π表示相机的投影模型，即将三维空间投影到图像上的变换。

步骤7.最小化亮度误差，最小化亮度误差的目标函数为：

ε^*＝argminε|i2(π((p；ε))-i2(p)|²

其中，i2(*)表示当前帧的像素点的亮度值。

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

其中，tij表示第j帧到第i帧的相对运动，ti,tj分别表示第i帧和第j帧的位姿；对于窗口外的图像帧的位姿，仍然将他们保留在graph图中，但在进行迭代时，对这些帧的位姿进行边缘化，并不对其进行更新。

滑动窗口优化作为本方案的另一个关键技术点，具有以下效果：

1.为当前帧的位姿计算提供了全局的信息，能够有效地减小累计误差，提高视觉里程计的精确度；

2.对于窗口外的图像帧的位姿，本方案采取的做法是将其保留在图(graph)中，但对其进行边缘化以及不对其进行迭代更新，这样可以将位姿图优化保持在一个固定大小的规模下，较少迭代次数，提高视觉里程计的实时性。

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

与现有技术相比，有益效果是：

1.采用两两帧间无结构式的相对位姿估计，无需建立局部地图并对其进行维护，提高了视觉里程计的实时性；

2.采用基于gms(网格运动统计)算法的特征匹配，保证在帧间运动较大以及亮度变化较大的情况下仍然能够筛选出足够数量的匹配点对供后续位姿计算时使用，提高了视觉里程计的鲁棒性；

3.过滑动窗口位姿图优化，对估计出来的位姿进行非线性优化，提高了视觉里程计的精确度，同时，通过窗口大小对位姿图优化的规模进行约束，降低非线性优化过程的迭代次数，保证了视觉里程计的实时性。

附图说明

图1是本发明的方法流程图。

图2是本发明实施例中滑动窗口位姿图优化示意图。

图3是本发明采用的graph图的结构示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

实施例1：

如图1所示，一种基于gms特征匹配及滑动窗口位姿图优化的rgb-d视觉里程计，包括以下步骤：

步骤1.通过rgb-d相机读取获取第一帧rgb图像作为参考帧，读取第一帧深度图像作为参考帧的深度信息，对参考帧提取特征点并计算orb特征描述子；提取的特征点为图像中fast(快速段测试特征)角点的像素点位置。orb特征通过将角点的亮度值与其周围128个像素点的亮度进行比较，比关键点亮记为1，反之记为0，最后产生一个128维的二进制向量作为该关键点的特征描述子。

步骤2.读取下一帧rgb图像作为当前帧，读取下一帧深度图像作为当前帧的深度信息，对当前帧进行提取特征点并计算orb特征描述子。

步骤3.对参考帧与当前帧提取的像素点进行初步的特征匹配；用汉明距离作为两个特征点相似度的度量，将参考帧上的每一个特征点与当前帧上的所有特征点逐一计算汉明距离，选取汉明距离最小的特征点作为匹配点，产生一对匹配点对。

步骤4.对第二步得到的特征匹配点对通过gms(网格运动统计)算法剔除错误匹配；gms(网格运动统计)算法基于运动的平滑性提出了一种假设：第一帧图像上的一个特征点p1在第二帧图像上的匹配点为p2，若该匹配为正确匹配则以p1为中心的3*3网格中的特征点的匹配点都大概率地落在第二帧图像以p2为中心的3*3网格中。基于该假设通过对两帧图像进行网格划分并对对应网格区域内的匹配点数进行统计，若匹配点数大于阈值t，这认为该匹配对为正确匹配对，反之为错误匹配对，其中t的计算公式如下：

其中，n为每个网格中的特征点的平均个数；在本方案中取α值为6。

步骤5.经过步骤s3，可以得到参考帧与当前帧的二维-二维匹配点对，在这一步里，需要将参考帧中经过步骤s3筛选的特征点利用相机投影模型以及参考帧的深度信息投影到三维空间中，得到特征点的三维空间坐标，由此将二维-二维匹配点对转换成三维-二维匹配点对；相机投影模型的计算公式为：

p＝dk^-1p

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

步骤6.最小化重投影误差，最小化重投影误差的目标函数为：

ε^*＝argminεi(π(t(p；ε))-p)1²

其中，ε为待估计的参考帧与当前帧之间的相对位姿，t表示将参考帧到当前帧的位姿变换，π表示相机的投影模型，即将三维空间投影到图像上的变换；

根据上式建立重投影误差最小二乘目标函数，采用lm(利文伯格-马夸特)迭代算法进行迭代优化，得到初步的帧间相对位姿。

步骤7.最小化亮度误差，最小化亮度误差的目标函数为：

ε^*＝argminε|i2(π((p；ε))-i2(p)|²

其中，i2(*)表示当前帧的像素点的亮度值；

根据上式建立亮度误差最小二乘目标函数，将步骤7得到的初步位姿作为此步骤迭代优化的初始值，采用lm(利文伯格-马夸特)迭代算法进行迭代优化，得到经过第二次优化的帧间相对位姿。

步骤8.如图2所示，滑动窗口位姿图优化，选取当前帧及其前面的9帧共计10帧的位姿作为窗口中要进行迭代优化的位姿；如图3所示本发明采用graph图来表示该位姿优化问题，其中顶点为每帧的位姿，边为两帧之间的相对位姿，误差的计算公式如下：

其中，tij表示第j帧到第i帧的相对运动，ti,tj分别表示第i帧和第j帧的位姿；对于窗口外的图像帧的位姿，仍然将他们保留在graph图中，但在进行迭代时，对这些帧的位姿进行边缘化，并不对其进行更新。

利用g2o库建立窗口优化的位姿图，设置好块求解器，线性方程求解器，迭代优化算法以及迭代次数，初始化后调用求解器优化接口进行位姿图优化。

步骤9.对经过位姿图优化的参考帧与当前帧的相对位姿进行存储。

步骤10.将当前帧作为参考帧，返回步骤2。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。