基于光流跟踪和点线特征匹配的双目视觉里程计设计方法与流程

本发明设计属于机器人定位与导航技术领域，具体涉及一种基于光流跟踪和点线特征匹配的双目视觉里程计设计方法。

背景技术：

视觉里程计(vo)作为导航系统的重要组成部分，在机器人技术中得到了广泛的应用，如无人驾驶汽车、无人机等。在这些应用中，vo的其中一种替代方案是使用惯性测量单元(imu)，但是这种方案的缺点在于：由于它们无法精确地抵消重力效应，随着时间的推移，会积累大量误差。传统的替代方案还包括了轮式里程计以及基于gps的导航系统等，其中轮式里程计不能取代vo的原因在于：它有较大的测量误差，长时间使用精度很差；而基于gps的导航系统方案的缺点在于：这些系统仅限于开放的、不遮挡的室外环境使用，而且还无法估计出它们所连接的设备的方向。vo技术可以很大程度上弥补以上导航方案的缺陷，vo的另一个优点是所需的信息(由相机提供)可用于slam和场景识别等其他与导航相关的任务。

在现有的vo算法当中，基于光流跟踪的方法，由于不需要计算描述子和匹配描述子，可节省出很大的计算量，算法速度快，较好地满足实时性的要求。但是，光流跟踪法的缺点在于容易受光照影响，弱纹理场景效果不好，并且当相机发生大尺度移动时，无法很好地跟踪；基于点特征的方法，在相机移动尺度过大时，相较于光流跟踪法而言具有较高的鲁棒性，且由于特征点之间是通过描述子的方式进行匹配的，故算法精度较高，但是，点特征法的缺点在于：点特征的提取和匹配计算量较大，花费比较多的时间，且在弱纹理的场景中，无法正常工作；基于线特征的方法，在弱纹理的场景中，相较于点特征法而言具有较高的鲁棒性，但是缺点在于线特征的提取与匹配相较于点特征而言，计算量较大。

技术实现要素：

要解决的技术问题

为了解决现有的vo技术中采用光流法定位精度不高以及采用点线特征方法实时性不强的问题，本发明提供一种基于光流跟踪和点线特征匹配的双目视觉里程计设计方法。

技术方案

一种基于光流跟踪和点线特征匹配的双目视觉里程计设计方法，其特征在于步骤如下：

步骤1：使用双目相机采集图像，首先将图像转换为灰度图像，然后利用自适应直方图均衡算法对图像做增强处理；

步骤2：使用klt光流法，对前一帧图像的点特征进行跟踪，并结合双向环形匹配策略建立前后帧之间点特征的数据关联：

步骤2.1：使用klt光流法，当前帧的左目图像跟踪上一帧的左目图像，进行点特征的跟踪匹配；

步骤2.2：筛选并补充点特征；

步骤2.3：当前帧的左、右目图像之间使用klt光流法进行点特征跟踪匹配，进而根据例立体视觉算法计算出匹配点对应的三维坐标；

步骤3：提取当前帧的线特征：

步骤3.1：基于lsd线特征提取算法提取线特征，基于lbd线特征描述算法计算线特征描述子；

步骤3.2：对所提取到的线特征，进行断线合并操作；

步骤3.3：当前帧的左、右目图像之间进行线特征匹配；

步骤4：前后帧之间进行线特征的匹配；

步骤5：基于前后帧之间点特征的数据关联结果，采用pnp位姿估计方法获得当前帧的初始位姿估计；

步骤6：利用前后帧之间的点特征和线特征的重投影误差所构建的代价函数，对当前帧的位姿进行优化调整；

步骤7：判断当前帧是否为关键帧，如果是，则执行步骤8；如果不是，则设定上一个关键帧为当前帧的参考关键帧，然后，跳转至步骤12；

步骤8：提取当前帧的点特征：

步骤8.1：采用orb算法对当前帧进行点特征的提取和描述；

步骤8.2：当前帧的左、右目图像之间的点特征通过描述子匹配的方式，进行稀疏立体匹配；

步骤9：局部关键帧之间进行点、线特征的数据关联；

步骤10：利用局部关键帧之间的点特征和线特征的重投影误差所构建的代价函数，对局部关键帧的位姿进行再次的优化调整；

步骤11：对非关键帧的位姿进行调整；

步骤12：输出全部图像帧的位姿。

