利用特征點(diǎn)采樣一致性改進(jìn)ICP算法點(diǎn)云配準(zhǔn)方法

宋成航 李晉儒 劉冠杰

(山東科技大學(xué) 測(cè)繪科學(xué)與工程學(xué)院, 山東 青島 266590)

0 引言

隨著點(diǎn)云數(shù)據(jù)處理技術(shù)和計(jì)算輔助設(shè)計(jì)技術(shù)的不斷進(jìn)步,點(diǎn)云配準(zhǔn)技術(shù)成為計(jì)算機(jī)視覺和圖像處理的重要研究方向。該技術(shù)在三維重建[1]、醫(yī)學(xué)[2]、計(jì)算機(jī)視覺[3]等領(lǐng)域得到廣泛的應(yīng)用。目前,應(yīng)用最廣泛的點(diǎn)云配準(zhǔn)方法是由Besl和Mckay等[4]于1992年提出的最近點(diǎn)迭代算法(Iterative Closest Point,ICP),該算法的基本原理是在兩組點(diǎn)云數(shù)據(jù)之間尋找對(duì)應(yīng)點(diǎn)對(duì)集,通過不斷迭代計(jì)算兩片點(diǎn)云之間的變換矩陣,獲取目標(biāo)點(diǎn)云集和源點(diǎn)云集之間的對(duì)應(yīng)關(guān)系[5]。傳統(tǒng)的ICP算法存在計(jì)算效率低和配準(zhǔn)初始位置要求高,容易陷入局部最優(yōu)等問題。因此,相關(guān)學(xué)者大多采用初始配準(zhǔn)的方法獲得良好的初始位置,并在傳統(tǒng)ICP方法的基礎(chǔ)上進(jìn)行大量的改進(jìn)算法研究。根據(jù)兩片點(diǎn)云表面局部幾何特征查找點(diǎn)對(duì)點(diǎn)的對(duì)應(yīng)關(guān)系,例如旋轉(zhuǎn)圖像(Spin Images,SI)[6]、方向直方圖特征(Signature of Histograms of Orientations,SHOT)[7]、點(diǎn)特征直方圖(Point Feature Histograms,PFH)[8]及改進(jìn)的快速點(diǎn)特征直方圖(Fast Point Feature Histogram,FPFH)[8-9]等,但對(duì)于密度較大的點(diǎn)云,計(jì)算每個(gè)點(diǎn)的特征,很大程度影響點(diǎn)云的配準(zhǔn)效率。文獻(xiàn)[10]通過采樣一致性算法(Sample Consensus Initial Alignment,SAC-IA)對(duì)兩片點(diǎn)云的特征點(diǎn)進(jìn)行配準(zhǔn);文獻(xiàn)[11]基于全局搜索對(duì)應(yīng)點(diǎn)利用四點(diǎn)法(4-Points Congruent Sets,4PCS)算法進(jìn)行點(diǎn)云配準(zhǔn)文獻(xiàn)[12]提出一種正態(tài)分布變換(Normal Distribution Transform,NDT)算法點(diǎn)云配準(zhǔn)方法。這些方法配準(zhǔn)效率高,但是穩(wěn)定性差,配準(zhǔn)精度低。

根據(jù)上述配準(zhǔn)方法存在的問題,本文提出一種基于特征點(diǎn)采樣一致性改進(jìn)ICP算法點(diǎn)云配準(zhǔn)方法。首先將2片點(diǎn)云數(shù)據(jù)體素下采樣后,根據(jù)點(diǎn)云法向量鄰域夾角均值提取特征點(diǎn)并通過FPFH進(jìn)行特征描述,然后采用SAC-IA算法粗配準(zhǔn)計(jì)算出點(diǎn)云的初始坐標(biāo)變換矩陣,使點(diǎn)云獲得良好的初始位置,最后通過K維樹改進(jìn)ICP算法進(jìn)行精確配準(zhǔn),有效地解決傳統(tǒng)ICP算法易陷入局部最優(yōu)和收斂速度慢的問題。

1 算法介紹

