一種基于幾何特征由粗到細(xì)點(diǎn)云配準(zhǔn)算法

胡加濤， 吳曉紅*， 何小海， 王正勇， 龔 劍

(1.四川大學(xué)電子信息學(xué)院，成都 610065; 2.成都西圖科技有限公司，成都 610021)

點(diǎn)云配準(zhǔn)是三維計算機(jī)視覺中的一個基本問題。通常采用激光掃描儀獲取點(diǎn)云，由于光在物體表面不能穿透，物體表面的信息往往需要多視角、多分辨率掃描獲得。給定不同坐標(biāo)系下的幾組點(diǎn)云，配準(zhǔn)的目的是找到將它們對齊到最佳公共坐標(biāo)系的變換。通常需要對多個視角的點(diǎn)云進(jìn)行配準(zhǔn)才能獲取完整的信息，其中最基本的是兩個視角點(diǎn)云配準(zhǔn)。點(diǎn)云配準(zhǔn)在許多視覺應(yīng)用中發(fā)揮著重要作用，例如三維重建[1]、機(jī)器人視覺[2]、醫(yī)學(xué)研究[3]、文物數(shù)字化[4]等領(lǐng)域。

點(diǎn)云配準(zhǔn)最經(jīng)典的算法是由Besl等[5]提出的迭代最近點(diǎn)( iterative closest points, ICP)算法，該算法簡單易理解，但配準(zhǔn)效果受初始位姿影響較大，每次迭代都要搜索最近點(diǎn)，且沒有包含局部特征信息。因此，中外學(xué)者提出許多改進(jìn)的方法。為了避免局部最小值，Yang等[6]將ICP算法和分支定界(branch and bound, BnB)算法相結(jié)合，有效地搜索旋轉(zhuǎn)和平移空間，保證全局最優(yōu)，但計算量大，難以用于數(shù)據(jù)量大的場合；針對ICP算法迭代速度慢的問題，Han等[7]將分層搜索方案與基于八叉樹的ICP算法相結(jié)合，有效提高配準(zhǔn)效率；Yang等[8]基于統(tǒng)計特性提出局部特征統(tǒng)計描述符(local feature statistics histogra，LFSH)，通過編碼局部深度、點(diǎn)密度和法線之間的角度的統(tǒng)計特性，形成局部形狀幾何的全面描述，獲得更穩(wěn)健的匹配點(diǎn)對；Lin等[9]等將點(diǎn)云轉(zhuǎn)換成2D方位角圖像，然后使用基于2D特征的匹配方法(speeded up robust features，SURF)來尋找兩幅圖像之間的匹配像素對，該方法對兩場景的相似性要求較高，不適合視角變化大的場景。上述算法大多忽略了點(diǎn)云的幾何特征。

針對ICP算法對初始位置敏感且收斂速度慢的問題，提出基于幾何特征由粗到細(xì)點(diǎn)云配準(zhǔn)算法。粗配準(zhǔn)階段，采用基于曲率特征4點(diǎn)匹配，為細(xì)配準(zhǔn)提供良好初始位姿；細(xì)配準(zhǔn)利用法向量夾角啟發(fā)最近點(diǎn)的搜索，加快ICP迭代速度。

1 基于曲率特征4點(diǎn)匹配的粗配準(zhǔn)

鄰域曲率作為魯棒且計算快速的局部特征描述子，具有剛性變換不變特性。利用曲率特征進(jìn)行粗配準(zhǔn)。首先尋找點(diǎn)云的最佳投影平面，將點(diǎn)云投影到平面上，然后在平面上提取4個輪廓點(diǎn)，再根據(jù)投影變換尋找輪廓點(diǎn)的三維對應(yīng)點(diǎn)，計算三維點(diǎn)鄰域內(nèi)的各點(diǎn)的曲率，根據(jù)曲率變化率的最值尋找特征點(diǎn)，匹配特征點(diǎn)得到初始變換參數(shù)。

1.1 曲率特征提取

表面曲率是描述三維曲面局部變化的一個重要度量，通過二次曲面擬合點(diǎn)p的k個最近鄰點(diǎn)來估計點(diǎn)p的曲率，將曲面定義為

