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

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

A scale recovery method for monocular vision odometry based on triangulation

The invention relates to a monocular vision odometer scale recovery method based on triangulation, which includes the following steps: 1) obtaining two I feature points which have been matched before and after the two frames

【技术实现步骤摘要】
一种基于三角剖分的单目视觉里程计尺度恢复方法
本专利技术涉及一种尺度恢复方法，尤其是涉及一种基于三角剖分的单目视觉里程计尺度恢复方法。
技术介绍
目前常用计算单目视觉里程计尺度的方法主要分为两种，一种需要融合其他传感器如激光雷达或者惯性传感器，另一种是考虑环境中的已知尺度。前一种方法可以更为精准的回复单目尺度，但增加了使用成本，同时使用前需要精准的标定；后一种方法常用的环境中已知尺度一般为相机到路面的高度，然后通过计算路面的几何模型来恢复。常用的计算路面几何模型的方法主要分为两种，一种是通过特征点的3D位置，用RANSAC的方法提出噪声或依赖于路面识别提出噪声，另外一种是通过在已知路面区域的前提下通过最小化单应性误差来计算路面几何模型；前一种方法往往由于路面上特征点较少无法准确的计算，后一种方法由于算法计算复杂度较高，需要大量的计算时间或计算资源。
技术实现思路
本专利技术的目的就是为了克服上述现有技术存在的缺陷而提供一种速度快、准确率高、无需做图像分割和路面识别、鲁棒性强的基于三角剖分的单目视觉里程计尺度恢复方法。本专利技术的目的可以通过以下技术方案来实现：一种基于三角剖分的单目视觉里程计尺度恢复方法，包括以下步骤：1)根据单目视觉里程计获取前后两帧已经匹配上的第i个特征点和以及对应的3D空间坐标；2)根据单目视觉里程计得到的旋转矩阵R和不具备真实尺度的位移向量t*计算出路面倾角的估计值θ*；3)将已经匹配上的特征点在后一帧图像中的位置(u1,v1)进行三角剖分，得到符合条件的三角形组Tri；4)根据每个特征点的3D空间坐标，计算每个三角形所在的空间平面的法向向量和高度；5)根据每个三角形所在的空间平面的法向向量计算三角形所在的空间平面的俯仰角θj，筛选三角形；6)根据筛选出的三角形的顶点采用RANSAC方法得到路面的高度h*；7)将相机真实高度h与路面的高度h*作商得到单目视觉里程计的尺度参数s，将不具备真实尺度的位移向量t*与尺度参数s相乘，即可恢复视觉里程计绝对尺度。所述的步骤4)中，第j个三角形的法向向量nj和高度hj的表达式为：njT*Xji+hj＝0j＝1,2,...,mi＝1,2,3其中，Xji为第j个三角形的第i个顶点，nj为第j个三角形的法向向量，hj为第j个三角形的高度。所述的路面倾角的估计值θ*的计算式为：其中，t*为不具备真实尺度的位移向量，t*[2]为不具备真实尺度的位移向量t*的第二个值，为t*的模长。所述的三角形所在的空间平面的俯仰角θj的计算式为：其中，nj[2]为三角形法向向量nj的第二个值，为nj的模长。所述的步骤5)中，筛选出三角形所在的空间平面的俯仰角θj与路面倾角的估计值θ*的差值小于5度时对应的三角形。与现有技术相比，本专利技术具有以下优点：一、无需做图像分割和路面识别，仅根据视觉里程计计算出特征点3D坐标计算路面几何模型，鲁棒性强；二、使用三角剖分方法判断特征点是否属于路面，速度快而且准确率高。附图说明图1为本专利技术的方法原理图。具体实施方式下面结合附图和具体实施例对本专利技术进行详细说明。实施例如图1所示，本专利技术的计算步骤如下：1)根据单目视觉里程计获取前后两帧已经匹配上的第i个特征点和以及对应的3D空间坐标；2)根据单目视觉里程计得到的旋转矩阵R和位移向量t*计算出路面倾角的估计值θ*(计算公式见下文)；3)将已经匹配上的特征点在后一帧图像中的位置(u1,v1)进行三角剖分，得到符合条件的三角形组Tri；4)根据每个特征点的3D空间坐标，计算每个三角形所在的空间平面的法向向量和高度；5)根据路面倾角的估计值θ*以及由每个三角形由法向向量计算三角形属于路面的倾角筛选三角形，保留误差在5度以内的三角形；6)使用步骤5)中符合条件的三角形的顶点使用RANSAC方法得到路面估计模型和符合路面模型的特征点7)根据选出三角形对应的特征点获取路面的几何模型，并将相机真实高度与计算出来的高度相除得到单目视觉里程计的尺度参数，将此位移向量t*乘此此参数，恢复即可恢复视觉里程计绝对尺度。本方法详细说明如下：首先利用现有技术(ORBSLAM，LIBVISO等)提取并匹配相机两帧图像中特征点和其中i＝1,2,...,n,为匹配上的特征点的个数，然后计算出相机在前后两帧之间的位姿变化R,t*，其中R为旋转矩阵，t*为位移向量，由于单目视觉损失了深度信息，此处计算出来的位移不具备真实的尺度，文中所有相对尺度的变量均有上角标*。继而通过三角测量得到所有成功匹配的特征点的3D坐标利用三角剖分技术将后一帧图像中的特征点分成符合规则的三角形组。对于每一个三角形，根据之前求出的每个点的3D坐标，根据如下公式求出每个三角形所在平面的法向向量njT*Xji+hj＝0j＝1,2,...,mi＝1,2,3其中m为三角形组中三角形的个数，j为三角形序号，i为三角形内三个顶点的序号nj为第j个三角形的法向，hj为第j个三角形的高度，对于每一个三角形,忽略法向的模长，求得nj之后，便可求出这个平面在相机坐标系中的俯仰角可以根据单目VO计算得到的相机运动的旋转矩阵R和位移向量t估计出地面相对于相机的俯仰角。当汽车行驶在平路上时，VO所计算出来的位移的俯仰角本质上就是路的俯仰角，如下图所示。此处的平路指汽车不发生俯仰方向旋转的路，包括符合这条件的坡路。计算方法如下：首先根据旋转矩阵R，计算汽车是否有俯仰θR＝arctan(-R32/R33)其中RijR中底i行第j列的元素，若θR小于5度，则认为汽车没有俯仰运动，根据下面公式计算路面俯仰角，若θR小于5度则路面俯仰角置为上一时刻计算的俯仰角比较平面俯仰角和道路俯仰角，相差越小，则说明这一平面在道路上的概率越大。然后考虑各个三角形的中心高度，首先剔除所有在相机上方的点，在剩下的点中，选择道路平面俯仰角和道路俯仰角相差5度以内的三角形。结果大概如下图所示，绿色区域为估计路面区域，然后根据路属于三角形的顶点使用RANSAC方法得到路面法向和高度，RANSAC方法具体步骤为：在筛选过后的三角形顶点中随机抽取三个顶点，并使用这三个顶点计算其所在平面的几何模型：nX+h＝0(该计算方法与前文计算三角形几何模型方法一致)，然后计算除此三点外所有的三角形顶点到这个平面的距离d＝|nXi+h|，距离小于高度0.1h的点认为是内点，并统计内点数量；然后重复此步骤，随机选取多次，取内点最多的路面几何模型为路面最终几何模型。最后将真实高度和这一几何模型中的高度相比得到真实尺度，即：t＝st*其中h为相机真实高度，h*为算法估计的路面高度(即相机高度)，t*为不带尺度的位移向量，t*为恢复尺度之后的位移向量。本文档来自技高网...

