基于KDTree改進(jìn)的Super-4PCS+ICP算法在點(diǎn)云配準(zhǔn)中的應(yīng)用研究

夏軍勇,高睿杰,鐘 飛

(湖北工業(yè)大學(xué)機(jī)械工程學(xué)院,湖北 武漢 430068)

1 引 言

點(diǎn)云配準(zhǔn)是三維模型重建的關(guān)鍵環(huán)節(jié)。在工程實(shí)際測(cè)量中,安裝在掃描儀內(nèi)部的相機(jī)角度固定不動(dòng),要想獲得待測(cè)物體完整的點(diǎn)云數(shù)據(jù),需要多次轉(zhuǎn)動(dòng)不同的角度掃描,其中不同視角多次掃描就是點(diǎn)云配準(zhǔn)的過(guò)程,用最精確的點(diǎn)云配準(zhǔn)算法,對(duì)多個(gè)視角下獲得的不同點(diǎn)云片段進(jìn)行配準(zhǔn)處理[1]。從二十世紀(jì)末期開始,國(guó)外研究人員對(duì)點(diǎn)云配準(zhǔn)技術(shù)做了大量的實(shí)驗(yàn)研究,最開始配準(zhǔn)的是圖像,現(xiàn)在主要是配準(zhǔn)三維點(diǎn)云數(shù)據(jù)。

早在20世紀(jì)90年代,國(guó)外學(xué)者就將四元數(shù)求解矩陣的思想運(yùn)用到點(diǎn)云配準(zhǔn)上,提出用四元數(shù)法進(jìn)行點(diǎn)云配準(zhǔn)實(shí)驗(yàn)[2]。Besl[3]在1992年提出了最近點(diǎn)迭代算法ICP,這個(gè)算法至今還是使用最多、應(yīng)用最廣的算法。Chen[4]的改進(jìn)方法是將兩個(gè)對(duì)應(yīng)點(diǎn)切平面的距離來(lái)代替直線距離,這樣就可以讓點(diǎn)云重疊區(qū)域變多,成功實(shí)現(xiàn)了兩塊點(diǎn)云的配準(zhǔn)。文獻(xiàn)[5]提出了一種基于法向量與其K近鄰法向量夾角不變特征的點(diǎn)云配準(zhǔn)方法,可以加快點(diǎn)對(duì)應(yīng)關(guān)系的選擇。2010年,文獻(xiàn)[6]用最小二乘法匹配重疊曲面,先估計(jì)局部和模板坐標(biāo)系的剛體變換參數(shù)。Milos Prokop提出用KDTree結(jié)構(gòu)來(lái)提高搜索效率,用四元數(shù)法和最小二乘法計(jì)算配準(zhǔn)結(jié)果,這個(gè)方法明顯提高了配準(zhǔn)的速度和精度[7]。朱延娟等提出將曲率相似約束條件應(yīng)用到對(duì)應(yīng)點(diǎn)的構(gòu)造中,成功實(shí)現(xiàn)了配準(zhǔn)[8]。儲(chǔ)珺等利用二維SIFT提取特征點(diǎn)實(shí)現(xiàn)點(diǎn)云配準(zhǔn),這個(gè)方法可以有效排除無(wú)效數(shù)據(jù)的影響[9]。

針對(duì)于傳統(tǒng)的點(diǎn)云配準(zhǔn)在配準(zhǔn)精度上還是不能達(dá)到一些高精度設(shè)備的要求。Super_4PCS算法常用于粗配準(zhǔn),ICP算法常用于精配準(zhǔn),本文將對(duì)Super_4PCS算法和ICP算法進(jìn)行改進(jìn),通過(guò)對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行“先粗后精”處理,提高點(diǎn)云數(shù)據(jù)的配準(zhǔn)精度。本文提出的“先粗后精”點(diǎn)云配準(zhǔn)流程圖如圖1所示。

圖1 點(diǎn)云配準(zhǔn)流程

2 點(diǎn)云粗配準(zhǔn)

