CN107248171B - 一种基于三角剖分的单目视觉里程计尺度恢复方法 - Google Patents

Abstract

本发明涉及一种基于三角剖分的单目视觉里程计尺度恢复方法，包括以下步骤：1)获取前后两帧已经匹配上的第i个特征点

和

以及对应的3D空间坐标；2)计算出路面倾角的估计值θ^*；3)将已经匹配上的特征点在后一帧图像中的位置(u₁,v₁)进行三角剖分；4)根据每个特征点的3D空间坐标，计算每个三角形所在的空间平面的法向向量和高度；5)计算三角形所在的空间平面的俯仰角θ_j，筛选三角形；6)路面的高度h^*；7)将相机真实高度h与路面的高度h^*作商得到单目视觉里程计的尺度参数s，恢复视觉里程计绝对尺度。与现有技术相比，本发明具有速度快、准确率高、无需做图像分割和路面识别、鲁棒性强等优点。

Description

一种基于三角剖分的单目视觉里程计尺度恢复方法

技术领域

本发明涉及一种尺度恢复方法，尤其是涉及一种基于三角剖分的单目视觉里程计尺度恢复方法。

背景技术

目前常用计算单目视觉里程计尺度的方法主要分为两种，一种需要融合其他传感器如激光雷达或者惯性传感器，另一种是考虑环境中的已知尺度。前一种方法可以更为精准的回复单目尺度，但增加了使用成本，同时使用前需要精准的标定；后一种方法常用的环境中已知尺度一般为相机到路面的高度，然后通过计算路面的几何模型来恢复。常用的计算路面几何模型的方法主要分为两种，一种是通过特征点的3D位置，用RANSAC的方法提出噪声或依赖于路面识别提出噪声，另外一种是通过在已知路面区域的前提下通过最小化单应性误差来计算路面几何模型；前一种方法往往由于路面上特征点较少无法准确的计算，后一种方法由于算法计算复杂度较高，需要大量的计算时间或计算资源。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种速度快、准确率高、无需做图像分割和路面识别、鲁棒性强的基于三角剖分的单目视觉里程计尺度恢复方法。

本发明的目的可以通过以下技术方案来实现：

一种基于三角剖分的单目视觉里程计尺度恢复方法，包括以下步骤：

1)根据单目视觉里程计获取前后两帧已经匹配上的第i个特征点

和

以及对应的3D空间坐标；

2)根据单目视觉里程计得到的旋转矩阵R和不具备真实尺度的位移向量t^*计算出路面倾角的估计值θ^*；

3)将已经匹配上的特征点在后一帧图像中的位置(u₁,v₁)进行三角剖分，得到符合条件的三角形组Tri；

4)根据每个特征点的3D空间坐标，计算每个三角形所在的空间平面的法向向量和高度；

5)根据每个三角形所在的空间平面的法向向量计算三角形所在的空间平面的俯仰角θ_j，筛选三角形；

6)根据筛选出的三角形的顶点采用RANSAC方法得到路面的高度h^*；

7)将相机真实高度h与路面的高度h^*作商得到单目视觉里程计的尺度参数s，将不具备真实尺度的位移向量t^*与尺度参数s相乘，即可恢复视觉里程计绝对尺度。

所述的步骤4)中，第j个三角形的法向向量n_j和高度h_j的表达式为：

n_j ^T*X_jk+h_j＝0j＝1,2,...,m k＝1,2,3

其中，X_jk为第j个三角形的第k个顶点，n_j为第j个三角形的法向向量，h_j为第j个三角形的高度。

所述的路面倾角的估计值θ^*的计算式为：

其中，t^*为不具备真实尺度的位移向量，t^*[2]为不具备真实尺度的位移向量t^*的第二个值，

为t^*的模长

所述的三角形所在的空间平面的俯仰角θ_j的计算式为：

其中，n_j[2]为三角形法向向量n_j的第二个值，

为n_j的模长。

所述的步骤5)中，筛选出三角形所在的空间平面的俯仰角θ_j与路面倾角的估计值θ^*的差值小于5度时对应的三角形。

与现有技术相比，本发明具有以下优点：

一、无需做图像分割和路面识别，仅根据视觉里程计计算出特征点3D坐标计算路面几何模型，鲁棒性强；

二、使用三角剖分方法判断特征点是否属于路面，速度快而且准确率高。

附图说明

图1为本发明的方法原理图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，本发明的计算步骤如下：

1)根据单目视觉里程计获取前后两帧已经匹配上的第i个特征点

和

以及对应的3D空间坐标；

2)根据单目视觉里程计得到的旋转矩阵R和位移向量t^*计算出路面倾角的估计值θ^*(计算公式见下文)；

3)将已经匹配上的特征点在后一帧图像中的位置(u₁,v₁)进行三角剖分，得到符合条件的三角形组Tri；

4)根据每个特征点的3D空间坐标，计算每个三角形所在的空间平面的法向向量和高度；

5)根据路面倾角的估计值θ^*以及由每个三角形由法向向量计算三角形属于路面的倾角筛选三角形，保留误差在5度以内的三角形；

6)使用步骤5)中符合条件的三角形的顶点使用RANSAC方法得到路面估计模型和符合路面模型的特征点