所述步骤2中的双向环形匹配策略指的是：将上一帧左目图像中的特征点集通过双向klt光流法的方式在当前帧左目图像中跟踪匹配到特征点集将当前帧左目图像中的特征点集通过双向klt光流法的方式在当前帧右目图像中跟踪匹配到临时特征点集x1；将当前帧右目图像中的临时特征点集x1通过双向klt光流法的方式在上一帧右目图像中跟踪匹配到临时特征点集x2；判断临时特征点集x2中的特征点是否落入原特征点集中的原特征点的领域范围，对落入领域范围以外的特征点从x2中删除，同时根据x1与x2的匹配关系，从x1中删除对应于x2中已删除的那些特征点，从而获得当前帧右目图像中的特征点集

所述步骤4中，成功匹配的两条线段l1,l2满足了以下条件：

(1)lsd检测的线段具有方向性，两匹配线段的方向向量的夹角小于φ；

(2)两线段的长度的比值

(3)计算两线段重叠区域的长度loverlap，

(4)对应lbd特征向量的距离小于ρt，并且是其中距离最小的。

所述步骤7中，关键帧的判断原则为：同时满足下列条件：

(1)与上一个关键帧之间至少含有20个图像帧；

(2)至少成功追踪到了50个点特征和15条线特征；

(3)与上一个关键帧之间的共视点、线特征信息少于75％。

有益效果

本发明所提出的一种基于光流跟踪和点线特征匹配的双目视觉里程计设计方法，首先在基于点特征的视觉里程计算法中引入线特征，即：使用点线特征相结合的特征匹配方式，提高了算法的鲁棒性和定位精度，即使在弱纹理场景中依然表现良好；同时，在基于点线特征的视觉里程计算法中引入光流法跟踪匹配点特征，提高了算法的快速性。该方法兼具到了光流跟踪法、点特征法和线特征法各自的优点，克服了单用光流跟踪法、单用点特征法和单用线特征法的缺点。

附图说明

图1为本发明实施例的流程图

图2为双向环形匹配策略

图3为线特征合并策略

图4为线特征的重投影误差示意图

图5为在mh_04_difficult图像序列上的实验结果

图6为ape结果比较

具体实施方式

现结合实施例、附图对本发明作进一步描述：

实验采用国际公认数据集euroc中的mh_04_difficult图像序列对本算法进行测试，实验所用计算机的配置为：cpu为a4-5000，主频为1.50ghz，内存为8gb,系统为ubuntu16.04。

euroc数据集是由搭载了视觉、惯导、雷达等传感器的无人机作为数据采集平台，其中，以毫米级的激光雷达所追踪到的定位数据作为无人机的运动真值。mh_04_difficult图像序列是无人机在光线较暗的室内厂房里，以0.93m/s的速度,0.24rad/s的角速度，运动了91.7m所采集到的,其中，相机以20hz的频率采集数据。

图1为本发明实施例的流程图。如图1所示，本发明所提出的一种基于光流跟踪和点线特征匹配的双目视觉里程计设计方法，包括以下步骤：

步骤1：读取双目图像，对图像进行预处理；

从mh_04_difficult图像序列中读取双目图像，其中，图像预处理方法为：首先将图像转换为灰度图像，然后利用自适应直方图均衡算法对图像做增强处理。

步骤2：使用klt光流法，对前一帧图像的点特征进行跟踪，并结合双向环形匹配策略建立前后帧之间点特征的数据关联，包括如下步骤：

步骤2.1：使用klt光流法，当前帧的左目图像跟踪上一帧的左目图像，进行点特征的跟踪匹配；

光流跟踪描述的是图像上的像素点随着时间，在图像之间运动的一种方法，即：对于前一帧中的像素点，确定出在后一帧即将出现的位置。光流跟踪匹配点特征的过程就是跟踪像素点的过程。

光流法把图像看作时间的函数i(t)，那么，一个在t时刻，位于(x,y)处的像素点，它的灰度可以写成i(x,y,t)。对于同一个空间点，为了确定出它在下一帧即将出现的位置，光流法做出了一个基本假设：灰度不变假设，即：同一个空间点的像素灰度值，在各个图像中是固定不变的。

t时刻，图像坐标(x,y)处的像素点，它在t+dt时刻运动到了图像坐标(x+dx,y+dy)处，根据灰度不变假设，有：

