利用特征點采樣一致性改進ICP算法點云配準方法

宋成航 李晉儒 劉冠杰

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

0 引言

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

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

1 算法介紹

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

圖1 本文算法流程圖

2 點云粗配準

2.1 提取特征點

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

點云數據任意一點Pi的法線估計近似于估計表面的一個相切面法線,即最小二次擬合平面,該平面的法向量為對應點處的法向量。為了保證該點處切平面為最下二乘擬合平面,分析一個協方差矩陣的特征矢量和特征值,對應的協方差矩陣C如下:

(1)

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

圖2 特征區域與非特征區域法向量夾角

(2)

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

根據點Pi與其鄰近點法向量的夾角來提取特征點,選取適當的閾值ε,當fi>ε時,點Pi處彎曲程度較大,故Pi為特征點;當fi<ε時,點Pi處較為平坦,故Pi為非特征點。

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

ni·nj<0,(i≠j)

(3)

式(3)中,ni為源點云法向量,nj為目標點云法向量。

2.2 FPFH特征描述

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

圖3 點Pq的直方圖鄰域特征影響范圍

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

圖4 計算特征點Pq的FPHF領域特征影響范圍

FPFH具體計算步驟如下:

(1)計算特征點Pq和鄰域點之間的特征元素,構建簡化的點特征直方圖(Simple Point Feature Histograms，SPFH);

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

(4)

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

2.3 SAC-IA算法粗配準

在點云粗配準過程中,為了滿足配準的前提條件,先利用單位四元素法計算點云配準初值,將源點云P的所有點Pi轉換到目標點云Q所在的坐標系下,即

(5)

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

當點云初始位置偏差較大時,配準的過程中容易陷入局部最優。為了解決這一問題,通過SAC-IA算法對點云進行初始配準,實現2片點云初始坐標變換,同時根據點云的法向量和FPFH特征描述提高變換矩陣的估計質量和配準的精度,其具體算法步驟如下:

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

(2)對應點查找:從目標點云Q中查找與點采樣點相似的FPFH特征的一個或多個點,并將這些相似點作為在點云Q中采樣點的對應點。

(6)

其中,t1為設定值;li為變換后第i組對應點的距離差。該算法較結果依賴參數的設定,例如初始配準時最小采樣的距離、計算法向量和建立FPFH鄰域的大小。根據對應點變換選取最優解,使點云配準距離誤差達到最小,獲得最終剛體變換矩陣,完成初始配準。

3 改進ICP算法

經過SAC-IA算法粗配準后,源點云和目標點云已經大致的重合在一起,但配準的精度較低,為了進一步提高配準的精度,在此基礎上采用改進的ICP算法進行精確配準。本文在傳統ICP算法精確配準的基礎上使用K維樹鄰近搜索算法進行優化,加速查找對應點對,有效地提高了計算效率。

K維樹[16]是一種分割K維數據空間的數據結構,用來表示K維空間中的點集合。傳統ICP算法在重復迭代搜索最近點過程中消耗大量時間,使計算效率降低。所以使用K維樹進行搜索對應點,可以有效地減少計算復雜度,實現鄰域關系的快速查找。

ICP算法原理將待配準點云轉換到目標點云所在的坐標系下,根據幾何關系計算坐標變換參數R(旋轉矩陣)和T(平移向量),使得2片點云數據變換后的距離最小。當待配準點云與目標點云經過不斷旋轉與平移,從而達到目標函數最小,即滿足最小二乘定理時,那么配準效果達到最優[17]。

經過粗配準坐標變換的源點云P′和目標點云Q作為輸入點云進行ICP算法精配準,其具體算法步驟如下:

(1)將初始配準后的點云P′(經過坐標變換后的待配準點云)和目標點云Q,作為精配準的初始點集;

(2)對待配準點云P′所有點Pi,在目標點云Q中尋找距離最近的對應點qi,作為該點在目標點云中的對應點,組成對應點對;

(3)計算旋轉矩陣R和平移向量T,使對應點對之間的均方誤差dk最小,其中:

(7)

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

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

4 實驗結果與分析

為了驗證上述點云配準方法的有效性,本文實驗平臺為:處理器i3-4005U CPU@1.70GHz,內存8GB的Win7系統,并使用Visual C++結合PCL進行編程。采用斯坦福大學開放三維點云數據庫中不同視角下的Bunny點云數量分別為28 869、29 491的Dragon點云數量分別為28 869、29 491進行驗證。

4.1 特征點提取結果

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

圖5 特征點提取結果對比

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

4.2 點云配準分析

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

圖6 點云粗配準結果

粗配準之后,通過K維樹加速對應點搜索,計算出剛體變換矩陣,完成精確配準。為了保證本文方法的有效性,文獻[13]與本文算法配準結果對比如圖7所示。

圖7 點云配準結果

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

為了驗證本文算法在收斂速度和配準精度方面的有效性,與傳統ICP和文獻[13]配準方法進行對比,結果如表1、表2和表3所示。

表1 SAC-IA算法配準結果

表3 本文算法配準與文獻[13]方法對比結果

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

經實驗證明,相比傳統ICP算法和文獻[13]配準方法,本文算法在收斂速度和配準精度方面得到很大的改善,驗證了該算法的可靠性和有效性。

5 結束語

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