7)根据选出三角形对应的特征点获取路面的几何模型，并将相机真实高度与计算出来的高度相除得到单目视觉里程计的尺度参数，将此位移向量t^*乘此此参数，恢复即可恢复视觉里程计绝对尺度。

本方法详细说明如下：

首先利用现有技术(ORBSLAM，LIBVISO等)提取并匹配相机两帧图像中特征点

和

其中i＝1,2,...,n,为匹配上的特征点的个数，然后计算出相机在前后两帧之间的位姿变化R,t^*，其中R为旋转矩阵，t^*为位移向量，由于单目视觉损失了深度信息，此处计算出来的位移不具备真实的尺度，文中所有相对尺度的变量均有上角标*。继而通过三角测量得到所有成功匹配的特征点的3D坐标X_i ^*。

利用三角剖分技术将后一帧图像中的特征点

i＝1,2,...,n分成符合规则的三角形组。

对于每一个三角形，根据之前求出的每个点的3D坐标，根据如下公式求出每个三角形所在平面的法向向量

n_j ^T*X_jk+h_j＝0 j＝1,2,...,m k＝1,2,3

其中m为三角形组中三角形的个数，j为三角形序号，k为三角形内三个顶点的序号n_j为第j个三角形的法向，h_j为第j个三角形的高度，对于每一个三角形,

忽略法向的模长，

求得n_j之后，便可求出这个平面在相机坐标系中的俯仰角

可以根据单目VO计算得到的相机运动的旋转矩阵R和位移向量t估计出地面相对于相机的俯仰角。当汽车行驶在平路上时，VO所计算出来的位移的俯仰角本质上就是路的俯仰角，如下图所示。此处的平路指汽车不发生俯仰方向旋转的路，包括符合这条件的坡路。计算方法如下：

首先根据旋转矩阵R，计算汽车是否有俯仰

θ_R＝arctan(-R₃₂/R₃₃)

其中R_ijR中底i行第j列的元素，若θ_R小于5度，则认为汽车没有俯仰运动，根据下面公式计算路面俯仰角，若θ_R小于5度则路面俯仰角置为上一时刻计算的俯仰角

比较平面俯仰角和道路俯仰角，相差越小，则说明这一平面在道路上的概率越大。然后考虑各个三角形的中心高度，首先剔除所有在相机上方的点，在剩下的点中，选择道路平面俯仰角和道路俯仰角相差5度以内的三角形。结果大概如下图所示，绿色区域为估计路面区域，然后根据路属于三角形的顶点使用RANSAC方法得到路面法向和高度，RANSAC方法具体步骤为：

在筛选过后的三角形顶点中随机抽取三个顶点，并使用这三个顶点计算其所在平面的几何模型：

nX+h＝0

(该计算方法与前文计算三角形几何模型方法一致)，然后计算除此三点外所有的三角形顶点到这个平面的距离d＝|nX_i+h|，距离小于高度0.1h的点认为是内点，并统计内点数量；然后重复此步骤，随机选取多次，取内点最多的路面几何模型为路面最终几何模型。

最后将真实高度和这一几何模型中的高度相比得到真实尺度，即：

t＝st^*

其中h为相机真实高度，h^*为算法估计的路面高度(即相机高度)，t^*为不带尺度的位移向量，t^*为恢复尺度之后的位移向量。

Claims (2)

1.一种基于三角剖分的单目视觉里程计尺度恢复方法，其特征在于，包括以下步骤：

1)根据单目视觉里程计获取前后两帧已经匹配上的第i个特征点

和

以及对应的3D空间坐标；

2)根据单目视觉里程计得到的旋转矩阵R和不具备真实尺度的位移向量t^*计算出路面倾角的估计值θ^*，所述的路面倾角的估计值θ^*的计算式为：

其中，t^*为不具备真实尺度的位移向量，t^*[2]为不具备真实尺度的位移向量t^*的第二个值，

为t^*的模长；

3)将已经匹配上的特征点在后一帧图像中的位置(u₁,v₁)进行三角剖分，得到符合条件的三角形组Tri；

4)根据每个特征点的3D空间坐标，计算每个三角形所在的空间平面的法向向量和高度；

5)根据每个三角形所在的空间平面的法向向量计算三角形所在的空间平面的俯仰角θ_j，筛选三角形，筛选出三角形所在的空间平面的俯仰角θ_j与路面倾角的估计值θ^*的差值小于5度时对应的三角形，所述的三角形所在的空间平面的俯仰角θ_j的计算式为：

其中，n_j[2]为三角形法向向量n_j的第二个值，

为n_j的模长；

6)根据筛选出的三角形的顶点采用RANSAC方法得到路面的高度h^*；

7)将相机真实高度h与路面的高度h^*作商得到单目视觉里程计的尺度参数s，将不具备真实尺度的位移向量t^*与尺度参数s相乘，即可恢复视觉里程计绝对尺度。

2.根据权利要求1所述的一种基于三角剖分的单目视觉里程计尺度恢复方法，其特征在于，所述的步骤4)中，第j个三角形的法向向量n_j和高度h_j的表达式为：

n_j ^T*X_jk+h_j＝0 j＝1,2,...,m k＝1,2,3

其中，X_jk为第j个三角形的第k个顶点，n_j为第j个三角形的法向向量，h_j为第j个三角形的高度，m为三角形组中三角形的个数。

Images