【技术保护点】
一种基于三角剖分的单目视觉里程计尺度恢复方法，其特征在于，包括以下步骤：1)根据单目视觉里程计获取前后两帧已经匹配上的第i个特征点

【技术特征摘要】
1.一种基于三角剖分的单目视觉里程计尺度恢复方法，其特征在于，包括以下步骤：1)根据单目视觉里程计获取前后两帧已经匹配上的第i个特征点和以及对应的3D空间坐标；2)根据单目视觉里程计得到的旋转矩阵R和不具备真实尺度的位移向量t*计算出路面倾角的估计值θ*；3)将已经匹配上的特征点在后一帧图像中的位置(u1,v1)进行三角剖分，得到符合条件的三角形组Tri；4)根据每个特征点的3D空间坐标，计算每个三角形所在的空间平面的法向向量和高度；5)根据每个三角形所在的空间平面的法向向量计算三角形所在的空间平面的俯仰角θj，筛选三角形；6)根据筛选出的三角形的顶点采用RANSAC方法得到路面的高度h*；7)将相机真实高度h与路面的高度h*作商得到单目视觉里程计的尺度参数s，将不具备真实尺度的位移向量t*与尺度参数s相乘，即可恢复视觉里程计绝对尺度。2.根据权利要求1所述的一种基于三角剖分的单目视觉里程计尺度恢复方法，其特征在于，所述的步骤4)中，第j个三角形的法向向量nj和高度hj的表达式为：njT*Xji+hj＝0j＝...

【专利技术属性】
技术研发人员：陈启军，王香伟，张会，尹小川，杜孝国，杜明晓，
申请(专利权)人：同济大学，
类型：发明
国别省市：上海,31