由粗到細(xì)的點(diǎn)云配準(zhǔn)算法*

趙夫群

(1.咸陽(yáng)師范學(xué)院 教育科學(xué)學(xué)院，陜西 咸陽(yáng) 712000; 2.西北大學(xué) 信息科學(xué)與技術(shù)學(xué)院，陜西 西安710127)

0 引 言

點(diǎn)云配準(zhǔn)就是為兩個(gè)點(diǎn)集求解最優(yōu)空間變換，使其能夠達(dá)到最佳配準(zhǔn)的過程。由于多個(gè)點(diǎn)云的配準(zhǔn)可以通過兩個(gè)點(diǎn)云的配準(zhǔn)來(lái)實(shí)現(xiàn)，因此通常所說的點(diǎn)云配準(zhǔn)都是指兩個(gè)點(diǎn)云的配準(zhǔn)。點(diǎn)云配準(zhǔn)已經(jīng)在3D重建、醫(yī)學(xué)研究、目標(biāo)識(shí)別以及顱面復(fù)原等領(lǐng)域[1～6]得到了廣泛的應(yīng)用。

目前，應(yīng)用最為廣泛的點(diǎn)云配準(zhǔn)算法是由Besl P J等人[7]提出的迭代最近點(diǎn) (iterative closest point,ICP) 算法，該算法簡(jiǎn)單，易于理解和實(shí)現(xiàn)，但對(duì)兩個(gè)點(diǎn)云的初始位置要求較高，且要求兩個(gè)點(diǎn)云具有包含關(guān)系。因此，國(guó)內(nèi)外學(xué)者提出了很多改進(jìn)的ICP算法: Zhu J等人[8]提出了一種基于雙向距離的仿射ICP算法，提高了算法的配準(zhǔn)精度和抗噪性；Han J等人[9]提出了一種基于分層搜索方案、預(yù)警機(jī)制和啟發(fā)式逃生方案的加強(qiáng)ICP算法，能夠快速準(zhǔn)確地實(shí)現(xiàn)點(diǎn)云配準(zhǔn)； Du S等人[10]提出了概率ICP算法，提高了算法的收斂速度和抗噪性； Du S等人[11]提出了尺度迭代最近點(diǎn)(scaling iterative closest point,SICP)算法，解決了點(diǎn)云的尺度配準(zhǔn)問題； Zhao L等人[12]提出了皺紋—感知點(diǎn)云配準(zhǔn)方法，實(shí)現(xiàn)了大尺度的點(diǎn)云配準(zhǔn)；周文振等人[13]提出了一種基于K—means聚類的改進(jìn)ICP算法，實(shí)現(xiàn)了精確的室內(nèi)地圖配準(zhǔn)。但上述算法未解決算法收斂區(qū)間和精度之間的矛盾。

為此，本文提出一種由粗到細(xì)的點(diǎn)云配準(zhǔn)算法。在粗配準(zhǔn)階段，采用尺度參數(shù)可變的點(diǎn)云配準(zhǔn)算法，可以解決點(diǎn)云配準(zhǔn)中收斂區(qū)間與配準(zhǔn)精度之間的矛盾。在細(xì)配準(zhǔn)階段，通過設(shè)置旋轉(zhuǎn)角約束和動(dòng)態(tài)迭代系數(shù)來(lái)改進(jìn)ICP算法，可以解決由旋轉(zhuǎn)角變化過大引起的配準(zhǔn)效果不佳的問題，并提高算法的迭代收斂速度。

1 粗配準(zhǔn)

在粗配準(zhǔn)階段，首先采用大尺度參數(shù)測(cè)度函數(shù)作為配準(zhǔn)測(cè)度函數(shù)，并逐漸減小尺度參數(shù)，使得算法在結(jié)束時(shí)能夠獲得足夠精確的配準(zhǔn)參數(shù)。

1.1 測(cè)度函數(shù)

E(Tr)=∑σ(d(Tr(mi),dj)),

i=1,…,NM,j=1,…,ND

(1)

式中mi和dj分別為點(diǎn)云M和D中的三維坐標(biāo)點(diǎn)，Tr(mi)為對(duì)點(diǎn)mi進(jìn)行空間變換，d(Tr(mi),dj)為Tr(mi)和dj之間的歐氏距離。函數(shù)σ定義為:當(dāng)t=0時(shí)，σ(t)=1;否則，σ(t)=0。

對(duì)式 (1)求最大值max(E(Tr))即可實(shí)現(xiàn)點(diǎn)云配準(zhǔn)。為了式 (1) 尋優(yōu)求解，采用高斯核函數(shù)對(duì)其進(jìn)行平滑，并采用平滑后的函數(shù)作為點(diǎn)云配準(zhǔn)的測(cè)度函數(shù)。該高斯核函數(shù)的定義為ρh(t)=exp(-t2/s2),s為尺度參數(shù)。那么平滑后的計(jì)數(shù)函數(shù)為

i=1,…,NM，j=1,…,ND

(2)