f(x,y)=a0x2+a1y2+a2xy+a3x+a4y+a5

(1)

利用最小二乘法來估計曲面的參數(shù)a0、a1、a2、a3、a4、a5，然后利用差分幾何計算高斯曲率K和平均曲率H。

(2)

(3)

式中：E=rxrx；F=rxry；L=rxxn；M=rxyn；E=rxrx；G=ryry；rx、ry、rxx、ryy、rxy是二次曲面的偏導(dǎo)數(shù)。

主曲率k1和k2通過高斯曲率和平均曲率計算得到。

(4)

式(4)中：k1是最大主曲率；k2是最小主曲率。

Chen等[10]研究表明，特征點(diǎn)是鄰域中曲率形狀變化最大的點(diǎn)。根據(jù)這一特點(diǎn)，利用式(5)計算局部曲率變化率C(p)，即

(5)

對于鄰域中某點(diǎn)pi若滿足：

C(pi)>max[C(pi1),C(pi2),…，C(pik)]

(6)

則點(diǎn)pi為鄰域凸點(diǎn)；若滿足：

C(pi)

(7)

則點(diǎn)pi為鄰域凹點(diǎn)。

式中：C(pi1)、C(pi2)、C(pik)為pi點(diǎn)局部曲率形狀描述C(p)值。

1.2 最大距離的輪廓特征點(diǎn)提取

通常，點(diǎn)云數(shù)據(jù)量很大，若直接計算所有點(diǎn)的曲率形狀特征，效率低。一般有兩種方法來處理源數(shù)據(jù)：一種是通過創(chuàng)建體素網(wǎng)格，用網(wǎng)格的重心點(diǎn)來代表這些點(diǎn)，并且用所有的重心點(diǎn)作為特征點(diǎn)；另一種辦法是選取點(diǎn)云最有代表性的特征點(diǎn)。4點(diǎn)全等集算法[11](4-points congruent sets，4PCS)利用共面4點(diǎn)的仿射不變性進(jìn)行點(diǎn)云配準(zhǔn)，受此啟發(fā)，通過選取不在一個平面的4點(diǎn)來粗略定位點(diǎn)云在空間中的大小和輪廓。在此基礎(chǔ)上提出基于投影的最大距離輪廓特征點(diǎn)提取算法。

(8)

在平面上選取的4個點(diǎn)如下：

第1個點(diǎn)f1為距離中心點(diǎn)最遠(yuǎn)的點(diǎn)，為

(9)

第2個點(diǎn)f2為距離f1點(diǎn)和中心點(diǎn)最遠(yuǎn)的點(diǎn)，為

(10)

點(diǎn)f1到中心點(diǎn)c的直線和點(diǎn)f2到中心點(diǎn)c的直線形成了以中心點(diǎn)c為頂點(diǎn)的∠f1cf2。

第3個點(diǎn)f3在∠f1cf2的角平分線上，并為距離中心點(diǎn)c最遠(yuǎn)的點(diǎn)。

第4個點(diǎn)f4在以f3為定點(diǎn)的射線f3c上，并為距離中心點(diǎn)c最遠(yuǎn)的點(diǎn)。

找到這4個點(diǎn)之后，將這4點(diǎn)映射到三維空間中，分別為p1、p2、p3和p4，pi∈S，然后在這4個點(diǎn)周圍選取k個最近鄰點(diǎn)，根據(jù)式(5)計算k個點(diǎn)的曲率形狀變化率C(p)，根據(jù)式(6)和式(7)尋找這4個點(diǎn)鄰域內(nèi)的特征凹點(diǎn)或特征凸點(diǎn)作為特征點(diǎn)，將4個特征點(diǎn)記為pf1、pf2、pf3和pf4，其中pfi∈S′∈S,點(diǎn)之間的距離為d1,2、d1,3、d1,4、d2,3、d2,4、d3,4，如圖1所示，其中di,j表示點(diǎn)pfi與pfj之間的距離。

圖1 特征點(diǎn)及特征點(diǎn)之間的距離Fig.1 Feature point and distance between feature points

