一種基于幾何特征由粗到細點云配準算法

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

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

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

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

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

1 基于曲率特征4點匹配的粗配準

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

1.1 曲率特征提取

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

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

(1)

利用最小二乘法來估計曲面的參數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是二次曲面的偏導數。

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

(4)

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

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

(5)

對于鄰域中某點pi若滿足：

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

(6)

則點pi為鄰域凸點；若滿足：

C(pi)

(7)

則點pi為鄰域凹點。

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

1.2 最大距離的輪廓特征點提取

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

(8)

在平面上選取的4個點如下：

第1個點f1為距離中心點最遠的點，為

(9)

第2個點f2為距離f1點和中心點最遠的點，為

(10)

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

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

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

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

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

1.3 基于相似度的4點粗配準

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

(11)

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

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

(12)

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

(13)

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

2 法向量啟發搜索的點云細配準

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

2.1 傳統ICP算法

(14)

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

2.2 本文算法

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

2.2.1 法向量及法向量夾角

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

(15)

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

2.2.2 ICP啟發式搜索

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

輸入：給定源點云與目標點云的一組粗匹配及收斂閾值ε。

輸出：最優旋轉和平移矩陣。

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

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

(3)將nsi與目標點鄰域內k個最近點的法向量nqj進行匹配，利用式(15)計算pi與qj鄰域各點法向量夾角，并把具有相同變化趨勢的法向量的點對構成對應點集合m(pi,qj)。

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

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

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

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

3 實驗及結果分析

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

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

圖2 ICP配準和本文粗配準結果Fig.2 ICP registration and coarse registration results

表1 ICP直接配準和本文粗配準結果

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

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

圖3 無噪聲時shoe模型的配準結果Fig.3 Registration result of shoe model without noise

圖4 無噪聲時對稱car模型的配準結果 Fig.4 Registration result of symmetric car model without noise

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

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

表2 配準算法結果

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

4 結論

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

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

(2)在細配準中，本文算法對比其他3種算法，在保障配準精度的同時，在迭代次數和配準速度上更具優勢且魯棒性強，與傳統ICP算法相比配準誤差減少59.15%，迭代耗時減少46.01%。

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