CN110570453A - 一种基于双目视觉的闭环式跟踪特征的视觉里程计方法 - Google Patents

Abstract

本发明涉及一种基于双目视觉的闭环式跟踪特征的视觉里程计方法领域。提取双目图像特征点，如果图像是第一帧，执行左右图像匹配及去除误匹配操作，利用匹配成功的左右图像特征点，计算特征点对应的世界三维坐标；当前帧非第一帧，执行跟踪机制，每一步都会记录相应标志位，用来滤除掉不合格的特征点；以左相机为参考，光流法跟踪前一帧图像与当前帧图像特征点，移除跟踪失败以及超出图像边界的点，保存跟踪的特征点在当前帧的坐标位置，运用标志位更新前一帧双目特征，及对应跟踪的世界三维坐标记为。本发明在新图像到来时，保护已跟踪特征，在已跟踪特征以外提取新的特征点，继续整个跟踪过程，这样做可以避免提取到重复的特征，以提高算法效率。

Description

一种基于双目视觉的闭环式跟踪特征的视觉里程计方法

技术领域

本发明涉及视觉里程计方法领域，特别是一种基于双目视觉的闭环式跟踪特征的视觉里程计方法领域。

背景技术

对于移动机器人要求具备的功能，通常包括目标探测、定位与自动寻路，而要实现自主导航寻路的前提就是自主定位，因此设计一个稳健且准确的定位方法至关重要。传统且经典的定位方式采用GPS、IMU、雷达定位等技术，然而GPS在室内没有信号，IMU会随时间产生严重的误差累积，而雷达设备过于昂贵。视觉传感器具有小巧、低价、获取信息具有实时性的特点，近年来，基于视觉传感器的导航定位技术取得了一定的成果，一种新的位姿估计方式应运而生，位姿估计结果即我们要得到的定位信息。相机首先利用视觉传感器来获取周围环境的图像信息，然后对这些图像进行分析和处理，如特征提取、匹配、运动恢复等，实现对周围环境的理解，进而可以获得载体的导航参数，包括位置、姿态等信息。视觉里程计根据采用的相机类型又可以分为基于单目、双目和深度相机实现。单目相机计算复杂且使用场景受限，深度相机虽然可以直接计算出场景深度，但由于其受光线辐射影响很大，只能在室内使用且价格贵，综合考虑采用双目摄像机是比较合适的选择。

从连续的图像序列中估计相机自身运动的思想，最早由Moravec等人提出，后来人们把基于影像序列恢复相机位姿和周围三维场景的问题称为运动恢复结构(StructureFrom Motion，SFM)，而视觉里程计是SFM技术的一个特例。2004年NASA发射的勇气号、机遇号火星探测漫游车顺利着陆，这两部火星车都采用了双目视觉进行定位导航，利用视觉信息，估计出图像帧与帧之间摄像机的外部参数，经图像特征提取、特征跟踪、立体匹配、运动估计等步骤完成计算，实现其在火星上的自主导航。但是，多数视觉里程计算法在特征检测、匹配与跟踪方面只采用了最基础的操作，跟踪特征的结构不够鲁棒，使得所跟踪的特征准确度不够，直接影响了后面位姿估计的结果。本发明提出一种闭环式跟踪特征的双目视觉里程计方法，提高特征准确性，进而减小位姿估计的误差。

发明内容

本发明的目的在于提供一种基于双目视觉的闭环式跟踪特征的视觉里程计方法。

本发明是这样实现的：

一种基于双目视觉的闭环式跟踪特征的视觉里程计方法，所述方法包括如下步骤：

(1)提取双目图像特征点，判断图像是否是第一帧，如果是第一帧，执行左右图像匹配及去除误匹配操作，利用匹配成功的左右图像特征点，记为p_prev0、p_prev1，计算特征点对应的世界三维坐标，记为d₃₀；

