基于Hausdorff距離改進的ICP算法

鄭曉璐，潘廣貞，楊 劍，楊小青

（中北大學(xué) 計算機與控制工程學(xué)院，山西 太原030051）

0 引 言

為了得到被測物體的完整數(shù)據(jù)模型，需要對多次測量的數(shù)據(jù)進行一個合適的坐標(biāo)變換，將不同視角的點云數(shù)據(jù)變換到統(tǒng)一的坐標(biāo)系內(nèi)進行可視化操作，這就是點云的配準(zhǔn)［1－4］。

Besl等提出了一種基于自由形態(tài)曲面的高層次配準(zhǔn)方法［5］，即 為 經(jīng) 典 的 迭 代 最 近 點 （iterative closest point，ICP）算法。首先根據(jù)一定的準(zhǔn)則確立對應(yīng)點集P與Q，其中對應(yīng)點對的個數(shù)為n。然后通過最小二乘法迭代計算最優(yōu)的坐標(biāo)變換，即旋轉(zhuǎn)矩陣和平移矢量t，使得誤差函數(shù)最小；隨后DAI提出了一種使用曲率特征點和K－D 樹尋找最近點的方法［6］。然而該些算法對點云沒有進行預(yù)處理。之后國內(nèi)外的許多學(xué)者對ICP進行了不同程度的研究和改進。隨著三維掃描設(shè)備精度的提高，數(shù)據(jù)規(guī)模和配準(zhǔn)精度也在提高，經(jīng)典的ICP算法不能滿足實時性的需求，因此本文在對各種ICP算法進行研究后，提出了一種基于Hausdorff距離處理點云的ICP 算法。在獲得大量點云數(shù)據(jù)后，以數(shù)據(jù)點主曲率和Hausdorff距離［7］為依據(jù)，利用正態(tài)分布圖將點云分為關(guān)鍵點和非關(guān)鍵點，關(guān)鍵點充分保留了點云的幾何特征，用來進行配準(zhǔn)，而非關(guān)鍵點作為幾何特征不明顯的點，為提高算法效率不予以配準(zhǔn)。采用K－D 樹［8］加速查找，利用最小二乘迭代計算最優(yōu)的坐標(biāo)變換，完成點云的配準(zhǔn)。數(shù)據(jù)規(guī)模巨大的點云配準(zhǔn)精度問題由此得到解決。

1 點云預(yù)處理

1.1 曲率計算

點云的曲率［9］是描述點云曲面幾何特征的主要數(shù)學(xué)手段，曲率是在連續(xù)且二階可導(dǎo)的曲面上定義的，沒有拓?fù)浣Y(jié)構(gòu)，因此離散的三維點云不能直接計算曲率。在局部坐標(biāo)系內(nèi)擬合一張連續(xù)曲面來計算各點的曲率，常用到的曲率分別有高斯曲率、平均曲率和主曲率。

設(shè)（d）＝du：dv 為曲面S：r＝r（u：v）在P 點處的主方向，沿主方向的主曲率為Kn，則Kn滿足

即Kn是二次方程的根，也就是方程K2n－2 HKn＋K ＝0的根

則曲面S 的高斯曲率為

則曲面S 的平均曲率為

則主曲率

1.2 關(guān)鍵點選取

Hausdorff距離［10］是研究分形空間的一個重要的基本概念，是描述兩組點集之間相似程度的一種度量，它是兩個點集之間距離的一種定義形式：假設(shè)有兩組集合A ＝｛a1，a2，…ap｝，B ＝｛b1，b2，…bq｝，則A，B 兩點集之間Hausdorff距離定義為

其中，d（A，B）和d（B，A）分別表示集合A 到集合B 和集合B 到集合A 的單向Hausdorff距離，分別定義如下

其中， · 表示某種距離范數(shù)。