針對(duì)傳統(tǒng)ICP算法存在收斂速度慢,容易陷入局部最優(yōu)等問題,提出一種基于特征點(diǎn)SAC-IA改進(jìn)ICP算法點(diǎn)云配準(zhǔn)方法,算法流程如圖1所示。首先對(duì)點(diǎn)云進(jìn)行下采樣,減少點(diǎn)云數(shù)量,其次計(jì)算點(diǎn)云的法向量鄰域夾角和FPFH,用于提取點(diǎn)云數(shù)據(jù)的特征點(diǎn)和特征描述;然后通過SAC-IA粗配準(zhǔn)得到特征點(diǎn)之間的對(duì)應(yīng)關(guān)系,完成2片點(diǎn)云的初始坐標(biāo)變換,進(jìn)而實(shí)現(xiàn)點(diǎn)云的初始配準(zhǔn),使2片點(diǎn)云獲得良好的初始位置;最后采樣用K維樹鄰近搜索算法加速對(duì)應(yīng)點(diǎn)的查找,提高計(jì)算效率,并計(jì)算出對(duì)應(yīng)點(diǎn)的剛體變換矩陣,完成點(diǎn)云精確配準(zhǔn)。

圖1 本文算法流程圖

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

2.1 提取特征點(diǎn)

在初始配準(zhǔn)之前,由于點(diǎn)云數(shù)據(jù)量較大,導(dǎo)致配準(zhǔn)效率降低,因此本文對(duì)原始點(diǎn)云進(jìn)行下采樣,有效減少點(diǎn)云數(shù)量,提高配準(zhǔn)效率。針對(duì)點(diǎn)云粗配準(zhǔn),先利用不同鄰域半徑估計(jì)點(diǎn)云的法向量,并根據(jù)法向量鄰域夾角提取特征點(diǎn),本文使用主成分分析(Principal Component Analysis,PCA)的方法估計(jì)點(diǎn)云的表面法向量。

點(diǎn)云數(shù)據(jù)任意一點(diǎn)Pi的法線估計(jì)近似于估計(jì)表面的一個(gè)相切面法線,即最小二次擬合平面,該平面的法向量為對(duì)應(yīng)點(diǎn)處的法向量。為了保證該點(diǎn)處切平面為最下二乘擬合平面,分析一個(gè)協(xié)方差矩陣的特征矢量和特征值,對(duì)應(yīng)的協(xié)方差矩陣C如下:

(1)

如圖2(a)所示,局部區(qū)域的點(diǎn)云法向量夾角較大,則表明該區(qū)域較為起伏;相反如圖2(b)法向量夾角變化不大則表明該區(qū)域較為平坦[13]。因此,定義點(diǎn)云某一點(diǎn)Pi與其鄰近點(diǎn)法向量夾角的算術(shù)平均值:

圖2 特征區(qū)域與非特征區(qū)域法向量夾角

(2)

式(2)中,θij為點(diǎn)云Pi與鄰近法向量的夾角。

根據(jù)點(diǎn)Pi與其鄰近點(diǎn)法向量的夾角來提取特征點(diǎn),選取適當(dāng)?shù)拈撝郸?當(dāng)fi>ε時(shí),點(diǎn)Pi處彎曲程度較大,故Pi為特征點(diǎn);當(dāng)fi<ε時(shí),點(diǎn)Pi處較為平坦,故Pi為非特征點(diǎn)。

為保證點(diǎn)云表面法向量方向的一致性,提取特征點(diǎn)更準(zhǔn)確,需要對(duì)求得的法向量進(jìn)行方向調(diào)整[14],使之滿足公式(2)。

ni·nj<0,(i≠j)

(3)

式(3)中,ni為源點(diǎn)云法向量,nj為目標(biāo)點(diǎn)云法向量。

2.2 FPFH特征描述

根據(jù)點(diǎn)云法向量夾角特性提取特征點(diǎn)后,通過FPFH對(duì)特征點(diǎn)進(jìn)行描述。FPFH是通過提取特征點(diǎn)的鄰域幾何性進(jìn)行局部特征描述的方法,它通過點(diǎn)云的法向量和鄰域點(diǎn)的表面曲率構(gòu)建一個(gè)特征直方圖[15],從而快速確定特征點(diǎn)之間的對(duì)應(yīng)關(guān)系。實(shí)際上它是由PFH改進(jìn)而來。PFH是根據(jù)對(duì)應(yīng)點(diǎn)對(duì)的歐式距離和法向量的幾何關(guān)系計(jì)算出特征點(diǎn)與源點(diǎn)云之間的對(duì)應(yīng)關(guān)系,并形成一個(gè)多維特征直方圖對(duì)點(diǎn)的K鄰域進(jìn)行幾何描述。其點(diǎn)Pq的PHF鄰域特征影響范圍如圖3所示。