(2)当前帧非第一帧，执行跟踪机制，每一步都会记录相应标志位，用来滤除掉不合格的特征点；以左相机为参考，光流法跟踪前一帧图像与当前帧图像特征点，移除跟踪失败以及超出图像边界的点，保存跟踪的特征点在当前帧的坐标位置，记为p_{curr0_tracked}，运用标志位更新前一帧双目特征，记为p_{prev0_tracked}、p_{prev1_tracked}，及对应跟踪的世界三维坐标记为d_{30_tracked}；

(3)对当前帧进行双目之间的特征匹配及去除误匹配操作，更新匹配特征后对应的世界三维坐标；利用标志位得到新的前一帧左右图像特征点集，记为p_{prev0_matched}、p_{prev1_matched}，当前帧左右图像特征点集，记为p_{curr0_matched}、p_{curr1_matched}，和匹配特征后保留的世界三维坐标点集，记为d_{30_matched}；

(4)用光流法将当前帧左右图像特征点分别向上一帧的左右图像匹配，若匹配正确在上一帧的点应该是同一个点，将保留重匹配后像素坐标之差的模长小于阈值σ的特征点匹配对；同样利用标志位得到新的前一帧左右图像特征点集，记为p_{prev0_proj}、p_{prev1_proj}，当前帧左右图像特征点集，记为p_{curr0_proj}、p_{curr1_proj}，和重跟踪后保留的世界三维坐标点集，记为d_{30_proj}；

(5)利用跟踪成功的特征点与其对应的世界三维坐标，最小化重投影误差求解位姿，即旋转矩阵R和平移向量t；

(6)由于在双目匹配和前后帧匹配的过程中会不断剔除特征点，所以完成一次跟踪后，需要为图像增加新的特征点，新增特征点选取在上述步骤跟踪得到特征点以外；

(7)添加新特征后，进行双目匹配及去除误匹配，保留特征点记为p_prev0、p_prev1，计算世界三维坐标，记为d₃₀；

(8)循环步骤二至步骤七。

所述步骤(1)中，对第一帧图像提取Oriented FAST特征点，特征点的描述子用BRIEF描述子表示，暴力匹配法匹配双目特征点，并采用最近邻与次近邻比值算法；所述最近邻与次近邻比值算法采用比值形式其中d_fir表示最近距离值，d_sec表示次近距离值，θ根据经验设置为0.7。

所述步骤(1)中的世界三维坐标，其计算方法为：

d＝u_L-u_R(1.2)

其中，f表示相机焦距，b是两平行相机光圈中心的水平距离，u_L为像素点在左目相机成像中的x轴坐标点，u_R为像素点在右目相机成像中的x轴坐标点，d为像素点在左右两图中的横坐标之差，称为视差，v_L为像素点在左、右目相机中的y轴坐标点，u_c是图像x轴主坐标点，v_c是图像y轴主坐标点，(X,Y,Z)为世界三维坐标。

所述步骤(5)中，位姿R、t的求解方法如下：

设(X_W,Y_W,Z_W)∈d_{30_proj}，(u₀,v₀)∈p_{curr0_proj}，(u₁,v₁)∈p_{curr1_proj}，世界三维坐标投影到平面坐标系特征点位置公式如下：

其中，d_{30_proj}为重跟踪保留的世界三维坐标点集，p_{curr0_proj}为重跟踪保留的当前帧左目相机图像特征点集，p_{curr1_proj}为重跟踪保留的当前帧右目相机图像特征点集， 是估计的相机系下坐标值，是重投影估计得到的左目相机图像特征点位置，是重投影估计得到的右目相机图像特征点位置，f表示相机焦距，c_x,c_y是相机主坐标点；重投影误差表示为：

采用高斯牛顿迭代即可得到使重投影误差最小的R、t。

所述步骤(6)中，增加特征点的方法为：以处于p_curr0_proj、p_curr1_proj点集中的特征点坐标为中心，每个特征点使用大小为5×5像素值的矩形块将其覆盖，在这些点所在范围以外提取新的特征。

本发明的有益效果是：

本发明在新图像到来时，保护已跟踪特征，在已跟踪特征以外提取新的特征点，继续整个跟踪过程，这样做可以避免提取到重复的特征，以提高算法效率；本发明跟踪特征的鲁棒性好，减小特征跟丢、跟错的概率，使得位姿求解的结果误差更低；本发明保留特征的历史关联性，并在这些特征以外提取新特征，具有更高的准确性和执行效率。

