CN110108258B - 一种单目视觉里程计定位方法 - Google Patents

Abstract

本发明公开了一种单目视觉里程计定位方法，所述方法包括如下步骤：该方法包括：读取当前帧输入图像；采用基于视差的方法对视觉里程计完成初始化，成功后则开始后续的运动估计步骤；对当前帧上，基于灰度不变性，构建以位姿为优化变量的非线性优化问题，得到初始帧间位姿；在初始位姿的基础上，构建局部地图，并采用基于密度的跟踪策略完成对局部地图投影点的特征匹配及亚像素位置优化；通过局部地图跟踪得到更准确的约束关系，通过该关系进一步优化位姿及地图点，并输出最终定位结果。应用本发明所述的单目视觉里程计定位方法，能够达到较高的定位精度，且具有无人机导航应用所需要的鲁棒性与实时性。

Description

一种单目视觉里程计定位方法

技术领域

本发明涉及计算机视觉技术领域，具体涉及一种单目视觉里程计定位方法。

背景技术

对于已知环境的自主导航研究已经较为成熟，但是对于未知环境的导航研究还未形成统一和完善的体系。自主导航任务可划分为定位、建图、路径规划三步骤，同步定位与建图技术涉及前两个要素，随着计算机视觉技术的迅速发展，视觉传感器的小型化、低成本化，视觉SLAM逐渐成为机器人自主导航领域研究的热点问题。

视觉里程计(Visual Odometry,VO)作为视觉SLAM前端重要的组成部分，其作用是实时响应输入图像帧，估计相机位姿，从实现方式上可划分为特征点法和直接法，特征点特征提取、匹配会花费大量时间，且仅能重建稀疏点云地图；直接法对光照、快速运动等情况鲁棒性不足。

Forster提出的半直接法单目视觉里程计SVO(Semi-direct VO)成功的应用于携带俯视视角相机的无人机平台，半直接法是指对图像中稀疏的角点特征使用直接法进行运动估计，因此SVO具有极快的运行速度。但SVO也具备直接法所存在的问题，以及不合理的定位策略，导致其对光照、快速运动、以及在特征分布集中的场景中鲁棒性不足。

发明内容

本发明的目的在于提供一种单目视觉里程计定位方法，以解决现有技术中导致的上述多项缺陷或缺陷之一。

为达到上述目的，本发明是采用下述技术方案实现的：

一种单目视觉里程计定位方法，所述方法包括如下步骤：读取当前帧图像，判断所述当前帧图像的状态；若所述当前帧图像处于初始状态，对所述当前帧图像进行初始化，得到所述当前帧图像中初始的三维地图点信息，完成初始化操作后进入稀疏图像对齐步骤；若所述当前帧图像处于正常状态，直接进入稀疏图像对齐步骤。

稀疏图像对齐，在所述当前帧图像上采用半直接法对前一帧图像建立的特征点进行图像对齐，得到前后两帧图像间的初始位姿；

局部地图跟踪，在所述初始位姿上构建由关键帧图像组成的局部地图，将局部地图点投影至所述当前帧图像，并筛选若干投影点进行像素位置优化，若所述投影点满足优化条件则认为对所述局部地图点成功跟踪，并为所述局部地图点在所述当前帧图像建立特征；

位姿及结构优化，在所述当前帧图像建立更准确的三维到二维的约束关系，并通过所述约束关系构建最小二乘优化，优化所述当前帧图像的位姿变量以使重投影误差平方和最小，得到所述当前帧图像最终的位姿信息。

本发明的优点在于：采用了基于视差的模型选择方法，使视觉里程计的初始化成功率更高；融入了ORB特征匹配，使视觉里程计具有更高的定位精度；采用了基于密度的跟踪策略，使视觉里程计具有更高的鲁棒性，尤其是在特征分布集中的场景。

附图说明

图1为本发明具体实施方式的视觉里程计定位方法流程图；

图2为本发明具体实施方式的视觉里程计初始化模块流程图；

图3为本发明具体实施方式运行效果图；

图4为本发明具体实施方式定位轨迹图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