式(2)即為點(diǎn)云配準(zhǔn)的測(cè)度函數(shù)。只要尺度參數(shù)s選取合適，平滑后的計(jì)數(shù)函數(shù)Eh(Tr)即為原計(jì)數(shù)函數(shù)E(Tr)的近似。本文通過BFGS(Broyden-Fletcher-Goldfarb Shanno)擬牛頓優(yōu)化算法搜索其全局極大值，從而確定點(diǎn)云配準(zhǔn)的最優(yōu)參數(shù)，實(shí)現(xiàn)點(diǎn)云粗配準(zhǔn)。

1.2 變尺度配準(zhǔn)算法

主要思想為：在算法的起始階段，采用大尺度參數(shù)測(cè)度函數(shù)作為點(diǎn)云配準(zhǔn)的測(cè)度函數(shù)，以免算法陷入局部極?。浑S著優(yōu)化過程的推進(jìn)，逐漸減小尺度參數(shù)，可在算法結(jié)束時(shí)獲得精確的配準(zhǔn)參數(shù)。具體步驟為：

1)設(shè)置初值：尺度參數(shù)s0，迭代次數(shù)k=1，初始剛體變換q0=(R0,t0)，R0為旋轉(zhuǎn)矩陣，t0為平移矢量；

2)利用BFGS進(jìn)行目標(biāo)函數(shù)的極大值搜索；

3)通過目標(biāo)函數(shù)的極大值位置獲得剛體配準(zhǔn)的參數(shù)Rk和tk；

4)若滿足終止條件則令剛體變換的最終旋轉(zhuǎn)矩陣為R*=Rk，平移矢量為t*=tk，算法結(jié)束;否則,轉(zhuǎn)步驟(5)；

(3)

6)執(zhí)行k=k+1操作，轉(zhuǎn)步驟(2)。

2 細(xì)配準(zhǔn)

細(xì)配準(zhǔn)采用改進(jìn)的ICP算法實(shí)現(xiàn)，在ICP算法中加入角度約束和動(dòng)態(tài)迭代系數(shù)，以解決由旋轉(zhuǎn)角變化過大引起的配準(zhǔn)效果不佳的問題，并提高算法的收斂速度。

2.1 ICP算法

s.t.RTR=In,det(R)=1

(4)

式中R為旋轉(zhuǎn)矩陣;t為平移矩陣。

ICP算法基本步驟如下：

1)根據(jù)第k步的已知?jiǎng)傮w變換Rk和tk，將點(diǎn)云M進(jìn)行Rkmi+tk變換，并建立兩個(gè)點(diǎn)云M和D間的相關(guān)性ck+1(i)，其數(shù)學(xué)描述為

i=1,…,NM

(5)

2)計(jì)算點(diǎn)集M和D的剛體變換，其數(shù)學(xué)描述為

(6)

2.2 改進(jìn)的ICP算法

ICP算法的改進(jìn)過程分2個(gè)步驟:加入旋轉(zhuǎn)角約束和加入動(dòng)態(tài)迭代系數(shù)。

定義旋轉(zhuǎn)矩陣R為R=RxRyRz且

(7)

式中θx,θy,θz為旋轉(zhuǎn)角，令θxb,θyb,θzb為旋轉(zhuǎn)角均值；Δθx,Δθy,Δθz為旋轉(zhuǎn)角偏差；θxb-Δθx,θyb-Δθy,θzb-Δθz為旋轉(zhuǎn)角的下界；θxb+Δθx,θyb+Δθy,θzb+Δθz為旋轉(zhuǎn)角上界。

加入旋轉(zhuǎn)角約束后，M和D的配準(zhǔn)問題可描述為

(8)

在算法中加入動(dòng)態(tài)迭代系數(shù)h。通常，動(dòng)態(tài)迭代系數(shù)h取大于等于0的整數(shù)，當(dāng)h不同時(shí)，算法的收斂速度也不同，根據(jù)不同h值對(duì)應(yīng)的迭代收斂曲線可知,隨著動(dòng)態(tài)迭代系數(shù)h的增大，算法的收斂速度越來(lái)越快，但當(dāng)h增大到一定的程度，收斂曲線會(huì)出現(xiàn)振蕩，當(dāng)h=4時(shí)，算法不再收斂。因此，要為h設(shè)定取值范圍，根據(jù)實(shí)驗(yàn)經(jīng)驗(yàn)，建議其取值范圍在1～4之間。

在ICP算法中加入動(dòng)態(tài)迭代系數(shù)h的步驟如下：

1)計(jì)算剛體變換矢量q=[R|t]T以及qk的相鄰2次迭代的變化量Δqk；

2)由剛體變換矢量Δqk更新基本ICP算法中的Mk共h次，即執(zhí)行Mk=Δqk(Mk)共h次。

加入旋轉(zhuǎn)角約束和動(dòng)態(tài)迭代系數(shù)后，該改進(jìn)ICP算法的具體步驟如下：

1)給定剛體變換初值q0=[R0,t0]T，R0為初始旋轉(zhuǎn)矩陣，t0為初始平移矢量，令M0=R0M+t0，迭代次數(shù)k=0，動(dòng)態(tài)迭代系數(shù)h=0；