附图说明

图1为闭环式跟踪特征双目视觉里程计流程框图；

图2为闭环式跟踪特征示意图；

图3为提取新特征方法示意图。

附图说明：所有的标号0代表左目相机图像，标号1代表右目相机图像。

具体实施方式

结合附图对本方法作进一步详细描述。

本发明涉及视觉里程计方法领域，特别是一种基于双目视觉的闭环式跟踪特征的视觉里程计方法领域。

针对现有的双目视觉里程计方法，本发明提供一种基于双目视觉的闭环式跟踪特征的视觉里程计方法，充分利用双目视觉所提供的信息，自前一帧左相机图像起始，到当前帧右相机图像之间形成闭环结构，执行三次去除异常特征点操作，这样的闭环结构化跟踪方法得到更准确的特征点，利用这些特征点及其对应的世界三维坐标点计算位姿。通常的双目视觉里程计，特征计算完位姿后摒弃不要了，在新到来图像全部重新提取特征，这样的做法不能很好的利用历史信息，不具有鲁棒的结构。本发明在新图像到来时，保护已跟踪特征，在已跟踪特征以外提取新的特征点，继续整个跟踪过程，这样做避免提取到重复的特征，以提高算法效率。

本方法具体实施步骤如下：

步骤一、提取双目图像特征点，判断图像是否是第一帧，如果是第一帧，执行左右图像匹配及去除误匹配操作，利用匹配成功的左右图像特征点(记为p_prev0、p_prev1)，计算特征点对应的世界三维坐标(记为d₃₀)，所有的标号0代表左图，标号1代表右图；

步骤二、当前帧非第一帧，执行跟踪机制，每一步都会记录相应标志位，用来滤除掉不合格的特征点。以左相机为参考，光流法跟踪前一帧图像与当前帧图像特征点，移除跟踪失败以及超出图像边界的点，保存跟踪的特征点在当前帧的坐标位置(记为p_{curr0_tracked})，运用标志位更新前一帧双目特征(记为p_{prev0_tracked}、p_{prev1_tracked})及对应跟踪的世界三维坐标(记为d_{30_tracked})；

步骤三、对当前帧进行双目之间的特征匹配及去除误匹配操作，更新对应的世界三维坐标。利用标志位得到匹配后保留的前一帧左右图像特征点集(记为p_{prev0_matched}、p_{prev1_matched})，匹配后保留的当前帧左右图像特征点集(记为p_{curr0_matched}、p_{curr1_matched})和匹配后保留的世界三维坐标点集(记为d_{30_matched})；

步骤四、用光流法将当前帧左右图像特征点分别向上一帧的左右图像匹配，若匹配正确在上一帧的点应该是同一个点，将保留重匹配后像素坐标之差的模长小于阈值σ的特征点匹配对。同样利用标志位得到重跟踪保留的前一帧左右图像特征点集(记为p_{prev0_proj}、p_{prev1_proj})，重跟踪保留的当前帧左右图像特征点集(记为p_{curr0_proj}、p_{curr1_proj})和重跟踪保留的世界三维坐标点集(记为d_{30_proj})；

步骤五、利用跟踪成功的特征点与其对应的世界三维坐标，最小化重投影误差求解位姿，即旋转矩阵R和平移向量t；

步骤六、由于在双目匹配和前后帧匹配的过程中会不断剔除特征点，所以完成一次跟踪后，需要为图像增加新的特征点，新增特征点选取在上述步骤跟踪得到特征点以外；

步骤七、添加新特征后，进行双目匹配及去除误匹配(保留特征点记为p_prev0、p_prev1)，计算世界三维坐标(记为d₃₀)；

步骤八、循环步骤二至步骤七。

本发明进一步描述如下：

