一種基于K-D樹優(yōu)化的ICP三維點(diǎn)云配準(zhǔn)方法

劉　江,張　旭,朱繼文

(1.黑龍江工程學(xué)院 測繪工程學(xué)院，黑龍江 哈爾濱 150050；2.北京建筑大學(xué) 測繪與城市空間信息學(xué)院,北京 100044)

?

一種基于K-D樹優(yōu)化的ICP三維點(diǎn)云配準(zhǔn)方法

劉江1,張旭2,朱繼文1

(1.黑龍江工程學(xué)院 測繪工程學(xué)院，黑龍江 哈爾濱 150050；2.北京建筑大學(xué) 測繪與城市空間信息學(xué)院,北京 100044)

摘要：為提高三維點(diǎn)云數(shù)據(jù)配準(zhǔn)精度和速度，提出一種基于K-D樹優(yōu)化的ICP三維點(diǎn)云配準(zhǔn)方法，首先采用中心重合法實(shí)現(xiàn)點(diǎn)云數(shù)據(jù)的粗配準(zhǔn)，然后利用K-D tree快速搜索最近點(diǎn)對改進(jìn)傳統(tǒng)ICP方法，完成三維點(diǎn)云數(shù)據(jù)精配準(zhǔn)，該方法克服傳統(tǒng)ICP算法中由于利用歐式距離來判斷最近點(diǎn)所引起的工作量大、耗費(fèi)時(shí)間多的缺陷，提高點(diǎn)云的配準(zhǔn)速度。在此基礎(chǔ)上利用斯坦福不同密度Bunny點(diǎn)云數(shù)據(jù)進(jìn)行實(shí)驗(yàn)驗(yàn)證，結(jié)果表明在采用中心重合法實(shí)現(xiàn)三維點(diǎn)云粗配準(zhǔn)的基礎(chǔ)上，利用K-D tree優(yōu)化ICP算法，能夠提高點(diǎn)云配準(zhǔn)的精度、速度和穩(wěn)定性。

關(guān)鍵詞：點(diǎn)云配準(zhǔn)；k-d tree；中心重合；精度；穩(wěn)定性

隨著數(shù)字城市不斷向前發(fā)展，大規(guī)模三維數(shù)據(jù)采集技術(shù)迅速提升，由激光原理、攝影測量原理等方式產(chǎn)生多種點(diǎn)云數(shù)據(jù)。這些三維點(diǎn)云數(shù)據(jù)不僅可以記錄物體的空間信息，同時(shí)還可以得到物體表面的幾何信息。在實(shí)際獲取點(diǎn)云數(shù)據(jù)時(shí)考慮到測量設(shè)備、測量范圍的限制以及被測物體外形的復(fù)雜性等，每次掃描只能獲取當(dāng)前視點(diǎn)下的點(diǎn)云，其坐標(biāo)是相對于當(dāng)前的儀器坐標(biāo)系而言的，要得到被測物體完整的三維模型，需要從不同的視點(diǎn)對被測物體進(jìn)行掃描，并將不同視點(diǎn)獲取的點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)。

H.Alt等提出基于Hausdorff距離和Frechetl距離幾何結(jié)構(gòu)形狀的配準(zhǔn)[1]。其中，在解決噪聲點(diǎn)和局部遮擋等問題上Hausdorff距離體現(xiàn)出明顯的優(yōu)勢，同時(shí)Hausdorff距離解決在計(jì)算變形時(shí)歐式距離所產(chǎn)生的扭曲與失真[2]。Frechetl距離是基于收斂性提出的，因此配準(zhǔn)后的點(diǎn)云數(shù)據(jù)具有很強(qiáng)的收斂特性。Barequet等提出以有向腳標(biāo)對部分表面進(jìn)行配準(zhǔn)，用這種方法進(jìn)行配準(zhǔn)既簡單而又快速，但是不足之處是配準(zhǔn)結(jié)果的精確度不高[3]。這些算法彼此之間存在著一定的共性，都是根據(jù)點(diǎn)云或曲面尋找特征相似的區(qū)域，尋求最佳的目標(biāo)函數(shù)，利用目標(biāo)函數(shù)值最小化來提高配準(zhǔn)的精度。ICP算法最初由Besl和Mckey提出，是一種點(diǎn)配準(zhǔn)方法，但配準(zhǔn)的效率不高、耗費(fèi)時(shí)間長，需要對算法進(jìn)行一定的改進(jìn)。在本文應(yīng)用k-d tree的方法對ICP算法進(jìn)行改進(jìn)，進(jìn)一步提高點(diǎn)云的配準(zhǔn)效率。

1改進(jìn)的三維點(diǎn)云配準(zhǔn)算法