圖3 點(diǎn)Pq的直方圖鄰域特征影響范圍

已知點(diǎn)云P中有n個(gè)點(diǎn),那么它的PFH計(jì)算復(fù)雜度是O(nk2),其中,k是點(diǎn)P云中每個(gè)點(diǎn)計(jì)算特征向量時(shí)的鄰域數(shù)量。由于密集點(diǎn)云的特征直方圖的計(jì)算量過大,為了減少計(jì)算復(fù)雜度,本文提出FPFH算法,計(jì)算特征點(diǎn)Pq在以r為半徑的源點(diǎn)云鄰域內(nèi)的幾何關(guān)系,將計(jì)算復(fù)雜度降到O(nk),仍然保留了PFH的識(shí)別特性,同時(shí)增加了鄰域特征范圍。點(diǎn)的FPHF鄰域特征影響范圍如圖4所示。

圖4 計(jì)算特征點(diǎn)Pq的FPHF領(lǐng)域特征影響范圍

FPFH具體計(jì)算步驟如下:

(1)計(jì)算特征點(diǎn)Pq和鄰域點(diǎn)之間的特征元素,構(gòu)建簡(jiǎn)化的點(diǎn)特征直方圖(Simple Point Feature Histograms，SPFH);

(2)確定每個(gè)點(diǎn)的K近鄰域,利用SPFH值來計(jì)算Pq的FPFH特征,如公式(4)所示:

(4)

式(4)中,fFPFH為快速點(diǎn)特征直方圖描述子,wk為權(quán)重,即表示特征點(diǎn)Pq到鄰域點(diǎn)Pk的距離。

2.3 SAC-IA算法粗配準(zhǔn)

在點(diǎn)云粗配準(zhǔn)過程中,為了滿足配準(zhǔn)的前提條件,先利用單位四元素法計(jì)算點(diǎn)云配準(zhǔn)初值,將源點(diǎn)云P的所有點(diǎn)Pi轉(zhuǎn)換到目標(biāo)點(diǎn)云Q所在的坐標(biāo)系下,即

(5)

式(5)中,Rz為旋轉(zhuǎn)矩陣,XT為平移向量。

當(dāng)點(diǎn)云初始位置偏差較大時(shí),配準(zhǔn)的過程中容易陷入局部最優(yōu)。為了解決這一問題,通過SAC-IA算法對(duì)點(diǎn)云進(jìn)行初始配準(zhǔn),實(shí)現(xiàn)2片點(diǎn)云初始坐標(biāo)變換,同時(shí)根據(jù)點(diǎn)云的法向量和FPFH特征描述提高變換矩陣的估計(jì)質(zhì)量和配準(zhǔn)的精度,其具體算法步驟如下:

(1)選取采樣點(diǎn):從源點(diǎn)云P中選取n個(gè)采樣點(diǎn),為了保證采樣點(diǎn)具有不同的FPFH特征,采樣點(diǎn)之間的距離應(yīng)滿足大于最小距離閾值d。

(2)對(duì)應(yīng)點(diǎn)查找:從目標(biāo)點(diǎn)云Q中查找與點(diǎn)采樣點(diǎn)相似的FPFH特征的一個(gè)或多個(gè)點(diǎn),并將這些相似點(diǎn)作為在點(diǎn)云Q中采樣點(diǎn)的對(duì)應(yīng)點(diǎn)。

(6)

其中,t1為設(shè)定值;li為變換后第i組對(duì)應(yīng)點(diǎn)的距離差。該算法較結(jié)果依賴參數(shù)的設(shè)定,例如初始配準(zhǔn)時(shí)最小采樣的距離、計(jì)算法向量和建立FPFH鄰域的大小。根據(jù)對(duì)應(yīng)點(diǎn)變換選取最優(yōu)解,使點(diǎn)云配準(zhǔn)距離誤差達(dá)到最小,獲得最終剛體變換矩陣,完成初始配準(zhǔn)。

3 改進(jìn)ICP算法