步骤一、对于第一帧图像，由于没有参考帧，认为所有检测到的特征均为跟踪的特征。对第一帧图像提取Oriented FAST特征点，特征点的描述子用BRIEF描述子表示，暴力匹配法匹配双目特征点，并采用最近邻与次近邻比值算法减少误匹配点。最近邻与次近邻比值算法指的是，在特征查找过程中，不仅存储最近点信息，还存储次最近邻的共K个邻居的信息，这些信息按距离从小到大排列，当最近点比其余次近邻点距离小很多时才认为是正确匹配，本发明采取比值的形式其中d_fir表示最近距离值，d_sec表示次近距离值，θ根据经验设置为0.7，匹配之后保留的特征点集记为p_prev0、p_prev1。

双目相机的基线b是两平行相机光圈中心的水平距离，利用双目相机基线视差原理，求取特征点在三维世界坐标。空间点P，它在左眼相机和右眼相机各成一像。由于相机基线的存在，这两个成像位置是不同的。左右相机只在x轴上有位移，因此P在平面上的像素位置也只在x轴上有差异。记它的左侧坐标为u_L,右侧坐标为u_R。那么根据相似三角形关系，有：

d＝u_L-u_R (2)

其中d为左右图的横坐标之差，称为视差。u_c，v_c是图像主坐标点,(X,Y,Z)为世界三维坐标。

因此用匹配之后保留的特征点集，计算得到世界三维坐标点集(记作d₃₀)。

步骤二、采用光流法跟踪左相机图像，光流法把图像看作时间的函数：I(t)。那么一个在t时刻，位于(x,y)处的像素，它的灰度可以写成I(x,y,t)。某个固定的空间点，它在t时刻的像素坐标为x,y。由于相机运动，它的图像坐标将发生变化，我们希望估计这个空间点在其他时刻图像中的位置，需要做一个灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。

对于t时刻位于(x,y)处的像素，我们设t+dt时刻它运动到(x+dx,y+dy)处，其中dt表示很小的一段时间，dx,dy表示在x,y方向移动很小的距离。因为灰度不变，所以有：

I(x+dx,y+dy,t+dt)＝I(x,y,t)

(4)

左侧泰勒展开，保留一阶项：

其中，分别表示函数对于x,y,t的偏导数。

因为灰度不变，下一时刻的灰度等于之前的灰度，从而：

两边除以dt，得：

其中为像素在x轴上的运动速度，为像素在y轴上的运动速度，把它们记作u，v，记作I_x，I_y。把上式写成矩阵形式得到：

考虑在一个w×w大小的窗口里，我们会得到w²个方程：

这样得到一个关于u，v的超定方程，运用最小二乘法求解。就会得到像素在图像之间的运动速度。

根据x和y轴方向速度，得到上一帧特征在当前帧的位置，检测每个特征点像素坐标是否超出图像边界，保存未超出图像边界的左图像特征点(记作p_curr0__tracked)，保存下来的坐标点标志位设为1，未保存的点标志位设为0。结合标志位信息，未保存的特征点的标志位值为0，所以对应的世界三维坐标的标志位也是0，把标志位是0的世界三维坐标删除，更新跟踪后前一帧左右图像特征点集和跟踪后的世界三维坐标点集，分别记作p_prev0__tracked、p_prev1__tracked和d₃₀__tracked。

步骤三、运用跟踪到的左图像的特征点与右图像做双目匹配，仍采用暴力匹配结合KNN去除误匹配的方法，记录标志位，保留匹配成功特征点。利用标志位得到左右匹配后保留的前一帧左右图像特征点集(记为p_prev0__matched、p_prev1__matched)，左右匹配后保留的当前帧左右图像特征点集(记为p_curr0__matched、p_curr1__matched)和左右匹配后保留的世界三维坐标点集(记为d₃₀__matched)。

步骤四、以当前帧为参考仍采用光流法，将左右图像特征向上一帧跟踪，若跟踪成功，那么得到重跟踪保留的特征点与p₀∈p_{prev0_matched}，p'₁∈p_{prev1_proj}与p₁∈p_{prev1_matched}应该是同一个点，因此我们将保留重跟踪后像素坐标模长小于阈值σ的点。同样利用标志位得到重跟踪保留的前一帧左右图像特征点集(记为p_{prev0_proj}、p_{prev1_proj})，重跟踪保留的当前帧左右图像特征点集(记为p_{curr0_proj}、p_{curr1_proj})和重跟踪保留的世界三维坐标点集(记为d_{30_proj})。