1.3 基于相似度的4點(diǎn)粗配準(zhǔn)

根據(jù)式(11)計算兩個4點(diǎn)集匹配的相似度。

(11)

參考文獻(xiàn)[12]，初始變換矩陣Tr可以通過公式計算：

Tr=[P(c)p(t)T][P(t)p(t)T]-1

(12)

式(12)中：P(c)和P(t)為源點(diǎn)云和目標(biāo)點(diǎn)云的4個輪廓特征點(diǎn)組成的特征矩陣。其中P(τ)定義如下：

(13)

當(dāng)τ=c時，為源點(diǎn)云特征點(diǎn)矩陣；當(dāng)τ=t時，為目標(biāo)點(diǎn)云特征點(diǎn)矩陣，p(c)和P(t)的每一列中的(x,y,z)三維坐標(biāo)點(diǎn)都是一對匹配的輪廓特征點(diǎn)。

2 法向量啟發(fā)搜索的點(diǎn)云細(xì)配準(zhǔn)

粗配準(zhǔn)后源點(diǎn)云和目標(biāo)點(diǎn)云部分重合，此時細(xì)配準(zhǔn)進(jìn)一步減少兩點(diǎn)云的均方根誤差。細(xì)配準(zhǔn)采用法向量作為點(diǎn)云特征匹配的度量。通過法向量夾角來啟發(fā)搜索以改善點(diǎn)云配準(zhǔn)速度，同時避免陷入局部最小值。

2.1 傳統(tǒng)ICP算法

(14)

利用奇異值分解(singular value decomposition,SVD)或者非線性優(yōu)化計算得到剛性變換矩陣，并更新目標(biāo)點(diǎn)云的位置，多次迭代，直到收斂于給定的閾值時停止。

2.2 本文算法

通過法向量及法向量夾角，使得搜索時利用法向量方向的相似性確定性向一個方向接近最近點(diǎn)，以改善傳統(tǒng)ICP迭代次數(shù)過多的問題。

2.2.1 法向量及法向量夾角

利用點(diǎn)云庫[13](point cloud library，PCL)很容易計算點(diǎn)的法向量。法向量夾角可以衡量物體表面局部區(qū)域變化趨勢，若點(diǎn)pj及其鄰域內(nèi)一點(diǎn)pi處的法向量分別為nj和ni，則法向量夾角為

(15)

若θ≤5°,則認(rèn)為這兩個向量具有相同變化趨勢，為了計算方便，取cos(θi,j)。

2.2.2 ICP啟發(fā)式搜索

傳統(tǒng)ICP 根據(jù)距離，在k鄰域內(nèi)逐個點(diǎn)搜索，有時收斂到錯誤的位置。啟發(fā)式搜索允許根據(jù)法向量夾角的相似性選擇更有效的搜索路徑，使ICP算法收斂更快。粗配準(zhǔn)基礎(chǔ)上，取源點(diǎn)云和目標(biāo)點(diǎn)云最近點(diǎn)作為一組粗略匹配點(diǎn)為(pi,qj)，其中pi∈S,qj∈T。給定剛體變換初值s0=[R0,t0]T，R0為初始旋轉(zhuǎn)矩陣，t0是初始平移矩陣，迭代次數(shù)num=0。此時使用啟發(fā)式搜索進(jìn)一步優(yōu)化R0和t0，如下。

輸入：給定源點(diǎn)云與目標(biāo)點(diǎn)云的一組粗匹配及收斂閾值ε。

輸出：最優(yōu)旋轉(zhuǎn)和平移矩陣。

(1)從源點(diǎn)云S中選擇點(diǎn)pi，并計算鄰域法向量nsi。

(2)從目標(biāo)點(diǎn)云T中選擇pi的粗匹配點(diǎn)qj的k個最近點(diǎn)，并計算這些點(diǎn)的法向量nqj。

(3)將nsi與目標(biāo)點(diǎn)鄰域內(nèi)k個最近點(diǎn)的法向量nqj進(jìn)行匹配，利用式(15)計算pi與qj鄰域各點(diǎn)法向量夾角，并把具有相同變化趨勢的法向量的點(diǎn)對構(gòu)成對應(yīng)點(diǎn)集合m(pi,qj)。