通過計算點與其鄰域之內(nèi)點之間Hausdorff距離作為依據(jù)，將點集P 分為關(guān)鍵點P′和非關(guān)鍵點P″（P′∪P″＝P且P′∩P″＝Φ）。關(guān)鍵點P′指幾何特征比較明顯，對點云配準(zhǔn)起至關(guān)重要的點。非關(guān)鍵點P″指幾何特征不明顯，配準(zhǔn)時可以忽略的點。對于任意點M 和它的鄰近點N ，其主曲率分別為k1，k2和。點M 和N 之間的曲率差別可以看作是｛k1，k2｝和｛｝之間的差別。我們可以通過一個相對度量H 來衡量Hausdorff距離

當(dāng)分母接近0時，誤差較大。于是，我們引進臨界值ε。當(dāng) ki＋＜ε 時， ki＋應(yīng)該轉(zhuǎn)換為，用來防止誤差較大。

M 點的Hausdorff距離可定義為：HM＝max（H1，H2，H3，…，Hk）。其中：H1，H2，H3，…Hk為點M 與其鄰域內(nèi)k個鄰近點的Hausdorff距離值。HM可以反映出M 點曲率的變化情況，對曲率變化大的區(qū)域保留足夠多的特征點，用來突出模型的曲面特征，而在曲率變化小的區(qū)域保留少量的點，所有保留的點即為關(guān)鍵點。即當(dāng)HM較大時，M 處的幾何特征比較明顯。我們采用HM值的正態(tài)分布圖來獲取點云的關(guān)鍵點。因為正態(tài)分布是自然界中一種常見的分布，存在于自然現(xiàn)象、生產(chǎn)、生活及科學(xué)技術(shù)等領(lǐng)域，我們繪制出點集P 的H 值正態(tài)分布圖，模型大致如圖1所示，橫坐標(biāo)是HM，縱坐標(biāo)是在值為HM的點云個數(shù)n，在正態(tài)分布圖中找到值H0使得小于H0的點 （圖中陰影部分）占點總數(shù)的20%，即為非關(guān)鍵點，剩余80%為關(guān)鍵點 （經(jīng)實驗驗證，28比例的劃分效果比較理想）。關(guān)鍵點非關(guān)鍵點的劃分使我們對點集P 進行了預(yù)處理，找出了集合特征明顯的特征點。然后對幾何特征明顯的點進行配準(zhǔn)，一方面節(jié)省了時間，另一方面使配準(zhǔn)更具有針對性。我們在接下來的ICP配準(zhǔn)時利用的就是這些幾何特征明顯的關(guān)鍵點，用來調(diào)高配準(zhǔn)的精確度。

圖1 點云正態(tài)分布

點集預(yù)處理算法流程說明如下：

步驟1 計算出集合P 中所有點的主曲率，利用式（5）計算。

步驟2 計算每個點及其鄰域點的Hausdorff距離，并取最大值為該點的Hausdorff值，利用式 （8）計算。

步驟3 將集合P 中所有點的Hausdorff值進行統(tǒng)計，并繪制出HM正態(tài)分布圖。

步驟4 根據(jù)上文介紹的分類方法，利用正態(tài)分布圖H 值的分布情況將點云分為關(guān)鍵點P′和非關(guān)鍵點P″。

步驟5 完成點云的預(yù)處理。

2 改進的ICP算法

對兩個三維點集P′和Q 進行ICP配準(zhǔn)，步驟如下：

步驟1 利用k－d tree尋找P′中每一個點在Q 點集中對應(yīng)的最近點。

步驟2 求得上述對應(yīng)點對平均距離最小的剛體變換，即旋轉(zhuǎn)變換向量qR和平移變換向量qT以及誤差。

步驟3 對于P′使用上步求得的變換向量得到新的變換點集。

步驟4 當(dāng)新的變換點集與參考點集Q 滿足式 （9）的目標(biāo)函數(shù)時，停止計算。否則跳轉(zhuǎn)到步驟2 一直迭代，直到達到目標(biāo)函數(shù)的要求。

步驟5 完成點云的配準(zhǔn)。

