本發(fā)明涉及一種尺度恢復(fù)方法，尤其是涉及一種基于三角剖分的單目視覺里程計(jì)尺度恢復(fù)方法。

背景技術(shù)：

目前常用計(jì)算單目視覺里程計(jì)尺度的方法主要分為兩種，一種需要融合其他傳感器如激光雷達(dá)或者慣性傳感器，另一種是考慮環(huán)境中的已知尺度。前一種方法可以更為精準(zhǔn)的回復(fù)單目尺度，但增加了使用成本，同時(shí)使用前需要精準(zhǔn)的標(biāo)定；后一種方法常用的環(huán)境中已知尺度一般為相機(jī)到路面的高度，然后通過計(jì)算路面的幾何模型來恢復(fù)。常用的計(jì)算路面幾何模型的方法主要分為兩種，一種是通過特征點(diǎn)的3d位置，用ransac的方法提出噪聲或依賴于路面識(shí)別提出噪聲，另外一種是通過在已知路面區(qū)域的前提下通過最小化單應(yīng)性誤差來計(jì)算路面幾何模型；前一種方法往往由于路面上特征點(diǎn)較少無法準(zhǔn)確的計(jì)算，后一種方法由于算法計(jì)算復(fù)雜度較高，需要大量的計(jì)算時(shí)間或計(jì)算資源。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的就是為了克服上述現(xiàn)有技術(shù)存在的缺陷而提供一種速度快、準(zhǔn)確率高、無需做圖像分割和路面識(shí)別、魯棒性強(qiáng)的基于三角剖分的單目視覺里程計(jì)尺度恢復(fù)方法。

本發(fā)明的目的可以通過以下技術(shù)方案來實(shí)現(xiàn)：

一種基于三角剖分的單目視覺里程計(jì)尺度恢復(fù)方法，包括以下步驟：

1)根據(jù)單目視覺里程計(jì)獲取前后兩幀已經(jīng)匹配上的第i個(gè)特征點(diǎn)和以及對(duì)應(yīng)的3d空間坐標(biāo)；

2)根據(jù)單目視覺里程計(jì)得到的旋轉(zhuǎn)矩陣r和不具備真實(shí)尺度的位移向量t^*計(jì)算出路面傾角的估計(jì)值θ^*；

3)將已經(jīng)匹配上的特征點(diǎn)在后一幀圖像中的位置(u1,v1)進(jìn)行三角剖分，得到符合條件的三角形組tri；

4)根據(jù)每個(gè)特征點(diǎn)的3d空間坐標(biāo)，計(jì)算每個(gè)三角形所在的空間平面的法向向量和高度；

5)根據(jù)每個(gè)三角形所在的空間平面的法向向量計(jì)算三角形所在的空間平面的俯仰角θj，篩選三角形；

6)根據(jù)篩選出的三角形的頂點(diǎn)采用ransac方法得到路面的高度h^*；

7)將相機(jī)真實(shí)高度h與路面的高度h^*作商得到單目視覺里程計(jì)的尺度參數(shù)s，將不具備真實(shí)尺度的位移向量t^*與尺度參數(shù)s相乘，即可恢復(fù)視覺里程計(jì)絕對(duì)尺度。

所述的步驟4)中，第j個(gè)三角形的法向向量nj和高度hj的表達(dá)式為：

nj^t*xji+hj＝0j＝1,2,...,mi＝1,2,3

其中，xji為第j個(gè)三角形的第i個(gè)頂點(diǎn)，nj為第j個(gè)三角形的法向向量，hj為第j個(gè)三角形的高度。

所述的路面傾角的估計(jì)值θ^*的計(jì)算式為：

其中，t^*為不具備真實(shí)尺度的位移向量，t^*[2]為不具備真實(shí)尺度的位移向量t^*的第二個(gè)值，為t^*的模長。

所述的三角形所在的空間平面的俯仰角θj的計(jì)算式為：

其中，nj[2]為三角形法向向量nj的第二個(gè)值，為nj的模長。

所述的步驟5)中，篩選出三角形所在的空間平面的俯仰角θj與路面傾角的估計(jì)值θ^*的差值小于5度時(shí)對(duì)應(yīng)的三角形。

與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點(diǎn)：

一、無需做圖像分割和路面識(shí)別，僅根據(jù)視覺里程計(jì)計(jì)算出特征點(diǎn)3d坐標(biāo)計(jì)算路面幾何模型，魯棒性強(qiáng)；

二、使用三角剖分方法判斷特征點(diǎn)是否屬于路面，速度快而且準(zhǔn)確率高。

附圖說明

圖1為本發(fā)明的方法原理圖。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說明。

實(shí)施例

如圖1所示，本發(fā)明的計(jì)算步驟如下：

1)根據(jù)單目視覺里程計(jì)獲取前后兩幀已經(jīng)匹配上的第i個(gè)特征點(diǎn)和以及對(duì)應(yīng)的3d空間坐標(biāo)；

2)根據(jù)單目視覺里程計(jì)得到的旋轉(zhuǎn)矩陣r和位移向量t^*計(jì)算出路面傾角的估計(jì)值θ^*(計(jì)算公式見下文)；

3)將已經(jīng)匹配上的特征點(diǎn)在后一幀圖像中的位置(u1,v1)進(jìn)行三角剖分，得到符合條件的三角形組tri；