i(x+dx,y+dy,t+dt)＝i(x,y,t)(1)

对上式左侧进行泰勒展开，保留一阶项，得：

根据灰度不变假设，同一个空间点在t+dt时刻的灰度值与t时刻的灰度值是相同的，于是，得：

上式左右两侧同除以dt，得：

其中，dx/dt为像素点处在x轴方向的运动速度，记为：u；dy/dt为像素点处在y轴方向的运动速度，记为：v。同时，为像素点处在x轴方向的梯度，记为：ix；为像素点处在y轴方向的梯度，记为：iy。另外，记it为图像灰度对时间的变化量，其矩阵形式如下：

计算出像素点在图像间的运动速度u,v后，便可以估计出在下一时刻，像素点在下一帧图像中所出现的位置。

图2为双向环形匹配策略。如图2所示，首先，将上一帧左目图像中的特征点集通过双向klt光流法的方式在当前帧左目图像中跟踪匹配到特征点集将当前帧左目图像中的特征点集通过双向klt光流法的方式在当前帧右目图像中跟踪匹配到临时特征点集x1；将当前帧右目图像中的临时特征点集x1通过双向klt光流法的方式在上一帧右目图像中跟踪匹配到临时特征点集x2；判断临时特征点集x2中的特征点是否落入原特征点集中的原特征点的领域范围，对于落入领域范围以外的特征点从x2中删除，同时根据x1与x2的匹配关系，从x1中删除对应于x2中已删除的那些特征点，从而获得当前帧右目图像中的特征点集

双向klt光流法指的是：假设对图像picture1、图像picture2进行点特征匹配，首先从图像picture1到图像picture2使用klt光流法进行点特征跟踪，将跟踪失败的点以及跟踪到图像边缘处的点剔除后，然后，将剩余的点再次反向从图像picture2到图像picture1使用klt光流法进行点特征的反向跟踪，再次将跟踪失败的点以及跟踪到图像边缘处的点剔除后，将剩余的点作为图像picture1与图像picture2之间相匹配的点。

步骤2.2：筛选并补充点特征。

在步骤2.1中，使用光流跟踪法，当前帧的左目图像跟踪上一帧的左目图像，进行点特征跟踪，判断成功跟踪上的点特征个数，通过shi-tomasi角点检测方法提取当前帧左目图像中的点特征，将落入原点特征设定范围领域的点特征删除，得到新增点特征，将当前帧左目图像中的点特征补充至200个。

步骤2.3：当前帧的左、右目图像之间使用klt光流法进行点特征跟踪匹配，进而根据例立体视觉算法计算出匹配点对应的三维坐标。其中，根据立体视觉算法计算匹配点对应的三维坐标，计算过程如下：

d＝ul-ur(6)

其中，(x,y,z)为空间点在相机坐标系下的三维坐标，f表示相机焦距，b表示双目相机的基线，cx,cy是相机内参，ul为空间点在左目图像中x轴方向的像素坐标，ur为空间点在右目图像中x轴方向的像素坐标，vl为空间点在左目图像中y轴方向的像素坐标，d为空间点在左、右目图像中像素点的横坐标之差，即：视差。

步骤3：提取当前帧的线特征，包括如下步骤：

步骤3.1：基于lsd线特征提取算法提取线特征，基于lbd线特征描述算法计算线特征描述子；

步骤3.2：对所提取到的线特征，进行断线合并操作；

图3为线特征合并策略。如图3所示，线特征合并主要包括以下子步骤：

步骤3.2.1：线特征集合l＝{li}i＝1,...,n按照长度大小进行排序，并选取其中最长的线段lmax；

步骤3.2.2：在lmax与l的成员之间进行求取主方向差dist(angi,angj)，点线距离d，端点距离l，描述子距离t，根据所设定的阈值，从集合l中找出与lmax比较接近线段，组成候选线段组l′。

步骤3.2.3：l′中的成员与lmax进行线段合并，并重新计算描述子，得到合并后线段组l″。

步骤3.2.4：计算l″i的线段长度s，以及l″i与lmax之间的主方向差，并判断是否小于设定阈值。找出与lmax产生了最小的主方向差的待合并线段l′i，并且用合并线段l″i来代替l′i和lmax。