(4)從對應(yīng)點(diǎn)集合中選取法向量夾角最小的一組對應(yīng)點(diǎn)(pi,qx)，用奇異值分解(SVD)計算旋轉(zhuǎn)矩陣Rk+1和平移矩陣tk+1，則sk+1=[Rk+1,tk+1]T，并更新目標(biāo)點(diǎn)云的位置。

(5)計算兩點(diǎn)云的均方根誤差(root mean square error，RMSE)，若RMSEk+1ε，以目標(biāo)點(diǎn)qx替代目標(biāo)點(diǎn)qj繼續(xù)步驟(2)的計算。

(6)若出現(xiàn)RMSEk+1>RMSEk，選取法向量夾角較大的一組點(diǎn)(pi,qx)，繼續(xù)步驟(4)。

(7)若多次出現(xiàn)RMSEk+1>RMSEk，則減小k，繼續(xù)迭代，直到收斂。

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

實(shí)驗(yàn)平臺為Intel(R) Core(TM) i5-7500 CPU@3.4 GHz 64位8 G內(nèi)存的計算機(jī)，軟件平臺為VS2013，結(jié)合PCL點(diǎn)云庫實(shí)現(xiàn)算法。實(shí)驗(yàn)數(shù)據(jù)來源于三維公園開放的三維掃描數(shù)據(jù)，數(shù)據(jù)本身噪點(diǎn)少且模型比較完整，選取其中雕塑、汽車和鞋子點(diǎn)云模型，其中汽車模型在結(jié)構(gòu)上具有對稱性，可以驗(yàn)證算法對對稱結(jié)構(gòu)的魯棒性。由于待配準(zhǔn)兩點(diǎn)云數(shù)據(jù)不是一一對應(yīng)關(guān)系，業(yè)內(nèi)還沒有統(tǒng)一的標(biāo)準(zhǔn)來衡量點(diǎn)云配準(zhǔn)精度，通常用均方根誤差RMSE來衡量精度。在對比算法效率時，記錄各算法在相同收斂閾值條件下，多次迭代所需總的物理運(yùn)行時間。

首先驗(yàn)證初始位置對ICP算法的影響和本文粗配準(zhǔn)結(jié)果，如圖2所示。實(shí)驗(yàn)中ICP算法直接從初始位置開始迭代配準(zhǔn)，由于初始位置較遠(yuǎn)及旋轉(zhuǎn)角度較大，迭代30次后陷入局部最小值，配準(zhǔn)后的結(jié)果圖RMSE誤差最小，但繼續(xù)迭代后，誤差變大且配準(zhǔn)結(jié)果偏差增大。表1中配準(zhǔn)點(diǎn)的數(shù)目是源點(diǎn)云和目標(biāo)點(diǎn)云下采樣后參與配準(zhǔn)的點(diǎn)數(shù)。圖 2(c)所示是本文粗配準(zhǔn)的結(jié)果圖，此時兩點(diǎn)云部分重合，需細(xì)配準(zhǔn)進(jìn)一步配準(zhǔn)以減小RMSE值。

圖2 ICP配準(zhǔn)和本文粗配準(zhǔn)結(jié)果Fig.2 ICP registration and coarse registration results

表1 ICP直接配準(zhǔn)和本文粗配準(zhǔn)結(jié)果

圖2、表1的結(jié)果表明，若兩點(diǎn)云初始位置較遠(yuǎn)且旋轉(zhuǎn)角度較大時，ICP算法需要多次迭代，最終可能陷入局部最小值，而所提出的基于曲率特征4點(diǎn)匹配的粗配準(zhǔn)能將兩點(diǎn)云快速拉近，并且不需要多次迭代計算，為細(xì)配準(zhǔn)提供良好的初始位置。