本文采用了基于Hausdorff距離改進的ICP 算法，目的在于解決ICP算法的高精度要求。利用曲率能表示鄰域形狀變化的特性，引入Hausdorff距離的點集定義，對點云進行預(yù)處理，使得點云的配準(zhǔn)更具有針對性。高精確度的優(yōu)勢使得該算法可應(yīng)用于海量數(shù)據(jù)的逆向工程，本文的算法流程如圖2所示。

圖2 本文算法流程

3 實驗結(jié)果與分析

為了驗證本文算法的正確性和有效性，以經(jīng)典算法作為對比，對三維模型進行了仿真實驗。本文實驗的硬件環(huán)境為Intel Xeon 的處理器，6.00G 內(nèi)存的64位的win7操作系統(tǒng)。軟件環(huán)境為PCL 點云庫、Visual Studio2010、cmake3.0、華碩Xtion Pro Live。實驗數(shù)據(jù)均取自PCL官網(wǎng)提供的pcd格式文件。

3.1 配準(zhǔn)結(jié)果

點云bunny的點數(shù)分別為20480、18566，下面分別給出了經(jīng)典ICP算法和本文ICP 算法的配準(zhǔn)結(jié)果。圖3 （a）為兩片未配準(zhǔn)的點云，淺灰色為源點云，深灰色為目標(biāo)點云。圖3 （b）為經(jīng)典ICP算法的配準(zhǔn)圖。圖3 （c）為本文改進的ICP算法的配準(zhǔn)圖。圖4為經(jīng)過30次迭代計算后的自由剛體變換。對比圖3 （b）和圖3 （c），圖3 （c）中 淺灰部分深灰部分距離小于圖3 （b）對應(yīng)淺灰深灰點云之間的距離，可以得出圖3 （c）的配準(zhǔn)精度得到進一步提高。

圖3 bunny實驗配準(zhǔn)

圖4 bunny最優(yōu)剛體變換

點云robot的點數(shù)分別為20991、19352，下面圖分別給出了經(jīng)典ICP算法和本文ICP算法的配準(zhǔn)結(jié)果。圖5 （a）為兩片未配準(zhǔn)的點云，圖5 （b）為經(jīng)典ICP算法的配準(zhǔn)圖。圖5 （c）為本文改進的ICP算法的配準(zhǔn)圖。圖6為經(jīng)過30次迭代計算后的自由剛體變換。對比圖5 （b）和圖5 （c），圖5 （c）中淺灰部分深灰部分距離小于圖5 （b）對應(yīng)淺灰深灰點云之間的距離，可以得出圖5 （c）的配準(zhǔn)精度有了很大的提高。

3.2 結(jié)果分析

圖5 robot實驗配準(zhǔn)

圖6 robot最優(yōu)剛體變換

實驗的配準(zhǔn)精度我們可以用配準(zhǔn)誤差來衡量。表1列出了經(jīng)典ICP算法和本文改進算法的時間和精度，分別對bunny、robot、horse、dragon進行了實驗。從表數(shù)據(jù)分析來看，bunny在時間上由3.842s縮短到了2.957s，配準(zhǔn)精度由0.3929mm 提高到了0.1604 mm。我們可以得出，在配準(zhǔn)時間幾乎相近的情況下利用基于Hausdorff距離的改進算法能有效的找出均勻的關(guān)鍵點，實驗精度有了很大的提高，取得了令人滿意的效果。

表1 點云配準(zhǔn)比較

由圖7可以看出本文改進算法的配準(zhǔn)誤差明顯低于經(jīng)典算法，并且隨著數(shù)據(jù)量的增大配準(zhǔn)誤差越低。改進算法配準(zhǔn)在精度上有明顯的優(yōu)勢，并且隨著數(shù)據(jù)量的增大配準(zhǔn)精度的優(yōu)勢越明顯。因此該配準(zhǔn)算法可以應(yīng)用在對配準(zhǔn)精度要求較高，點云凹凸性較明顯的物體上。