4)根據(jù)每個(gè)特征點(diǎn)的3d空間坐標(biāo)，計(jì)算每個(gè)三角形所在的空間平面的法向向量和高度；

5)根據(jù)路面傾角的估計(jì)值θ^*以及由每個(gè)三角形由法向向量計(jì)算三角形屬于路面的傾角篩選三角形，保留誤差在5度以內(nèi)的三角形；

6)使用步驟5)中符合條件的三角形的頂點(diǎn)使用ransac方法得到路面估計(jì)模型和符合路面模型的特征點(diǎn)

7)根據(jù)選出三角形對(duì)應(yīng)的特征點(diǎn)獲取路面的幾何模型，并將相機(jī)真實(shí)高度與計(jì)算出來的高度相除得到單目視覺里程計(jì)的尺度參數(shù)，將此位移向量t^*乘此此參數(shù)，恢復(fù)即可恢復(fù)視覺里程計(jì)絕對(duì)尺度。

本方法詳細(xì)說明如下：

首先利用現(xiàn)有技術(shù)(orbslam，libviso等)提取并匹配相機(jī)兩幀圖像中特征點(diǎn)和其中i＝1,2,...,n,為匹配上的特征點(diǎn)的個(gè)數(shù)，然后計(jì)算出相機(jī)在前后兩幀之間的位姿變化r,t^*，其中r為旋轉(zhuǎn)矩陣，t^*為位移向量，由于單目視覺損失了深度信息，此處計(jì)算出來的位移不具備真實(shí)的尺度，文中所有相對(duì)尺度的變量均有上角標(biāo)*。繼而通過三角測(cè)量得到所有成功匹配的特征點(diǎn)的3d坐標(biāo)

利用三角剖分技術(shù)將后一幀圖像中的特征點(diǎn)分成符合規(guī)則的三角形組。

對(duì)于每一個(gè)三角形，根據(jù)之前求出的每個(gè)點(diǎn)的3d坐標(biāo)，根據(jù)如下公式求出每個(gè)三角形所在平面的法向向量

nj^t*xji+hj＝0j＝1,2,...,mi＝1,2,3

其中m為三角形組中三角形的個(gè)數(shù)，j為三角形序號(hào)，i為三角形內(nèi)三個(gè)頂點(diǎn)的序號(hào)nj為第j個(gè)三角形的法向，hj為第j個(gè)三角形的高度，對(duì)于每一個(gè)三角形,

忽略法向的模長，

求得nj之后，便可求出這個(gè)平面在相機(jī)坐標(biāo)系中的俯仰角

可以根據(jù)單目vo計(jì)算得到的相機(jī)運(yùn)動(dòng)的旋轉(zhuǎn)矩陣r和位移向量t估計(jì)出地面相對(duì)于相機(jī)的俯仰角。當(dāng)汽車行駛在平路上時(shí)，vo所計(jì)算出來的位移的俯仰角本質(zhì)上就是路的俯仰角，如下圖所示。此處的平路指汽車不發(fā)生俯仰方向旋轉(zhuǎn)的路，包括符合這條件的坡路。計(jì)算方法如下：

首先根據(jù)旋轉(zhuǎn)矩陣r，計(jì)算汽車是否有俯仰

θr＝arctan(-r32/r33)

其中rijr中底i行第j列的元素，若θr小于5度，則認(rèn)為汽車沒有俯仰運(yùn)動(dòng)，根據(jù)下面公式計(jì)算路面俯仰角，若θr小于5度則路面俯仰角置為上一時(shí)刻計(jì)算的俯仰角

比較平面俯仰角和道路俯仰角，相差越小，則說明這一平面在道路上的概率越大。然后考慮各個(gè)三角形的中心高度，首先剔除所有在相機(jī)上方的點(diǎn)，在剩下的點(diǎn)中，選擇道路平面俯仰角和道路俯仰角相差5度以內(nèi)的三角形。結(jié)果大概如下圖所示，綠色區(qū)域?yàn)楣烙?jì)路面區(qū)域，然后根據(jù)路屬于三角形的頂點(diǎn)使用ransac方法得到路面法向和高度，ransac方法具體步驟為：

在篩選過后的三角形頂點(diǎn)中隨機(jī)抽取三個(gè)頂點(diǎn)，并使用這三個(gè)頂點(diǎn)計(jì)算其所在平面的幾何模型：

nx+h＝0

(該計(jì)算方法與前文計(jì)算三角形幾何模型方法一致)，然后計(jì)算除此三點(diǎn)外所有的三角形頂點(diǎn)到這個(gè)平面的距離d＝|nxi+h|，距離小于高度0.1h的點(diǎn)認(rèn)為是內(nèi)點(diǎn)，并統(tǒng)計(jì)內(nèi)點(diǎn)數(shù)量；然后重復(fù)此步驟，隨機(jī)選取多次，取內(nèi)點(diǎn)最多的路面幾何模型為路面最終幾何模型。

最后將真實(shí)高度和這一幾何模型中的高度相比得到真實(shí)尺度，即：

t＝st^*

其中h為相機(jī)真實(shí)高度，h^*為算法估計(jì)的路面高度(即相機(jī)高度)，t^*為不帶尺度的位移向量，t^*為恢復(fù)尺度之后的位移向量。