點(diǎn)云配準(zhǔn)大致分成兩個(gè)過程粗配準(zhǔn)和精配準(zhǔn)，粗配準(zhǔn)是對點(diǎn)云數(shù)據(jù)的重疊部分進(jìn)行大致擬合，為精配準(zhǔn)提供平移和旋轉(zhuǎn)參數(shù)的初始值，精配準(zhǔn)是在粗配準(zhǔn)的基礎(chǔ)上進(jìn)行再次配準(zhǔn)，使用這種配準(zhǔn)方式可以提高配準(zhǔn)的準(zhǔn)確度。目前，粗配準(zhǔn)的方法主要有標(biāo)簽法、主方向貼合法、中心重合法等，在本文利用中心重合法進(jìn)行點(diǎn)云粗配準(zhǔn)。

1.1中心重合法

中心重合法在點(diǎn)云粗配準(zhǔn)中利用點(diǎn)云A與待配準(zhǔn)點(diǎn)云B的中心重合，從而降低兩組點(diǎn)云數(shù)據(jù)之間的平移差值。

1)計(jì)算點(diǎn)云A與點(diǎn)云B對應(yīng)點(diǎn)集的質(zhì)心

(1)

(2)

(3)

(4)

對兩組點(diǎn)云數(shù)據(jù)進(jìn)行中心重合，可縮小點(diǎn)云之間的平移錯(cuò)位，為下一步的精配準(zhǔn)提供初始位置，提高點(diǎn)云的配準(zhǔn)精度。

1.2精配準(zhǔn)

在三維點(diǎn)云數(shù)據(jù)配準(zhǔn)過程中，計(jì)算旋轉(zhuǎn)變換矩陣和平移矩陣，這是任何一種算法都不可避免的。在利用粗配準(zhǔn)得到的初始位置，本文選用ICP算法進(jìn)行點(diǎn)云精配準(zhǔn)，并在算法中用單位四元數(shù)算法求解迭代過程中的旋轉(zhuǎn)矩陣。

1.2.1單位四元數(shù)算法

1)求解兩組對應(yīng)點(diǎn)集協(xié)方差

(5)

式中：∑ab兩點(diǎn)集協(xié)方差；N為點(diǎn)云數(shù)目；

2)計(jì)算出反對稱矩陣

(6)

式中：∑ba為∑ab的轉(zhuǎn)置據(jù)矩陣。

3)利用求得的反對稱矩陣A中A23,A31,A12位置上的元素定義一個(gè)新的列向量，Δ=[A23,A31A12]′.

4)用列向量Δ與上面的協(xié)方差∑ab組成一個(gè)新的4階矩陣

(7)

式中：E3為一個(gè)三階的單位矩陣；Δ′為Δ的轉(zhuǎn)置矩陣；tr(∑ab)為協(xié)方差∑ab的跡。

(8)

式中:q0,q1,q2,q3為四元向量的參數(shù)；

(9)

1.2.2ICP算法

ICP算法在點(diǎn)云配準(zhǔn)中廣泛應(yīng)用，特別是在精配準(zhǔn)過程中，在點(diǎn)云配準(zhǔn)中進(jìn)行迭代運(yùn)算，配準(zhǔn)精度高、效果可靠。ICP算法步驟：

1)確定對應(yīng)點(diǎn)對：由點(diǎn)云A中的點(diǎn)在待配準(zhǔn)點(diǎn)云B中搜索出相應(yīng)的最近的點(diǎn)，組成對應(yīng)點(diǎn)對。

2)計(jì)算兩組新點(diǎn)集的質(zhì)心，按照式(1)和式(2)計(jì)算新點(diǎn)集質(zhì)心。

3)計(jì)算目標(biāo)函數(shù)f(Rk,Tk)，以目標(biāo)函數(shù)值最小為原則。

(10)

式中：Rk為最優(yōu)旋轉(zhuǎn)矩陣；Tk為平移矩陣；f(Rk,Tk)為目標(biāo)函數(shù)值。

4)用最優(yōu)旋轉(zhuǎn)矩陣Rk和平移矩陣Tk，使配準(zhǔn)點(diǎn)云B坐標(biāo)變換到Bk+1。

(11)

5)計(jì)算點(diǎn)對的平均距離

(12)

(13)

1.3ICP算法改進(jìn)

ICP算法雖然在配準(zhǔn)中廣泛應(yīng)用，但算法本身配準(zhǔn)的效率低、花費(fèi)時(shí)間長，特別在海量的點(diǎn)云數(shù)據(jù)中更為明顯。因此需要對ICP算法進(jìn)行改進(jìn)，以提高配準(zhǔn)的效率。傳統(tǒng)的ICP算法以歐式距離為衡量標(biāo)準(zhǔn)來判斷最近點(diǎn)，這種方式工作量大、耗費(fèi)時(shí)間多。而利用K-Dtree能快速搜索最近點(diǎn)對，提高點(diǎn)云的配準(zhǔn)效率。