經(jīng)過SAC-IA算法粗配準(zhǔn)后,源點(diǎn)云和目標(biāo)點(diǎn)云已經(jīng)大致的重合在一起,但配準(zhǔn)的精度較低,為了進(jìn)一步提高配準(zhǔn)的精度,在此基礎(chǔ)上采用改進(jìn)的ICP算法進(jìn)行精確配準(zhǔn)。本文在傳統(tǒng)ICP算法精確配準(zhǔn)的基礎(chǔ)上使用K維樹鄰近搜索算法進(jìn)行優(yōu)化,加速查找對(duì)應(yīng)點(diǎn)對(duì),有效地提高了計(jì)算效率。

K維樹[16]是一種分割K維數(shù)據(jù)空間的數(shù)據(jù)結(jié)構(gòu),用來表示K維空間中的點(diǎn)集合。傳統(tǒng)ICP算法在重復(fù)迭代搜索最近點(diǎn)過程中消耗大量時(shí)間,使計(jì)算效率降低。所以使用K維樹進(jìn)行搜索對(duì)應(yīng)點(diǎn),可以有效地減少計(jì)算復(fù)雜度,實(shí)現(xiàn)鄰域關(guān)系的快速查找。

ICP算法原理將待配準(zhǔn)點(diǎn)云轉(zhuǎn)換到目標(biāo)點(diǎn)云所在的坐標(biāo)系下,根據(jù)幾何關(guān)系計(jì)算坐標(biāo)變換參數(shù)R(旋轉(zhuǎn)矩陣)和T(平移向量),使得2片點(diǎn)云數(shù)據(jù)變換后的距離最小。當(dāng)待配準(zhǔn)點(diǎn)云與目標(biāo)點(diǎn)云經(jīng)過不斷旋轉(zhuǎn)與平移,從而達(dá)到目標(biāo)函數(shù)最小,即滿足最小二乘定理時(shí),那么配準(zhǔn)效果達(dá)到最優(yōu)[17]。

經(jīng)過粗配準(zhǔn)坐標(biāo)變換的源點(diǎn)云P′和目標(biāo)點(diǎn)云Q作為輸入點(diǎn)云進(jìn)行ICP算法精配準(zhǔn),其具體算法步驟如下:

(1)將初始配準(zhǔn)后的點(diǎn)云P′(經(jīng)過坐標(biāo)變換后的待配準(zhǔn)點(diǎn)云)和目標(biāo)點(diǎn)云Q,作為精配準(zhǔn)的初始點(diǎn)集;

(2)對(duì)待配準(zhǔn)點(diǎn)云P′所有點(diǎn)Pi,在目標(biāo)點(diǎn)云Q中尋找距離最近的對(duì)應(yīng)點(diǎn)qi,作為該點(diǎn)在目標(biāo)點(diǎn)云中的對(duì)應(yīng)點(diǎn),組成對(duì)應(yīng)點(diǎn)對(duì);

(3)計(jì)算旋轉(zhuǎn)矩陣R和平移向量T,使對(duì)應(yīng)點(diǎn)對(duì)之間的均方誤差dk最小,其中:

(7)

(4)設(shè)定閾值ε(即dk-dk+1<ε)和最大迭代次數(shù)Nmax。

將上一步得到的剛體變換作用于源點(diǎn)云P′,得到新點(diǎn)云P″,并計(jì)算P″和Q的距離誤差,如果兩次迭代的誤差小于閾值ε或者達(dá)到最大迭代次數(shù)大于Nmax,則迭代結(jié)束,不然更新初始配準(zhǔn)的點(diǎn)集為P″和Q,重復(fù)上述步驟,直至滿足收斂條件。

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

為了驗(yàn)證上述點(diǎn)云配準(zhǔn)方法的有效性,本文實(shí)驗(yàn)平臺(tái)為:處理器i3-4005U CPU@1.70GHz,內(nèi)存8GB的Win7系統(tǒng),并使用Visual C++結(jié)合PCL進(jìn)行編程。采用斯坦福大學(xué)開放三維點(diǎn)云數(shù)據(jù)庫中不同視角下的Bunny點(diǎn)云數(shù)量分別為28 869、29 491的Dragon點(diǎn)云數(shù)量分別為28 869、29 491進(jìn)行驗(yàn)證。

4.1 特征點(diǎn)提取結(jié)果

本文采用兩個(gè)組點(diǎn)云數(shù)據(jù)密度較高,所以在保留原始點(diǎn)云特征的基礎(chǔ)上,對(duì)原始點(diǎn)云進(jìn)行下采樣,并利用不同鄰域半徑估計(jì)點(diǎn)云的法向量,通過計(jì)算Bunny和Dragon點(diǎn)云數(shù)據(jù)與鄰域之間的法向量夾角,設(shè)定法向量夾角閾值ε=5,其法向量夾角均值大于閾值即為特征點(diǎn),反之為非特征點(diǎn)。文獻(xiàn)[13]與本文提取特征點(diǎn)的結(jié)果對(duì)比如圖5所示。