以无人机公开数据集EUROC为例，它的采集图像分辨率为752*480，图像序列存在光照变换，以及由于无人机的快速运动导致的运动模糊等，因此具有挑战性。

本发明提供一种融合特征匹配与半直接法的单目视觉里程计定位方法，如图1所示，包括如下具体流程：

步骤1)、读取当前帧输入图像。

步骤2)、读取当前帧图像的当前状态，若是初始状态，则进入初始化模块。若当前帧图像处于正常状态，直接进入稀疏图像对齐步骤。如图2所示，初始化流程具体为：

步骤2-1)、将第一帧图像设为世界坐标系，对第一帧图像提取ORB点特征，判断第一帧图像特征数量是否大于100，如果满足条件则进入步骤2-2)。

步骤2-2)、获取当前帧图像作为第二帧，采用金字塔Lucas-Kanade光流法对前一帧特征进行跟踪，判断匹配点数量是否大于40，否则重启初始化流程。判断匹配点间的平均视差是否大于40像素，否则重新进入步骤2-2)。平均视差的表达式为：

其中p_li，p_ci分别代表匹配点对在第一帧、第二帧中的像素坐标，n表示总的匹配点数量，i表示匹配点序号。

步骤2-3)、步骤2-2)根据金字塔光流法建立的二维到二维的匹配关系，通过该匹配关系恢复第一二帧的运动。该运动的求解对应于两种模型：本质矩阵模型、单应矩阵模型，其表达式分别为：

其中E为本质矩阵，可通过八点法求得；H为单应矩阵，可通过四点法求得；R、t分别表示旋转矩阵、平移向量；t^表示t的反对称矩阵，P_c与P_l分别为匹配点在归一化图像平面的坐标。根据视差的方差选择不同的模型恢复运动：

其中，d_i表示第i对匹配点的视差，σ²表示方差。当σ大于15像素时，采用本质矩阵模型，反之采用单应矩阵模型。

步骤2-4)、采用随机抽样一致性算法来避免误匹配点，以及不符合该模型的匹配点参与模型求解，通过重投影误差分析恢复出的位姿的精度。对本质矩阵或单应矩阵分解可得到某一尺度下的旋转矩阵、平移向量。

步骤2-5)、由于二维到二维的匹配关系恢复出的运动并不是真实尺度下的运动，因此会设定一个尺度。尺度设定方法为：对地图点的平均深度进行归一化。最后得到该尺度下的运动、及地图点，初始化完成并将第一二帧保存为关键帧，后续的运动估计则在该尺度下进行。

步骤3)、稀疏图像对齐：在当前帧图像上，基于灰度不变性，构建以帧间位姿T_k，k-1为优化变量的最小二乘问题，并通过高斯牛顿法、LM算法等非线性优化法迭代求解，该问题表示为：

其中，N表示前一帧的特征点数量，p_i表示前一帧上第i个特征点，δI(T_k，k-1，p_i)为灰度残差函数，表示为：

δI(T_k，k-1，p_i)＝I_k(patch_4×4(p′_i))-I_k-1(patch_4×4(p_i))

其中p′_i表示地图点P_i在当前帧k上的投影点，patch(·)是指通过双线性插值法以像素点为中心构建图像块，I(patch_4×4(p′_i))表示该图像块像素灰度值之和。p_i与p′_i的关系由帧间位姿T_k，k-1得到：

p′_i＝π(T_k，k-1(π^-1(p_i，z)))

上式的具体含义为：首先将前一帧图像特征点p_i通过对应地图点P的深度z恢复到前一帧的相机坐标系下，然后将该点在位姿T_k，k-1下变换到当前帧相机坐标系下，最后投影至当前帧图像。

步骤4)、局部地图跟踪：具体为：

步骤4-1)、构建局部地图，具体方法为：遍历过去保存的所有关键帧图像，找到与当前帧图像共视的关键帧图像，并按与当前帧图像的距离由近及远取前10帧组成局部关键帧地图。其中关键帧图像是指通过关键帧策略筛选的图像序列中具有代表性的图像帧，基于关键帧的建图保存了地图信息及位姿。