圖1為ICP算法配準(zhǔn)流程，在尋找最近點(diǎn)對時(shí)，利用K-Dtree搜索最近點(diǎn)。其搜索步驟如下:

圖1　ICP算法配準(zhǔn)流程

1)將待查詢的結(jié)點(diǎn)與所確定的分裂維的值進(jìn)行比較，若小于等于分裂維的值進(jìn)入左子樹分子；若大于分裂維的值就進(jìn)入右子樹分支，這種方式循環(huán)到二叉樹的葉子結(jié)點(diǎn)，沿著搜索路徑找到處于和待查詢點(diǎn)同一子空間的最近鄰近似點(diǎn)；

2)對點(diǎn)進(jìn)行“回溯”操作，若在搜索路徑上結(jié)點(diǎn)的其它子空間結(jié)點(diǎn)上有更近點(diǎn)，則跳到子空間結(jié)點(diǎn)上去搜索距離最近點(diǎn)；

3)反復(fù)進(jìn)行步驟①和②到搜素路徑為空結(jié)束搜索。

2實(shí)驗(yàn)驗(yàn)證與分析

本文采用的是斯坦福Bunny點(diǎn)云數(shù)據(jù)，其中稀疏點(diǎn)云分辨率低，點(diǎn)云數(shù)目為112 782；密集點(diǎn)云分辨率高，點(diǎn)云數(shù)目為330 406。兩組點(diǎn)云數(shù)據(jù)都是在不同視點(diǎn)下且存在著一定平移旋轉(zhuǎn)平移。其中圖2為2組點(diǎn)云數(shù)據(jù)的示意圖。

圖2　原始點(diǎn)云數(shù)據(jù)

對圖2中的稀疏以及密集點(diǎn)云按照本文方法分別粗配準(zhǔn)，得圖3所示的點(diǎn)云數(shù)據(jù)粗配準(zhǔn)結(jié)果圖。

圖3　點(diǎn)云數(shù)據(jù)粗配準(zhǔn)結(jié)果

在圖3粗配準(zhǔn)的基礎(chǔ)上，采用ICP對其精確配準(zhǔn)。得圖4所示的點(diǎn)云數(shù)據(jù)精配準(zhǔn)結(jié)果圖。

圖4　點(diǎn)云數(shù)據(jù)ICP精配準(zhǔn)結(jié)果

在圖3粗配準(zhǔn)基礎(chǔ)上，采用改進(jìn)ICP算法對其精配準(zhǔn)，得圖5所示點(diǎn)云數(shù)據(jù)精配準(zhǔn)結(jié)果圖。

圖5　改進(jìn)的ICP配準(zhǔn)結(jié)果

對實(shí)驗(yàn)結(jié)果分析得到傳統(tǒng)ICP算法迭代次數(shù)與配準(zhǔn)誤差關(guān)系圖，見圖6；K-D樹優(yōu)化ICP算法迭代次數(shù)與配準(zhǔn)誤差關(guān)系圖，見圖7。

圖6　迭代次數(shù)，誤差關(guān)系

圖7　改進(jìn)后迭代次數(shù)，誤差關(guān)系

從實(shí)驗(yàn)結(jié)果圖6和圖7上可以發(fā)現(xiàn)：

1)在配準(zhǔn)誤差上：前5次的迭代中誤差雖然隨著迭代次數(shù)的增加而逐漸減少，而在圖7中前5次的迭代中誤差減小的幅度大而快。傳統(tǒng)的ICP算法誤差減小的幅度要比改進(jìn)的ICP算法小的多，即在誤差上改進(jìn)的ICP算法要優(yōu)于傳統(tǒng)的ICP；

2)在迭代次數(shù)上：在相同的15次迭代中傳統(tǒng)的ICP算法要得到與改進(jìn)后ICP算法相同的配準(zhǔn)效果，要進(jìn)行更多的迭代次數(shù)。即在迭代次數(shù)上改進(jìn)的ICP算法要優(yōu)于傳統(tǒng)的ICP算法；

3)對相同兩組點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)，在配準(zhǔn)的時(shí)間上，傳統(tǒng)的ICP算法迭代的次數(shù)要多，誤差減小的幅度要慢，改進(jìn)的ICP在配準(zhǔn)的過程中耗費(fèi)的時(shí)間比傳統(tǒng)算法小的多。傳統(tǒng)ICP算法對點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)耗費(fèi)時(shí)間為36s，而改進(jìn)后算法對點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)耗費(fèi)的時(shí)間僅為1.3s，花費(fèi)的時(shí)間要比傳統(tǒng)的ICP算法低的多。