2)估計(jì)旋轉(zhuǎn)角θx,θy,θz的邊界，即θx∈[θxb-Δθx,θxb+Δθx],θy∈[θyb-Δθy,θyb+Δθy]和θz∈[θzb-Δθz,θzb+Δθz]；

3)利用式(5)建立點(diǎn)云M和D相關(guān)性ck(i)；

4)利用奇異值分解(singular value decomposition,SVD)的方法計(jì)算旋轉(zhuǎn)矩陣Rk+1和平移矢量tk+1，則qk+1=[Rk+1,tk+1]T;

5)計(jì)算qk+1的相鄰兩次迭代的變化量Δqk+1;

6)利用式(8)計(jì)算Mk+1=Rk+1M+tk+1;

7)判斷均方根(root mean square,RMS)誤差，若RMSk+1-RMSk>ε，則執(zhí)行h=h+1操作;否則執(zhí)行h=0操作RMS定義為

(9)

8)判斷動(dòng)態(tài)迭代系數(shù)h，若h>0，則通過執(zhí)行Mk+1=Δqk+1(Mk+1)共h次來(lái)更新點(diǎn)集Mk+1；

9)若滿足|RMSk+1-RMSk|<ε或k>Stepmax則算法終止;否則,轉(zhuǎn)到步驟(10)，ε為預(yù)設(shè)的閾值，Stepmax為最大迭代次數(shù);

10)令k=k+1，并轉(zhuǎn)步驟(2)。

3 實(shí)驗(yàn)結(jié)果與分析

點(diǎn)云配準(zhǔn)的實(shí)驗(yàn)數(shù)據(jù)源于Stanford 3D Scanning Repository，如圖1所示。

圖1 待配準(zhǔn)的點(diǎn)云

基于本文配準(zhǔn)算法，首先采用變尺度點(diǎn)云配準(zhǔn)算法實(shí)現(xiàn)粗配準(zhǔn)，然后分別采用ICP 算法和提出的改進(jìn)ICP 算法實(shí)現(xiàn)細(xì)配準(zhǔn)、配準(zhǔn)結(jié)果如圖2所示，算法的匹配點(diǎn)數(shù)、匹配誤差、迭代次數(shù)和耗時(shí)等運(yùn)行參數(shù)如表1所示。

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

點(diǎn)云類型點(diǎn)云大小算法配準(zhǔn)點(diǎn)數(shù)配準(zhǔn)誤差迭代次數(shù)耗時(shí)/s兔子20128,20048粗配準(zhǔn)ICP改進(jìn)ICP5566671297940.05960.02890.01353222103.22.11.1龍20920,17418粗配準(zhǔn)ICP改進(jìn)ICP5017611389070.06580.02990.01653524133.62.41.3

在采用改進(jìn)ICP算法的細(xì)配準(zhǔn)過程中，旋轉(zhuǎn)角偏差取值為Δθx=Δθy=Δθz=10°，動(dòng)態(tài)迭代系數(shù)取值為h=3。

從圖3的配準(zhǔn)結(jié)果來(lái)看，變尺度的點(diǎn)云粗配準(zhǔn)算法可以將兩個(gè)點(diǎn)云初步對(duì)齊；ICP算法和改進(jìn)ICP算法可將兩個(gè)點(diǎn)云進(jìn)一步對(duì)齊，但改進(jìn)ICP算法比ICP算法的配準(zhǔn)誤差更小，具有更高的配準(zhǔn)精度。

從表1可見，改進(jìn)ICP算法比ICP算法具有更多的配準(zhǔn)點(diǎn)數(shù)、更低的配準(zhǔn)誤差、更少的迭代次數(shù)和耗時(shí)。與ICP算法相比，改進(jìn)ICP算法配準(zhǔn)精度和速度均提高了約50 %，是一種精度更高、速度更快的點(diǎn)云配準(zhǔn)算法。因此，提出的由粗到細(xì)的點(diǎn)云配準(zhǔn)算法是一種有效的點(diǎn)云配準(zhǔn)算法。

4 結(jié) 論

本文由粗到細(xì)的點(diǎn)云配準(zhǔn)算法解決了算法中收斂區(qū)間與配準(zhǔn)精度之間的矛盾和旋轉(zhuǎn)角變化過大引起的配準(zhǔn)效果不佳的問題，是一種精度高、速度快的點(diǎn)云配準(zhǔn)算法。該算法具有一定的抗噪性，但未考慮尺度各異的點(diǎn)云配準(zhǔn)問題。在今后的研究中，還要綜合考慮點(diǎn)云配準(zhǔn)中更多的影響因素，如噪聲因素、尺度因素、外點(diǎn)的干擾因素等，設(shè)計(jì)更加合理有效的配準(zhǔn)算法，進(jìn)一步提高點(diǎn)云配準(zhǔn)的精度、速度、魯棒性和抗噪性，擴(kuò)大點(diǎn)云配準(zhǔn)技術(shù)的應(yīng)用領(lǐng)域。