步骤4-2)、将当前帧图像按行列划分为25*25像素尺寸的图像格，局部地图点投影至当前帧，分别落在不同的图像网格中。其中地图点的生成方法为：对新的关键帧图像提取新的ORB点特征，然后通过三角测量或深度滤波器等方法估计特征深度而建立。

步骤4-3)、给每一个网格分配需要成功优化的投影点数量，设定当前帧图像总的需要成功优化的数量为maxFts＝180，统计存在投影点的网格数量ncells，若满足条件ncells＞1.5×maxFts，则每个网格需优化1个投影点。反之则按照密度分配数量，表达式为：

其中n_i代表第i格的投影点数量。

步骤4-4)、按照由内而外的随机顺序遍历网格，对网格里的投影点根据Shi-Tomasi算子计算响应分数，按由高到低的顺序对投影点进行优化。

步骤4-5)、优化的具体方法：首先，在投影点处选取30像素的矩形区域，在该图像区域中提取ORB特征点，并与该地图点在参考关键帧图像中对应的特征进行匹配，其中参考关键帧图像是指地图点被首次观测到的关键帧图像，特征匹配的条件为：汉明距离需小于30；最近邻距离与次近邻距离比值应小于0.55。然后，在特征匹配优化的像素位置上进行亚像素精度的位置优化，基于灰度不变性，构建以投影点位置为优化变量的最小二乘问题，表示为：

δI(p_i，p′_i)＝I_k(patch_8×8(p′_i))-I_r(A_kr*patch_8×8(p_i))

其中函数I(patch_8×8(p′_i))表示该图像块像素灰度值之和，函数patch_8×8(·)表示以该投影点为中心，通过双线性插值法构建8×8的图像块。p′_i表示当前帧图像上的投影点，p_i表示参考关键帧图像上与p′_i对应的特征点，A_kr表示图像块之间的仿射矩阵。

对于上式，通过非线性优化算法迭代求解，在设定的15次迭代次数内，若第n+1次迭代值p′_i ⁽ⁿ⁺¹⁾相比第n次迭代值p′_i ⁽ⁿ⁾，有

则认为投影点位置优化成功，并在当前帧图像上建立特征点。

步骤5)、位姿及结构优化：重投影误差表示为：

e(T，P)＝p-π(T(P))

其中T表示位姿，P表示地图点，函数T(·)表示位姿变换，函数π(·)表示相机的成像变换。

步骤4)对当前帧图像建立了更准确的三维到二维的约束关系，通过该约束关系构建最小二乘优化问题，优化当前帧图像的位姿变量以使重投影误差平方和最小，得到当前帧图像最终的位姿信息，表示为：

其中，其中T_k，w表示位姿，P_i表示当前帧根据局部地图跟踪得到的第i个地图点，函数e(·)表示地图点通过位姿变换后的重投影误差。

对于一个地图点，其会被若干关键帧图像所观测到，则同样具有若干的三维到二维的约束关系，通过该约束关系构建最小二乘优化问题，优化地图点的三维参数以使重投影误差平方和最小，表示为：

其中，P_w表示地图点，T_i，w表示跟踪到P_w的第i帧关键帧的位姿，函数e(·)表示地图点通过位姿变换后的重头误差。

如图3所示为本发明实施例运行效果图，图中小方框所示为当前帧跟踪的特征点。如图4所示为本发明实施例定位轨迹图，其中估计轨迹为虚线标识，真实轨迹为实线标识。本发明实施例该图像序列中运行的定位精度以绝对轨迹误差作评价指标，其均方根误差为0.07米。

由技术常识可知，本发明可以通过其它的不脱离其精神实质或必要特征的实施方案来实现。因此，上述公开的实施方案，就各方面而言，都只是举例说明，并不是仅有的。所有在本发明范围内或在等同于本发明的范围内的改变均被本发明包含。

Claims (6)

1.一种单目视觉里程计定位方法，其特征在于，所述方法包括如下步骤：

读取当前帧图像，判断所述当前帧图像的状态；

若所述当前帧图像处于初始状态，对所述当前帧图像进行初始化，得到所述当前帧图像中初始的三维地图点信息，完成初始化操作后进入稀疏图像对齐步骤；若所述当前帧图像处于正常状态，直接进入稀疏图像对齐步骤；