對(duì)于需要進(jìn)行配準(zhǔn)的兩片初始掃描點(diǎn)云,獲取手法及方式的差異會(huì)導(dǎo)致它們的初始位姿區(qū)別很大。而點(diǎn)云精配準(zhǔn)的前提是兩片點(diǎn)云初始位姿良好,故在精配準(zhǔn)前需要進(jìn)行粗配準(zhǔn)[10],使兩片點(diǎn)云的初始位姿大致對(duì)齊。粗配準(zhǔn)使用的算法是基于隨機(jī)采樣一致性思想,其中使用比較多的就是SAC-IA[11]算法以及Super-4PCS[12]算法。

2.1 Super-4PCS算法

4PCS是將同一個(gè)平面內(nèi)的四點(diǎn)作為基本單位點(diǎn)來(lái)尋找對(duì)應(yīng)點(diǎn),然后進(jìn)行RANSAC迭代。其原理是根據(jù)同一平面內(nèi)四點(diǎn)之間的仿射不變關(guān)系來(lái)確定對(duì)應(yīng)點(diǎn)。假定其共面的四點(diǎn)B={a,b,c,d}可以確定滿足下列關(guān)系的兩個(gè)仿射不變量r1和r2:

(1)

(2)

得到了兩個(gè)仿射不變量后尋找對(duì)應(yīng)的四點(diǎn)集,取目標(biāo)點(diǎn)云里面的任意兩個(gè)點(diǎn),計(jì)算可能會(huì)存在的交點(diǎn)e1,e2:

e1=q1+r1(q2-q1)

(3)

e2=q1+r2(q2-q1)

(4)

假如任意選取的一對(duì)點(diǎn)所計(jì)算出來(lái)的e1與另一對(duì)點(diǎn)計(jì)算出來(lái)的e2近似相等,如下圖3所示,就可以確定這兩對(duì)點(diǎn)之間構(gòu)成的四點(diǎn)集U1是和基B對(duì)應(yīng)的一個(gè)四點(diǎn)集。而對(duì)于剛體而言,剛體變換時(shí)對(duì)應(yīng)點(diǎn)對(duì)之間的距離保持不變,故在點(diǎn)云旋轉(zhuǎn)平移變換過(guò)程中四點(diǎn)集還應(yīng)該滿足下式條件:

(5)

(6)

Super-4PCS算法降低了4PCS的計(jì)算復(fù)雜度,該算法通過(guò)角度的約束手段減少了4PCS算法中無(wú)效對(duì)的產(chǎn)生。同時(shí)采用了一種柵格化的方法,將計(jì)算距離的二次復(fù)雜度降低了,從而極大地提高算法的效率。

2.2 基于KDTree樹改進(jìn)的Super-4PCS點(diǎn)云粗配準(zhǔn)

本文點(diǎn)云數(shù)據(jù)由三維激光掃描儀FreeScan_UE自動(dòng)掃描輪廓獲得,數(shù)據(jù)量很大,在對(duì)點(diǎn)云處理過(guò)程中會(huì)耗費(fèi)大量的時(shí)間,極大地影響了點(diǎn)云配準(zhǔn)的效率。故本文先對(duì)掃描得到的數(shù)據(jù)進(jìn)行去噪處理,在保留特征點(diǎn)的前提下進(jìn)行點(diǎn)云精簡(jiǎn),極大地減少了點(diǎn)云數(shù)量。然后提出KDTree樹搜索對(duì)應(yīng)點(diǎn),結(jié)合提取特征點(diǎn)[13]方法對(duì)點(diǎn)云的粗配準(zhǔn)進(jìn)行改進(jìn),解決計(jì)算量巨大的問(wèn)題。

本文將選取局部區(qū)域法向量變化較大的點(diǎn)集來(lái)選為特征點(diǎn)集。假定點(diǎn)云區(qū)域內(nèi)的一點(diǎn)pi處法向量的特征度用k近鄰點(diǎn)法向量和此局部區(qū)域本身的法向量夾角的算術(shù)平均值來(lái)表示:

(7)

式中,θij是近鄰點(diǎn)pj處的法向量與點(diǎn)pi之間的夾角。

選取合適的閾值ε1,因?yàn)閒i<ε1時(shí)會(huì)出現(xiàn)局部區(qū)域特征不明顯的問(wèn)題,故選取所有fi>ε1的點(diǎn),而fi<ε1的全部不考慮,去除這不部分無(wú)用點(diǎn)。對(duì)于其中任意一點(diǎn)pm,若滿足:

f(pm)=max[f(pm1),f(pm2),…f(pmk)]

(8)

其中,點(diǎn)pm就被設(shè)定為局部區(qū)域的一個(gè)特征點(diǎn)。

本文改進(jìn)方法的具體步驟為:

1)對(duì)初始散亂點(diǎn)云建立KDTree,加速鄰域點(diǎn)的查找。通過(guò)三維激光掃描儀獲得的點(diǎn)云數(shù)據(jù)是散亂的,點(diǎn)云的分布沒有規(guī)律,無(wú)法用數(shù)學(xué)模型來(lái)描述,故無(wú)法對(duì)點(diǎn)云坐標(biāo)信息進(jìn)行查詢,所以需要建立拓?fù)湫畔⑹沟脪呙璧玫降狞c(diǎn)云數(shù)據(jù)變得更有規(guī)律,從而實(shí)現(xiàn)可查詢的功能和要求,而KDTree就是建立拓?fù)潢P(guān)系中應(yīng)用最為廣泛的一種方法。

2)對(duì)散亂點(diǎn)云數(shù)據(jù)按照設(shè)置的邊長(zhǎng)l mm進(jìn)行分塊,使其在邊長(zhǎng)為l mm的正方形小局部區(qū)域內(nèi)都能獲得相應(yīng)的法向量。

3)通過(guò)計(jì)算指定的夾角算術(shù)平均值得出各分塊區(qū)域的特征度,并將特征度大于給定閾值的點(diǎn)保留下來(lái)。

4)選取任意保留點(diǎn)計(jì)算其鄰近點(diǎn)的特征度,若保留點(diǎn)的特征度為最大值,則此點(diǎn)即為此局部區(qū)域的特征點(diǎn),直至選完所有的特征點(diǎn)。

5)再使用Super-4PCS算法中的隨機(jī)采樣一致性算法RANSAC進(jìn)行迭代來(lái)確定匹配關(guān)系,實(shí)現(xiàn)點(diǎn)云粗配準(zhǔn),求解旋轉(zhuǎn)平移矩陣。

2.3 點(diǎn)云粗配準(zhǔn)實(shí)驗(yàn)及分析

為驗(yàn)證本文方法改進(jìn)的有效性,本文將在AMD Ryzen 7 2700,機(jī)帶RAM為16GB的Windows10操作系統(tǒng)上基于MATLAB平臺(tái)對(duì)標(biāo)準(zhǔn)的點(diǎn)云測(cè)試實(shí)例斯坦福兔點(diǎn)云、龍點(diǎn)云進(jìn)行算法有效性驗(yàn)證實(shí)驗(yàn)。其中源點(diǎn)云為深灰色顯示,目標(biāo)點(diǎn)云淺灰色顯示。各實(shí)例粗配準(zhǔn)實(shí)驗(yàn)前初始位姿如圖2所示。

圖2 粗配準(zhǔn)實(shí)驗(yàn)前初始位姿

對(duì)其使用Super-4PCS算法及本文改進(jìn)算法進(jìn)行粗配準(zhǔn),配準(zhǔn)后得到的效果圖如圖3所示。

圖3 粗配準(zhǔn)結(jié)果

本文方法成功完成了源點(diǎn)云與目標(biāo)點(diǎn)云的粗配準(zhǔn)。兔點(diǎn)云和龍點(diǎn)云的點(diǎn)云數(shù)據(jù)殘缺較大,輪廓特征不明顯,粗配準(zhǔn)效果一般。但相對(duì)于Super-4PCS算法,明顯可以看出本文方法配準(zhǔn)效果更優(yōu)。