步骤五、用之前保存下来的p_{curr0_pr}o_j、p_{curr1_proj}，世界系三维坐标点集d_{30_proj}，计算旋转矩阵R和平移向量t。设(X_W,Y_W,Z_W)∈d_{30_proj}，(u₀,v₀)∈p_{curr0_proj}，(u₁,v₁)∈p_{curr1_proj}，世界三维坐标投影到平面坐标系特征点位置公式如下：

其中，分别是估计的相机系下坐标值，和分别是重投影估计得到的左右图像特征点位置，f表示相机焦距，c_x,c_y是相机主坐标点。

重投影误差表示为：

因为相机内参均已知，因此采用高斯牛顿迭代即可得到使重投影误差最小的R、t。至此，求得当前帧相机位姿。

步骤六、经过一系列去除误匹配操作之后的图像特征会减少很多，如果不添加新的特征，每次跟踪继续减少，几次循环后算法将无法进行下去。在提取新的特征时，以处于p_curr0_proj、p_curr1_proj点集中的特征点坐标为中心，每个特征点使用大小为5×5像素值的矩形块将其覆盖，在这些点所在范围以外提取新的特征。这样做的好处是保留经过闭环式跟踪得到的优质特征点，增加了特征的历史相关性，且避免了重复检测到优质特征点所需额外的时间。

步骤七、提取新的特征后和原来在p_{curr0_proj}、p_{curr1_proj}点集中的点都存放到新的点集中，做双目之间匹配和去除误匹配操作，计算世界三维坐标。匹配成功的左右图像特征点集仍记为p_prev0、p_prev1，对应的世界三维坐标点集仍记为d₃₀。

步骤八、循环步骤二至步骤七，就实现了闭环式跟踪特征的双目视觉里程计。

综上所述，本发明公开了一种闭环式跟踪特征的双目视觉里程计方法。包括以下步骤：1)对第一帧提取双目图像特征点，执行左右图像匹配及去除误匹配操作，利用保留的匹配点计算对应的三维坐标；2)新图像到来，光流法跟踪前一帧左图像与当前帧左图像特征点，移除跟踪失败以及超出图像边界的点；3)对当前帧进行双目之间的特征匹配及去除误匹配操作；4)再用光流法将当前帧左右图像特征点分别向上一帧的左右图像匹配，保留重匹配后像素坐标之差的模长小于阈值的特征点匹配对；5)利用跟踪成功的特征点与其对应的世界三维坐标，最小化重投影误差求解位姿，即旋转矩阵R和平移向量t；6)以已跟踪特征点坐标位置为中心，表面覆盖上一个大小为5×5像素值的矩形块，在矩形块覆盖范围以外为图像增加新的特征点；7)添加新特征后，进行双目匹配及去除误匹配操作；8)跳转回到步骤2)重新循环。本发明跟踪特征的结构鲁棒性好，效率高。

Claims (5)

1.一种基于双目视觉的闭环式跟踪特征的视觉里程计方法，其特征在于：所述方法包括如下步骤：

(1)提取双目图像特征点，判断图像是否是第一帧，如果是第一帧，执行左右图像匹配及去除误匹配操作，利用匹配成功的左右图像特征点，记为p_prev0、p_prev1，计算特征点对应的世界三维坐标，记为d₃₀；

(2)当前帧非第一帧，执行跟踪机制，每一步都会记录相应标志位，用来滤除掉不合格的特征点；以左相机为参考，光流法跟踪前一帧图像与当前帧图像特征点，移除跟踪失败以及超出图像边界的点，保存跟踪的特征点在当前帧的坐标位置，记为p_{curr0_tracked}，运用标志位更新前一帧双目特征，记为p_{prev0_tracked}、p_{prev1_tracked}，及对应跟踪的世界三维坐标记为d_{30_tracked}；