圖5 特征點(diǎn)提取結(jié)果對(duì)比

從圖5(a)中可以看出文獻(xiàn)[13]提取的特征點(diǎn)過多且分布密集,不利于后續(xù)的點(diǎn)云配準(zhǔn),然而本文在下采樣后,有效地減少點(diǎn)云的密度,提取的特征點(diǎn)較為稀疏且保留了原始點(diǎn)云的體表特性。

4.2 點(diǎn)云配準(zhǔn)分析

根據(jù)法向量夾角特性提取特征點(diǎn)后,在半徑為0.05 m范圍內(nèi)的所有鄰元素進(jìn)行FPFH特征描述,最后通過SAC-IA算法進(jìn)行粗配準(zhǔn),計(jì)算出初始坐標(biāo)變換矩陣,完成粗配準(zhǔn)，配準(zhǔn)結(jié)果如圖6所示。從圖6中可以看出點(diǎn)云Bunny和點(diǎn)云Dragon已經(jīng)有良好的初始位置,但有些部位還有些配準(zhǔn)偏差。

圖6 點(diǎn)云粗配準(zhǔn)結(jié)果

粗配準(zhǔn)之后,通過K維樹加速對(duì)應(yīng)點(diǎn)搜索,計(jì)算出剛體變換矩陣,完成精確配準(zhǔn)。為了保證本文方法的有效性,文獻(xiàn)[13]與本文算法配準(zhǔn)結(jié)果對(duì)比如圖7所示。

圖7 點(diǎn)云配準(zhǔn)結(jié)果

從圖中可以看出點(diǎn)云Bunny和點(diǎn)云Dragon得到很好的配準(zhǔn)效果,相比初始配準(zhǔn)一些部位出現(xiàn)偏差有了較大的改善。圖7(b)文獻(xiàn)[13] 配準(zhǔn)方法也得到較好的配準(zhǔn)效果,但兔子的耳朵和龍頭部分仍存在一些偏差。

為了驗(yàn)證本文算法在收斂速度和配準(zhǔn)精度方面的有效性,與傳統(tǒng)ICP和文獻(xiàn)[13]配準(zhǔn)方法進(jìn)行對(duì)比,結(jié)果如表1、表2和表3所示。

表1 SAC-IA算法配準(zhǔn)結(jié)果

表3 本文算法配準(zhǔn)與文獻(xiàn)[13]方法對(duì)比結(jié)果

從表中可以看出,本文算法與傳統(tǒng)ICP算法相比，配準(zhǔn)效率提高59%,與文獻(xiàn)[13]配準(zhǔn)方法相比，配準(zhǔn)效率提高22%,提高了收斂速度,同樣保證了配準(zhǔn)的精度。

經(jīng)實(shí)驗(yàn)證明,相比傳統(tǒng)ICP算法和文獻(xiàn)[13]配準(zhǔn)方法,本文算法在收斂速度和配準(zhǔn)精度方面得到很大的改善,驗(yàn)證了該算法的可靠性和有效性。

5 結(jié)束語

針對(duì)傳統(tǒng)ICP算法對(duì)點(diǎn)云初始位置要求高且配準(zhǔn)效率低的問題,本文提出一種基于特征點(diǎn)SAC-IA算法進(jìn)行粗配準(zhǔn),進(jìn)而獲得良好的初始位置,有效地解決傳統(tǒng)ICP算法易陷入局部最優(yōu)化的問題。在粗配準(zhǔn)過程中,通過點(diǎn)云法向量夾角特性提取特征點(diǎn)并使用FPFH進(jìn)行特征描述,保證了兩片對(duì)云之間的對(duì)應(yīng)關(guān)系。經(jīng)粗配準(zhǔn)后的點(diǎn)云作為ICP算法的新輸入點(diǎn)云,同時(shí)基于K維樹ICP算法加速對(duì)應(yīng)點(diǎn)對(duì)搜索,進(jìn)一步精確配準(zhǔn)。實(shí)驗(yàn)結(jié)果表明本文提出的方法大幅度提高配準(zhǔn)的效率和精度。