其中，des＝{desi}i＝1,...,n为线特征集合l＝{li}i＝1,...,n所对应的描述子集合，angth为主方向差阈值，dth为点线距离阈值，lth为端点距离阈值，desth为描述子距离阈值，sth为线段长度阈值。

步骤3.3：当前帧的左、右目图像之间进行线特征匹配。

步骤4：前后图像帧之间进行线特征的匹配。

成功匹配的两条线段l1,l2满足了以下条件：

⑴lsd检测的线段具有方向性，两匹配线段的方向向量的夹角小于φ；

⑵两线段的长度的比值

⑶计算两线段重叠区域的长度loverlap，

⑷对应lbd特征向量的距离小于ρt，并且是其中距离最小的。

步骤5：基于前后帧之间点特征的数据关联结果，采用pnp位姿估计方法获得当前帧的初始位姿估计；

步骤6：利用前后帧之间的点特征和线特征的重投影误差所构建的代价函数，对当前帧的位姿进行优化调整；

点特征与位姿的约束关系如下：

pc＝[pxpypz]^t＝rcwpw+tcw(8)

其中，φ表示针孔相机的投影模型。

空间点特征的重投影误差定义如下：

上式中，(xi,j,yi,j)表示第j个点特征在第i帧图像上的像素坐标。

图4为线特征的重投影误差示意图，其中，sw,k,是第i帧图像所观测到的第k条线特征在世界坐标系下的两个3d端点，是将sw,k,ew,k重投影到第i帧图像上的两个2d端点，si,k,是sw,k,ew,k在第i帧图像中对应的两个端点，^hsi,k,分别是si,k,ei,k对应的齐次坐标点，ds,de分别表示两个端点各自到重投影线段的距离。

π是由两个齐次坐标点^hsi,k,^hei,k以及相机的光心ci所确定的平面，用li,k表示平面π的单位法向量：

空间线特征的重投影误差定义如下：

另外，^hφ为φ的齐次坐标形式，即：

在假设观测误差为高斯分布的情况下，可以构建综合了点线特征的重投影误差的代价函数f：

其中，分别是点特征、线特征重投影误差的信息矩阵，hp,hl分别是点特征、线特征的huber鲁棒核函数，ρl,ιl分别表示点特征集、线特征集。

由于最小化的是误差项的二范数平方和，其增长速度是误差的平方，若出现误匹配会导致系统向错误的值进行优化，因此引入huber核函数可以降低误匹配的影响。

huber核函数的具体形式如下：

其中，error是点特征、线特征的重投影误差的总和，当error的值大于阈值δ时，代价函数的增长速度由二次函数转为一次函数，从而降低了误匹配的影响。

最小化代价函数f的过程就是在求解待估计状态量的过程。为了利用非线性优化方法优化代价函数，需要先计算出误差函数关于状态变量的雅克比矩阵。

当相机发生微小位姿变化ξ∈se(3)时(ξ是变换矩阵tiw所对应的李代数),记gp是pw,j在相机坐标系下对应的坐标，gs,ge分别是sw,k,ew,k在相机坐标系下对应的坐标：

gp＝error^ξpw,j＝riwpw,j+tiw(15)

gs＝error^ξsw,k＝riwsw,k+tiw(16)

ge＝error^ξew,k＝riwew,k+tiw(17)

点特征重投影误差关于相机微小位姿变化ξ∈se(3)的雅克比矩阵，如下所示：

其中，

线特征重投影误差关于相机微小位姿变化ξ∈se(3)的雅克比矩阵，如下所示：

其中，

在上述过程中,[]∧表示进行反对称矩阵的转换，具体转换如下式

计算出雅克比矩阵后，采用gauss-newton的非线性优化方法来求解优化问题。

步骤7：判断当前帧是否为关键帧，如果是，则执行步骤8；如果不是，则设定上一个关键帧为当前帧的参考关键帧，然后，跳转至步骤12；

关键帧选择是指剔除冗余的图像帧，保留具有代表性的图像帧。当选取的关键帧数量过多时，关键帧之间的信息是冗余的，会带来很大的计算量，难以满足对于实时性的要求；当选取的关键帧数量过少时，关键帧之间的信息关联性较差，难以正确地估计相机位姿，导致定位失败。本发明所选取的关键帧同时满足了以下条件：

⑴与上一个关键帧之间至少含有20个图像帧；

⑵至少成功追踪到了50个点特征和15条线特征；