(3)对当前帧进行双目之间的特征匹配及去除误匹配操作，更新匹配特征后对应的世界三维坐标；利用标志位得到新的前一帧左右图像特征点集，记为p_{prev0_matched}、p_{prev1_matched}，当前帧左右图像特征点集，记为p_{curr0_matched}、p_{curr1_matched}，和匹配特征后保留的世界三维坐标点集，记为d_{30_matched}；

(4)用光流法将当前帧左右图像特征点分别向上一帧的左右图像匹配，若匹配正确在上一帧的点应该是同一个点，将保留重匹配后像素坐标之差的模长小于阈值σ的特征点匹配对；同样利用标志位得到新的前一帧左右图像特征点集，记为p_{prev0_proj}、p_{prev1_proj}，当前帧左右图像特征点集，记为p_{curr0_proj}、p_{curr1_proj}，和重跟踪后保留的世界三维坐标点集，记为d_{30_proj}；

(5)利用跟踪成功的特征点与其对应的世界三维坐标，最小化重投影误差求解位姿，即旋转矩阵R和平移向量t；

(6)由于在双目匹配和前后帧匹配的过程中会不断剔除特征点，所以完成一次跟踪后，需要为图像增加新的特征点，新增特征点选取在上述步骤跟踪得到特征点以外；

(7)添加新特征后，进行双目匹配及去除误匹配，保留特征点记为p_prev0、p_prev1，计算世界三维坐标，记为d₃₀；

(8)循环步骤二至步骤七。

2.根据权利要求1所述的一种基于双目视觉的闭环式跟踪特征的视觉里程计方法，其特征在于：所述步骤(1)中，对第一帧图像提取Oriented FAST特征点，特征点的描述子用BRIEF描述子表示，暴力匹配法匹配双目特征点，并采用最近邻与次近邻比值算法；所述最近邻与次近邻比值算法采用比值形式其中d_fir表示最近距离值，d_sec表示次近距离值，θ根据经验设置为0.7。

3.根据权利要求1所述的一种基于双目视觉的闭环式跟踪特征的视觉里程计方法，其特征在于：所述步骤(1)中的世界三维坐标，其计算方法为：

d＝u_L-u_R (1.2)

其中，f表示相机焦距，b是两平行相机光圈中心的水平距离，u_L为像素点在左目相机成像中的x轴坐标点，u_R为像素点在右目相机成像中的x轴坐标点，d为像素点在左右两图中的横坐标之差，称为视差，v_L为像素点在左、右目相机中的y轴坐标点，u_c是图像x轴主坐标点，v_c是图像y轴主坐标点，(X,Y,Z)为世界三维坐标。

4.根据权利要求1所述的一种基于双目视觉的闭环式跟踪特征的视觉里程计方法，其特征在于：所述步骤(5)中，位姿R、t的求解方法如下：

设(X_W,Y_W,Z_W)∈d_{30_proj}，(u₀,v₀)∈p_{curr0_proj}，(u₁,v₁)∈p_{curr1_proj}，世界三维坐标投影到平面坐标系特征点位置公式如下：

其中，d_{30_proj}为重跟踪保留的世界三维坐标点集，p_{curr0_proj}为重跟踪保留的当前帧左目相机图像特征点集，p_{curr1_proj}为重跟踪保留的当前帧右目相机图像特征点集， 是估计的相机系下坐标值，是重投影估计得到的左目相机图像特征点位置，是重投影估计得到的右目相机图像特征点位置，f表示相机焦距，c_x,c_y是相机主坐标点；重投影误差表示为：

采用高斯牛顿迭代即可得到使重投影误差最小的R、t。

5.根据权利要求1所述的一种基于双目视觉的闭环式跟踪特征的视觉里程计方法，其特征在于：所述步骤(6)中，增加特征点的方法为：以处于p_curr0_proj、p_curr1_proj点集中的特征点坐标为中心，每个特征点使用大小为5×5像素值的矩形块将其覆盖，在这些点所在范围以外提取新的特征。