Super-4PCS與本文方法的粗配準(zhǔn)實(shí)驗(yàn)數(shù)據(jù)結(jié)果如上表1所示,分別列出了粗配準(zhǔn)時(shí)間、粗配準(zhǔn)誤差。從表中可以看出,兩種方法在配準(zhǔn)時(shí)間相差無(wú)幾的情況下,應(yīng)用本文方法的兩個(gè)實(shí)例兩片初始點(diǎn)云已經(jīng)大部分重疊,成功實(shí)現(xiàn)了源點(diǎn)云與目標(biāo)點(diǎn)云的粗配準(zhǔn),且配準(zhǔn)誤差相對(duì)于Super-4PCS算法明顯降低,配準(zhǔn)精度有所提高。本文方法對(duì)標(biāo)準(zhǔn)數(shù)據(jù)庫(kù)實(shí)例斯坦福兔點(diǎn)云進(jìn)行點(diǎn)云配準(zhǔn)時(shí),其配準(zhǔn)精度相對(duì)于Super-4PCS算法最大提高了38.71 %,而龍點(diǎn)云的配準(zhǔn)精度提高了11.8 %。數(shù)據(jù)證明本文改進(jìn)方法的可行性。滿足后續(xù)使用精配準(zhǔn)算法對(duì)于初始位姿的要求。

3 點(diǎn)云精配準(zhǔn)

上一節(jié)介紹了兩片散亂點(diǎn)云的粗配準(zhǔn),從實(shí)驗(yàn)結(jié)果可以看出,散亂點(diǎn)云經(jīng)過(guò)粗配準(zhǔn)后,兩片點(diǎn)云的初始位姿發(fā)生了很大變化,距離變近,方向?qū)R,基本實(shí)現(xiàn)重疊。但是對(duì)于初始位姿很差的點(diǎn)云,錯(cuò)位等問(wèn)題依然存在,粗配準(zhǔn)的精度還遠(yuǎn)遠(yuǎn)達(dá)不到我們的要求。故在此基礎(chǔ)上需要進(jìn)行精配準(zhǔn)來(lái)提升配準(zhǔn)精度。

3.1 ICP算法

(9)

然后求解使得上述誤差函數(shù)值最小的剛體變換向量q=[qR|qR]T,記作(q,d)=?(P,Y),上式中d為相應(yīng)的均方誤差函數(shù),即d=f(q)。算法將求解得到的剛體變換作用到第一片點(diǎn)云數(shù)據(jù)上,記作q(P)。ICP算法迭代地進(jìn)行該操作直到滿足某一設(shè)定的收斂準(zhǔn)則。

3.2 基于KDTree樹改進(jìn)的ICP點(diǎn)云精配準(zhǔn)

1)減少無(wú)用點(diǎn)集,本文在進(jìn)行實(shí)驗(yàn)之前,用三維掃描儀獲取點(diǎn)云時(shí),會(huì)將旁邊環(huán)境物刷漆涂黑,使周圍物體不被掃出,避免出現(xiàn)不必要的噪聲點(diǎn)。

2)對(duì)應(yīng)點(diǎn)的選擇采用的是KDTree結(jié)構(gòu)搜索歐式距離最小的點(diǎn)。匹配點(diǎn)是選擇源點(diǎn)云中與其歐式距離最近的點(diǎn),設(shè)定距離閾值,剔除錯(cuò)誤點(diǎn)對(duì)。因?yàn)榻?jīng)過(guò)粗配準(zhǔn)后的兩片點(diǎn)云初始很近,不需要再用特征點(diǎn)進(jìn)行快速匹配,而使用KDTree臨近查找可以極大地提高搜索效率。

3)點(diǎn)加權(quán)采用所有點(diǎn)對(duì)常數(shù)加權(quán)均勻分配,并取權(quán)值為常數(shù)1,因?yàn)楸疚木_配準(zhǔn)之前使用了粗配準(zhǔn),初始位姿保持極好,使用常數(shù)為1的加權(quán)可以滿足條件且便于計(jì)算。

4)去除算術(shù)平均值以下的錯(cuò)誤點(diǎn)對(duì),只保留處于閾值范圍內(nèi)的對(duì)應(yīng)點(diǎn)。誤差函數(shù)選取點(diǎn)到面的距離平方和。

5)將目標(biāo)誤差函數(shù)最小化采用自適應(yīng)閾值的方式,去中心化后對(duì)斜方差矩陣進(jìn)行SVD分解[14]和ICP算法計(jì)算[15-17],求解當(dāng)前旋轉(zhuǎn)矩陣和平移向量的誤差函數(shù)是否收斂,若誤差函數(shù)不收斂,則回到KDTree搜索對(duì)應(yīng)點(diǎn)繼續(xù)計(jì)算,直至迭代收斂,算出最后的旋轉(zhuǎn)平移矩陣。

其中,SVD分解具體原理如下所示為:

對(duì)于任意的矩陣A∈Rm×n,存在矩陣U=[u1u2…un]∈Rm×n和V=[u1u2…un]∈Rm×n使得滿足:

(10)

式中,∑r=diag(σ1…σn)且σ1≤σ2≤…?σn。

基于SVD分解改進(jìn)的ICP算法的計(jì)算步驟如下示:

首先計(jì)算目標(biāo)數(shù)據(jù)點(diǎn)集P和源數(shù)據(jù)點(diǎn)集Q的重心坐標(biāo);

其次計(jì)算源數(shù)據(jù)點(diǎn)集和目標(biāo)數(shù)據(jù)點(diǎn)集中點(diǎn)坐標(biāo)相對(duì)于重心坐標(biāo)的變化量;

(11)

(12)

然后利用其各自的重心變化量構(gòu)造SVD分解矩陣;

(13)

最后計(jì)算旋轉(zhuǎn)平移矩陣。

(14)

R=VUT

(15)

3.3 點(diǎn)云精配準(zhǔn)的實(shí)驗(yàn)及分析

為驗(yàn)證本文改進(jìn)的點(diǎn)云精配準(zhǔn)方法的有效性和可行性,繼續(xù)使用標(biāo)準(zhǔn)數(shù)據(jù)庫(kù)中的兔點(diǎn)云和龍點(diǎn)云及非標(biāo)準(zhǔn)實(shí)例籃球進(jìn)行有效性驗(yàn)證。其中深灰色點(diǎn)云為源點(diǎn)云,淺灰色點(diǎn)云為目標(biāo)點(diǎn)云,使用ICP算法和本文改進(jìn)精配準(zhǔn)算法得到的實(shí)驗(yàn)結(jié)果如圖4所示。

圖4 精配準(zhǔn)后結(jié)果

從效果圖上看,相較于ICP算法,本文方法對(duì)點(diǎn)云精配準(zhǔn)的效果更加好。但由于兩點(diǎn)云的配準(zhǔn)誤差非常細(xì)微,故對(duì)兩種方法的實(shí)驗(yàn)數(shù)據(jù)進(jìn)行統(tǒng)計(jì),作出詳細(xì)對(duì)比,如表2所示。

表2 精配準(zhǔn)結(jié)果數(shù)據(jù)

由上述可知,本文方法進(jìn)行精配準(zhǔn)后,相較于ICP算法來(lái)說(shuō)配準(zhǔn)誤差更小,配準(zhǔn)精度得到了提高,表明了本文方法精配準(zhǔn)的必要性。使用本文方法配準(zhǔn)標(biāo)準(zhǔn)例子兔點(diǎn)云和龍點(diǎn)云的精度相對(duì)于傳統(tǒng)ICP算法精度提高了41.77 %和68.12 %,證明了本文改進(jìn)方法的有效性。

兔點(diǎn)云旋轉(zhuǎn)平移矩陣為:

龍點(diǎn)云旋轉(zhuǎn)平移矩陣為:

籃球點(diǎn)云旋轉(zhuǎn)平移矩陣為:

4 結(jié) 語(yǔ)

本文利用“先粗后精”的思想對(duì)兩點(diǎn)云進(jìn)行配準(zhǔn)。首先對(duì)Super-4PCS算法改進(jìn)優(yōu)化,進(jìn)行粗配準(zhǔn)。配準(zhǔn)精度最大提高了38.71 %。然后在精配準(zhǔn)時(shí)用KDTree臨近查找,常數(shù)加權(quán)及對(duì)誤差函數(shù)采用自適應(yīng)閾值等方法對(duì)ICP算法進(jìn)行優(yōu)化,分別使用標(biāo)準(zhǔn)數(shù)據(jù)庫(kù)和非標(biāo)準(zhǔn)例子進(jìn)行實(shí)驗(yàn)驗(yàn)證,實(shí)驗(yàn)結(jié)果表明本文方法精配準(zhǔn)的效果相較于ICP算法的配準(zhǔn)效果有顯著提升,配準(zhǔn)精度相較于ICP算法最大提升了68.12 %。因此本文的點(diǎn)云配準(zhǔn)方法對(duì)后期點(diǎn)云模型重建有一定的參考價(jià)值。