⑶与上一个关键帧之间的共视点、线特征信息少于75％。

根据上述关键帧的判断条件，如果当前帧不是关键帧，那么将设定上一个关键帧作为当前帧的参考关键帧，并计算出两帧之间的相对位姿量relative，将其记录下来。

当前帧currentimage位姿为tc∈se(3),参考关键帧referimage位姿为tr∈se(3)，则相对位姿量relative为：trel＝tc(tr)^-1。

步骤8：提取当前帧的点特征，包括如下步骤：

步骤8.1：采用orb算法对当前帧进行点特征的提取和描述；

对当前帧图像建立金字塔，分别在金字塔图像上进行影像分块，获取一定大小的影像区域，利用orb特征点提取算法在每个分块里进行点特征的提取与描述。

步骤8.2：当前帧的左、右目图像之间的点特征通过描述子匹配的方式，进行稀疏立体匹配。

左、右目图像之间，根据描述子之间的距离大小进行点特征的匹配，根据双目匹配出的特征点对，求出对应的空间点的3d坐标。

步骤9：局部关键帧之间进行点、线特征的数据关联；

此处的局部关键帧指的是当前帧与之前的10个关键帧所组成的11个关键帧。局部关键帧之间的点特征是通过描述子匹配的方式进行数据关联的，需要特别指出的是在步骤2中前后帧之间的特征点是通过klt光流法跟踪点特征进行数据关联的。

步骤10：利用局部关键帧之间的点特征和线特征的重投影误差所构建的代价函数，对局部关键帧的位姿进行再次的优化调整；

其中，分别是点特征、线特征重投影误差的信息矩阵，hp,hl分别是点特征、线特征的huber鲁棒核函数，ρl,ιl,κl分别表示点特征集、线特征集以及局部关键帧。

在步骤6中，是根据前后帧之间的点、线特征的数据关联关系，构建出了代价函数，从而优化求解出当前帧的位姿；在步骤10中，是根据局部关键帧之间的点、线特征的数据关联关系，再次构建了新的代价函数，采用gauss-newton的非线性优化方法对局部关键帧的位姿进行了优化调整。

步骤11：对非关键帧的位姿进行调整；

对于每一个非关键帧而言，都为它们设定了相对应的参考关键帧，并且求出了两者之间的相对位姿量。在步骤10中对局部关键帧进行了位姿的调整，因而，当参考关键帧referimage的位姿tr被优化调整为tr′时，也会根据相对位姿量relative对那些非关键帧的位姿也进行调整，即：tc被调整为tc′，其中，tc′＝treltr′。通过设定参考关键帧的方式，相当于对非关键帧的位姿也进行了再次优化调整。

步骤12：输出图像帧的位姿。

循环步骤1至步骤12对mh_04_difficult图像序列进行处理，计算出每一图像帧所对应的位姿，输出位姿结果，以txt文档的形式记录下来。

接下来，根据记录在txt文档中的图像帧的位姿结果，对本算法的性能进行评价分析。

图5表示本算法在mh_04_difficult图像序列上的实验结果与真实轨迹的对比效果图，其中，实验结果表明：本算法能够完成在mh_04_difficult图像序列上的定位任务。

在定位精度分析的过程中，采用了绝对位姿误差(absoluteposeerror，ape)作为评价指标来评估本算法的定位精度，ape是通过计算估计位姿与真实位姿的距离得到的，i时刻，真实位姿为ti∈se(3),估计位姿为ti′∈se(3)，则ape为：apei＝ti(t′i)^-1。

对比了本算法与不含回环检测环节的orb-slam2算法在eurocmh_04_diffucult图像序列上的定位精度。

图6-1是本算法的ape与不含回环检测环节的orb-slam2算法的ape对比图，为了更好的对比定位精度，将图6-2中的定位精度信息，汇总如下表：

由上表可知，在测试mh_04_difficult图像序列时，本算法的最大定位误差为0.268466m,比orb-slam2算法减小了0.046196m,本算法的最小定位误差为0.011937，比orb-slam2算法减小了0.000562m,本算法的均方根误差为0.096488m,相较于orb-slam2算法而言，提高了28.8％的定位精度。

本算法对每一图像帧的平均处理时间为42.837ms，约为23.3帧/秒，相较于相机20hz的数据采集频率而言，本算法能够满足实时性要求。