稀疏图像对齐，在所述当前帧图像上采用半直接法对前一帧图像建立的特征点进行图像对齐，得到前后两帧图像间的初始位姿；

局部地图跟踪包括如下步骤：

步骤6-1)、构建局部地图，遍历保存的所有关键帧图像，找到与所述当前帧图像共视的关键帧图像，并按与所述当前帧图像的距离由近及远取前5至15帧组成局部关键帧图像地图；

步骤6-2)、将所述当前帧图像按行列划分为尺寸为20至60像素的方形图像网格，局部地图点投影至当前帧图像，分别落在不同的图像网格中；

步骤6-3)、给每一个网格分配需要成功优化的投影点数量，设当前帧总的需要成功优化的数量为maxFts≥120，统计存在投影点的网格数量ncells，若满足条件ncells＞1.5×maxFts，则每个网格需优化1个投影点；反之则按照密度分配数量，表达式为：

其中n_i代表第i格的投影点数量；

步骤6-4)、按照由内而外的随机顺序遍历所述网格，对所述网格里的投影点根据Shi-Tomasi算子计算响应分数，按由高到低的顺序对投影点进行优化；

位姿及结构优化，在所述当前帧图像建立更准确的三维到二维的约束关系，并通过所述约束关系构建最小二乘优化，优化所述当前帧图像的位姿变量以使重投影误差平方和最小，得到所述当前帧图像最终的位姿信息。

2.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述对所述当前帧图像进行初始化包括如下步骤：

步骤2-1)、将获取的第一帧图像设为世界坐标系，对所述第一帧图像提取ORB点特征，判断所述ORB点特征数量是否大于最低阈值100，如果满足条件则进入步骤2-2)；

步骤2-2)、获取当前帧图像作为第二帧，采用金字塔Lucas-Kanade光流法对所述第一帧图像特征进行跟踪，判断匹配点数量是否大于最低阈值40，否则重启初始化流程；判断匹配点间的平均视差是否大于最低阈值40，否则重新进入步骤2-2)；

步骤2-3)、根据步骤2-2)中建立的二维到二维的匹配关系，恢复所述第一帧图像和所述第二帧图像的运动；其中所述运动包括两种运动模型，根据所述平均视差方差选择相应的模型恢复所述运动；

步骤2-4)、采用随机抽样一致性算法来避免误匹配点以及不符合所述运动模型的匹配点参与所述运动模型求解，通过重投影误差分析恢复出的位姿的精度；

步骤2-5)、设定尺度，对地图点的平均深度进行归一化；得到所述尺度下的运动模型及地图点，初始化完成并将所述第一帧图像和所述第二帧图像保存为关键帧。

3.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述稀疏图像对齐是基于灰度不变性构建帧间位姿T_k，k-1为优化变量的最小二乘优化，并通过高斯牛顿法算法或LM算法迭代求解。

4.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，在所述初始位姿上构建由关键帧图像组成的局部地图中的关键帧图像指通过关键帧策略筛选的图像序列中具有代表性的图像帧，基于关键帧的建图保存了地图信息及位姿。

5.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述局部地图跟踪中所述地图点的生成方法为：对新的关键帧图像提取新的ORB点特征，通过三角测量方法或深度滤波器方法估计特征深度建立所述地图点。

6.根据权利要求1所述的单目视觉里程计定位方法，其特征在于，所述位姿及结构优化中，重投影误差表示为：

e(T，P)＝p-π(T(P))

其中T表示位姿，P表示地图点，函数T(·)表示位姿变换，函数π(·)表示相机的成像变换；

在所述位姿优化中，所述最小二乘优化表示为：

其中，T_k，w表示位姿，N为当前帧根据局部地图跟踪得到的地图点数量，P_i表示第i个地图点，函数e(·)表示地图点通过位姿变换后的重投影误差；

在所述结构优化，所述最小二乘优化表示为：

其中，P_w表示地图点，T_i，w表示跟踪到P_w的第i帧关键帧的位姿，函数e(·)表示地图点通过位姿变换后的重头误差。

Images