在粗配準(zhǔn)的基礎(chǔ)上，細(xì)配準(zhǔn)對比算法包括經(jīng)典ICP算法、基于協(xié)方差矩陣改進(jìn)的GICP[14]算法和迭代全局相似點(diǎn)的GH-ICP[15]算法。測試時，先使用理想點(diǎn)云模型進(jìn)行實(shí)驗(yàn)，圖3、圖4是沒有添加噪聲的實(shí)驗(yàn)結(jié)果，然后分別在源點(diǎn)云和目標(biāo)點(diǎn)云中添加高斯噪聲，模擬外部設(shè)備獲取點(diǎn)云時離群點(diǎn)較多或者環(huán)境噪點(diǎn)較大的情形，測試算法對噪聲的魯棒性，結(jié)果如圖5、圖6所示，表2所示是實(shí)驗(yàn)數(shù)據(jù)對比。

圖3 無噪聲時shoe模型的配準(zhǔn)結(jié)果Fig.3 Registration result of shoe model without noise

圖4 無噪聲時對稱car模型的配準(zhǔn)結(jié)果 Fig.4 Registration result of symmetric car model without noise

圖5 添加高斯噪聲后在shoe模型上的結(jié)果Fig.5 Results on the shoe model after adding Gaussian noise

圖6 添加高斯噪聲后在對稱的car模型上的配準(zhǔn)結(jié)果Fig.6 Registration results on a symmetric car model adding gaussian noise

表2 配準(zhǔn)算法結(jié)果

從圖3、圖5可以看出，各算法對于鞋子模型這類非對稱結(jié)構(gòu)具有較好配準(zhǔn)效果，而對于圖4、圖6這種對稱結(jié)構(gòu)來說，配準(zhǔn)效果受到影響，并且有噪聲時，這種差異性表現(xiàn)更明顯。從表2可以看出，經(jīng)典ICP算法往往需要多次迭代，配準(zhǔn)精度不高，且易受噪聲和對稱結(jié)構(gòu)的影響。GICP和GHICP算法迭代次數(shù)相比ICP有所減少，4種算法中GHICP算法在配準(zhǔn)精度表現(xiàn)最好，但需要復(fù)雜的計算，耗時較多。而本文提出的基于法向量和法向量夾角的點(diǎn)云精細(xì)配準(zhǔn)算法通過法向量夾角啟發(fā)搜索匹配點(diǎn)，加速了點(diǎn)云的收斂，在保證配準(zhǔn)精度的同時，減少迭代次數(shù)，耗時少，且對于對稱結(jié)構(gòu)具有良好的魯棒性，并且對具有噪聲的點(diǎn)云也有較好的配準(zhǔn)效果。

4 結(jié)論

粗配準(zhǔn)通過投影和映射尋找三維空間各4個輪廓特征點(diǎn)，利用特征點(diǎn)的曲率和特征點(diǎn)之間的距離尋找兩點(diǎn)云特征點(diǎn)的最佳匹配，最終得到初始變換參數(shù)；細(xì)配準(zhǔn)階段，以法向量為特征尋找最近點(diǎn)，通過法向量夾角啟發(fā)最近點(diǎn)搜索，快速尋找到最優(yōu)的匹配點(diǎn)，加快ICP的迭代速度。最后，選取了具有代表性的點(diǎn)云模型進(jìn)行了實(shí)驗(yàn)，得出如下結(jié)論。

(1)提出的基于曲率特征4點(diǎn)匹配的粗配準(zhǔn)能使距離較遠(yuǎn)、旋轉(zhuǎn)角度較大的兩點(diǎn)云快速靠攏，為細(xì)配準(zhǔn)提供良好的初始位置，避免陷入局部最小值。

(2)在細(xì)配準(zhǔn)中，本文算法對比其他3種算法，在保障配準(zhǔn)精度的同時，在迭代次數(shù)和配準(zhǔn)速度上更具優(yōu)勢且魯棒性強(qiáng)，與傳統(tǒng)ICP算法相比配準(zhǔn)誤差減少59.15%，迭代耗時減少46.01%。

今后考慮有效融合物體表面的顏色紋理特征，提高點(diǎn)特征匹配的精確性，從而提升搜索匹配點(diǎn)的正確率，最終提高配準(zhǔn)的速度。