表1說明對不同數(shù)目點(diǎn)云利用k-dtree改進(jìn)的ICP算法進(jìn)行配準(zhǔn)比用傳統(tǒng)的ICP算法配準(zhǔn)耗費(fèi)的時(shí)間少的多，并在點(diǎn)云數(shù)量較多時(shí)表現(xiàn)更為明顯，則可得在點(diǎn)云配準(zhǔn)中利用k-dtree改進(jìn)的ICP算法能提高配準(zhǔn)效率。

表1　點(diǎn)云實(shí)驗(yàn)配準(zhǔn)時(shí)間統(tǒng)計(jì)

3結(jié)束語

本文采用中心重合法實(shí)現(xiàn)點(diǎn)云數(shù)據(jù)的粗配準(zhǔn)，然后利用k-d tree快速搜索最近點(diǎn)對改進(jìn)傳統(tǒng)ICP方法，完成三維點(diǎn)云數(shù)據(jù)精配準(zhǔn)。該方法提高三維點(diǎn)云數(shù)據(jù)配準(zhǔn)的精度和速度，特別在海量點(diǎn)云數(shù)據(jù)和不同密度三維點(diǎn)云數(shù)據(jù)應(yīng)用中更為明顯，而且具有較好的穩(wěn)定性。但是對于大型點(diǎn)云數(shù)據(jù)，該方法效率還是不能完全滿足應(yīng)用需求，在搜索策略需進(jìn)一步的改善。

參考文獻(xiàn)：

[1]張蕾,冀治航,普杰信.約束改進(jìn)的ICP點(diǎn)云配準(zhǔn)方法[J].計(jì)算機(jī)工程與應(yīng)用,2012(18):197-200.

[2]馬婷.點(diǎn)云數(shù)據(jù)的顯示及配準(zhǔn)[D].長春:吉林大學(xué),2007.

[3]王蕊,李俊山,劉玲霞.基于幾何特征的點(diǎn)云配準(zhǔn)算法[J].華東理工大學(xué)學(xué)報(bào)(自然科學(xué)版),2009,(05):768-773.

[4]朱寧寧.基于共面條件的點(diǎn)云配準(zhǔn)旋轉(zhuǎn)角參數(shù)求解方法[J].測繪與空間地理信息，2015，38(5)：202-205.

[5]吳祿慎,孔維敬.基于特征點(diǎn)的改進(jìn)ICP三維點(diǎn)云配準(zhǔn)技術(shù)[J].南昌大學(xué)學(xué)報(bào)(工科版),2008(3):294-297.

[6]鄭德華.ICP算法及其在建筑物掃描點(diǎn)云數(shù)據(jù)配準(zhǔn)中的應(yīng)用[J].測繪科學(xué),2007,32(2):31-32.

[7]吳靜,靳奉祥,王健.基于三維激光掃描數(shù)據(jù)的建筑物三維建模[J].測繪工程,2007,16(5):57-60.

[8]賀磊,余春平,李廣云.激光掃描數(shù)據(jù)的多站配準(zhǔn)方法[J].測繪科學(xué)技術(shù)學(xué)報(bào),2008,(06):410-413.

[責(zé)任編輯：李銘娜]

ICP three-dimensional point cloud registration based on K-D tree optimizationLIU Jiang1,ZHANG Xu2,ZHU Jiwen1

(1.School of Surveying and Mapping Engineering,Heilongjiang Institute of Technology,Harbin 150050，China；2.School of Mapping and City Space Information,Beijing University of Civil Engineering and Architecture,Beijing 100044,China)

Abstract：In order to improve the precision and speed of the three-dimensional point cloud registration,it presents a three dimensional point cloud registration based on ICP K-D tree optimization in this paper.First of all,the centre superposition method is adopted to realize the point cloud coarse registration,and then improve the traditional ICP where the K-D tree is used to quickly search the closest pair of points to enhance the speed of the point cloud registration.Finally the three dimensional point cloud coarse registration is completed precisely.The method overcomes the defects of the traditional ICP algorithm using Euclidean distance to determine the closest pair of points which is time-consuming and plains lots of work.On the basis of this method,the experiment can be verified through different density Bunny Stanford point cloud data.The result shows that using K-d tree optimization of ICP algorithm,the precision,speed and stability of the point cloud registration are improved when the centre superposition method is adopted to realize the three dimensional point cloud coarse registration.

Key words：point cloud registration；K-D tree；centre superposition；precision；stability

中圖分類號：P208

文獻(xiàn)標(biāo)識碼：A

文章編號：1006-7949(2016)06-0015-04

作者簡介：劉江(1980-)，男，講師，碩士.

基金項(xiàng)目：黑龍江省自然科學(xué)基金資助項(xiàng)目(D201413)

收稿日期：2015-06-26,修回日期：2015-07-21