4 結(jié)束語

圖7 改進算法與經(jīng)典算法的配準(zhǔn)誤差比較

本文在研究了各種ICP 算法后，針對點云數(shù)據(jù)量大、對配準(zhǔn)精度要求較高的情況，提出了一種基于Hausdorff距離改進的算法。采用三維點云擬合曲面的主曲率和點鄰域內(nèi)Hausdorff距離相結(jié)合的方法，利用正態(tài)分布圖選擇出點云幾何特征明顯的關(guān)鍵點。然后采用K－D 樹加速查找，通過最小二乘迭代進行點云ICP 配準(zhǔn)。實驗結(jié)果表明，本文提出的算法提高了點云配準(zhǔn)的精確度。在實際應(yīng)用中，本算法適合對配準(zhǔn)精度要求較高，點云凹凸性比較明顯的物體配準(zhǔn)上。

［1］DUN WZ，HUN G，HONG LX，et al.The research of optical 3D measuring precision influencing factor in reverse engineering ［J］.Applied Mechanics and Materials，2010，33：157－162.

［2］Hacene A，Mekki A.Bio－CAD reverse engineering of freeform surfaces by planar contours［J］.Computer－Aided Design＆Applications，2011，8 （1）：37－42.

［3］XU Gang，WANG Chunyan.Based on vitural reality modeling language medical volume data reconstruction ［J］.Automation and Instrumentation，2012 （4）：38－40 （in Chinese）. ［徐剛，王春燕.基于虛擬現(xiàn)實建模語言的醫(yī)學(xué)體數(shù)據(jù)三維重建研究［J］.自動化與儀器儀表，2012 （4）：38－40.］

［4］Karen RS，Alexandra SC.Reliability of photogrammetry in the evaluation of postural aspect of individuals with structural scoliosis［J］.Journal of Bodywork and Movement Therapies，2011，16 （2）：210－216.

［5］LI Shifei，WANG Ping，SHEN Zhenkang.A survey of iterative closest point algorithm ［J］.Signal Processing，2009，25（10）：1582－1588 （in Chinese）.［李世飛，王平，沈振康.迭代最近點算法研究進展 ［J］.信號處理，2009，25 （10）：1582－1588.］

［6］YANG Xianhui，WANG Huinan.Application research of ICP algorithm in 3Dpoint cloud alignment［J］.Computer Simulation，2010，27 （8）：235－288 （in Chinese）. ［楊現(xiàn)輝，王惠南.ICP算法在3D點云配準(zhǔn)中的應(yīng)用研究 ［J］.計算機仿真，2010，27 （8）：235－288.］

［7］ZHU Yu，KANG Baosheng，LI Hong’an，et al.An improved method to streamline point cloud ［J］.Computer Application，2012，32 （2）：521－524 （in Chinese）. ［朱煜，康寶生，李洪安，等.一種改進的點云精簡方法 ［J］.計算機應(yīng)用，2012，32 （2）：521－524.］

［8］YU Xiaogao，YU Xiaopeng.Based on angular similarity K－nearest neighbor search algorithm ［J］.Application Research of Computers，2009，26 （9）：239－256 （in Chinese）. ［余小高，余小鵬.一種基于角相似性的K－最近鄰搜索算法 ［J］.計算機應(yīng)用研究，2009，26 （9）：239－256.］

［9］Bu Yanlong，Peng Shuangchun，Wangnan，et al.Constriction of mutual information based matching－suitable features for SAR image aided navigation ［C］／／International Conference on Environmental Science and Information Application Technology，2009.

［10］BAI Yanbing.Free－form surfaces Haudorff diatance approximation calculation to freedom of curves and surfaces［D］.Beijing：Tsinghua University，2011 （in Chinese）.［白彥冰.自由曲面到自由曲線曲面的Hausdorff距離近似值的計算 ［D］.北京：清華大學(xué)，2011.］