[發(fā)明專利]一種基于三維曲線匹配的點(diǎn)云相對(duì)位姿估計(jì)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201310742491.2
• 申請(qǐng)日: 2013-12-30
• 公開(kāi)（公告）號(hào): CN103729882A
• 公開(kāi)（公告）日: 2014-04-16
• 發(fā)明（設(shè)計(jì)）人: 熊蓉;李千山
• 申請(qǐng)（專利權(quán)）人: 浙江大學(xué)
• 主分類號(hào): G06T17/00
• 分類號(hào): G06T17/00
• 代理公司: 杭州求是專利事務(wù)所有限公司 33200
• 代理人: 張法高

技術(shù)領(lǐng)域

本發(fā)明涉及三維環(huán)境重構(gòu)領(lǐng)域，尤其涉及一種基于三維曲線匹配的點(diǎn)云相對(duì)位姿估計(jì)方法。

背景技術(shù)

已有的點(diǎn)云相對(duì)位姿估計(jì)方法主要分為兩類。一類基于點(diǎn)云特征匹配，缺點(diǎn)是受點(diǎn)云密度變化影響很大，密度差異會(huì)直接導(dǎo)致錯(cuò)誤概率提高。另一類基于點(diǎn)云的直接匹配，缺點(diǎn)是依賴初始位姿，且運(yùn)算量大。

對(duì)于三維曲線的匹配，已有方法也主要基于卷積，難以適用于多曲線對(duì)多曲線的匹配。

發(fā)明內(nèi)容

本發(fā)明的目的是克服現(xiàn)有技術(shù)的不足，提供一種基于三維曲線匹配的點(diǎn)云相對(duì)位姿估計(jì)方法。

基于三維曲線匹配的點(diǎn)云相對(duì)位姿估計(jì)方法是：

1）分別提取兩幀輸入點(diǎn)云的邊緣輪廓，得到代表三維邊緣輪廓曲線的三維點(diǎn)集????????????????????????????????????????????????、；

2）分別針對(duì)三維點(diǎn)集、提取得到有序點(diǎn)集的集合、,每個(gè)有序點(diǎn)集或均表示了一條三維曲線；

3）將每個(gè)有序點(diǎn)集或進(jìn)行分解，分解后的每個(gè)子集表示一個(gè)圓弧段，建立每段圓弧的特征描述向量；

4）匹配兩幀點(diǎn)云邊緣曲線圓弧段的特征描述向量，建立匹配關(guān)系，計(jì)算兩幀點(diǎn)云的相對(duì)位姿變換矩陣,為旋轉(zhuǎn)矩陣，為平移矩陣。

所述的步驟1）為：a）對(duì)于無(wú)序點(diǎn)云，搜索每個(gè)點(diǎn)周圍半徑小于范圍內(nèi)的數(shù)量不超過(guò)的所有最近鄰點(diǎn)，得到點(diǎn)集，為點(diǎn)集擬合平面，以點(diǎn)集在平面內(nèi)的投影點(diǎn)位置為自變量，點(diǎn)集到平面的距離為函數(shù)值，擬合二元二次函數(shù)，得到Hessian矩陣，計(jì)算Hessian矩陣的特征值和，假定，若且，其中、分別為相應(yīng)閾值，則認(rèn)為該點(diǎn)為邊緣點(diǎn)；b）對(duì)于有序點(diǎn)云,即深度圖，利用Canny算法提取邊緣點(diǎn)集。

所述的為點(diǎn)集擬合平面的方法為：計(jì)算點(diǎn)集的均值，得到平面的中心；計(jì)算的特征向量，其最小特征值對(duì)應(yīng)的特征向量即為平面的法向量；平面的中心和法向量即表示了一個(gè)經(jīng)過(guò)中心、法向量為的平面。

所述的擬合二元二次函數(shù)并得到Hessian矩陣的方法為：對(duì)于點(diǎn)集中每一點(diǎn)，假定最大的兩個(gè)特征值對(duì)應(yīng)的特征向量分別為和，計(jì)算一個(gè)以、為自變量，為值的鍵值對(duì)，?

最終形成一組向的鍵值映射,并利用最小二乘法求取Hessian矩陣

，

則可表示為

。

所述的步驟2）為：使用區(qū)域增長(zhǎng)算法，以每次從三維點(diǎn)集或中隨機(jī)選取的區(qū)域增長(zhǎng)種子點(diǎn)為起始，以點(diǎn)集所表示的曲線一階連續(xù)為條件，通過(guò)不斷吸納符合增長(zhǎng)條件的新點(diǎn)，有序地拓展曲線點(diǎn)集，若不能再拓展，則另取種子點(diǎn)并拓展另一個(gè)曲線點(diǎn)集，直至所有點(diǎn)被拓展完畢。

所述的曲線一階連續(xù)條件為：

其中為點(diǎn)云中兩近鄰點(diǎn)、之間的距離，、分別為兩點(diǎn)離坐標(biāo)原點(diǎn)的距離，為閾值。

所述的步驟3）為：對(duì)于每一個(gè)表示三維曲線的有序點(diǎn)集或，依次計(jì)算每個(gè)點(diǎn)處曲線的曲率和扭率，

其中

為有序點(diǎn)集中的下一個(gè)點(diǎn)，隨后將表示三維曲線的有序點(diǎn)集拆分成若干子集，每個(gè)子集內(nèi)的點(diǎn)具有相同的曲線曲率，且扭率為零，即該子集表示了一個(gè)圓弧，記該圓弧曲率為，弧長(zhǎng)為，中心坐標(biāo)為，依據(jù)右手坐標(biāo)系規(guī)則，以圓弧法方向、圓弧起始點(diǎn)切法向以及與這兩個(gè)方向垂直的方向?yàn)槿齻€(gè)軸，建立坐標(biāo)系，記該坐標(biāo)系和世界坐標(biāo)系所成角度向量為,定義該圓弧的特征描述向量為

。

所述的步驟4）為：?針對(duì)兩幀點(diǎn)云邊緣曲線圓弧段的特征描述向量，建立k-d樹(shù)，在k-d樹(shù)中，建立兩幀點(diǎn)云邊緣曲線圓弧段之間的匹配關(guān)系，運(yùn)用隨機(jī)抽樣一致性算法，去除部分錯(cuò)誤匹配，建立代價(jià)函數(shù)

其中為兩幀點(diǎn)云邊緣曲線圓弧段之間的匹配關(guān)系中一組匹配關(guān)系，為所有存在的匹配關(guān)系，、分別為兩幀點(diǎn)云中的一段圓弧段的特征描述向量，

是評(píng)估特征描述向量、之間距離的函數(shù)，與兩幀點(diǎn)云的相對(duì)位姿轉(zhuǎn)換矩陣有關(guān)，的定義為

其中、為指定權(quán)重的參數(shù)，

最后，優(yōu)化求解

得到兩幀點(diǎn)云的相對(duì)位姿變換矩陣。

本發(fā)明與現(xiàn)有技術(shù)相比，具有的有益效果：

1.不依賴初始位姿，僅通過(guò)環(huán)境的輪廓進(jìn)行匹配，估計(jì)相對(duì)位姿；

2.僅處理邊緣點(diǎn)集，降低了運(yùn)算量；

3.適應(yīng)多曲線對(duì)多曲線的匹配，可抵抗點(diǎn)云中干擾物造成的影響。