應用改進ICP算法的點云配準

楊小青，楊秋翔，楊 劍，鄭曉璐

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

0 引 言

隨著計算機輔助設計技術的提高，利用實物數字化技術采集數據的逆向工程技術得到越來越廣泛的應用［1］。然而在實際測量中由于受到測量儀器和環(huán)境的影響，需要多次測量才能獲得物體表面的完整信息，在后期點云數據配準中可能存在一定程度上的旋轉、平移錯位等問題［2］。因此，提出一種改進策略優(yōu)化ICP 算法，對多次測量所得的覆蓋物體局部信息的點云數據進行有效整合和配準具有非常重要的意義。

點云配準中應用最廣泛的是迭代最近點 （iterative closest point，ICP）算法及其改進算法［3－5］，決定ICP 算法 效率和精度的關鍵在于能否在海量點云數據中，正確、快速地完成基準表面與待匹配表面距離最近點的搜尋。對于最近點的選取，常用方法有多鄰接三角面距離最近點法［6］、二次曲面逼近法［7］、度量點k 鄰域特征空間相似度最大法［8］，上述方法雖能有效加快算法運行速度，但其魯棒性較差。對于傳統(tǒng)ICP 算法存在的收斂速度慢、易于陷入局部最優(yōu)化等問題，張曉娟等提出根據點云拓撲關系中各點周圍的8個鄰近點，按照一定鄰近關系對應準則在鄰近候選點中搜索最近點的算法思想［9］，該方法改善了傳統(tǒng)ICP算法存在的問題，但匹配耗時較長，影響算法效率。

本文針對此問題，利用盒子結構劃分點云數據，將最鄰近點的查找控制在較小的空間范圍中，針對每一獨立單元盒提取特征點構建三角形，結合相似原理，計算最近點對之間的匹配度及其對其余點對匹配正確性的支持度，最終提出一種點云配準改進ICP算法。

1 ICP算法原理與步驟

1.1 基本原理

ICP算法對待拼接的兩片點云要求有較大范圍的重合區(qū)域，首先按照一定準則確定對應點集P 和Q，利用最小二乘法的優(yōu)化思想，重復選擇對應點對，并計算最優(yōu)剛體變換將不同坐標系下點云數據合并到同一坐標系中，直到可以滿足精確配準的收斂精度要求。通過計算求得使目標函數 （1）最小化的旋轉矩陣R 和平移矢量T，從而得到兩片點云之間的配準矩陣

式中：Pi——源數據點集，Qi——目標點集中對應Pi的最近點，R——3＊3矩陣，T——3＊1矢量，F（R，T）——源點集經過平移和旋轉后，其點集中每個點與目標點集中對應點之間距離的平方和，要滿足最小二乘的要求，即要使得F（R，T）達到最小。

1.2 算法步驟

ICP算法是要找到源數據點云與目標數據點云之間的R和T，使得兩數據點集之間滿足某種度量準則達到最優(yōu)匹配。ICP配準步驟如下：

（1）對原始點云數據進行采樣。盡可能的保留表針物體關鍵特征的數據點，減少噪聲點，從而提高后續(xù)步驟的效率；

（2）確立初始對應點集。在兩點云數據之間以距離最近點來近似替代真實對應點確定對應關系；

（3）去除錯誤對應點對。引入某一種約束條件或者評價標準用以去除不可靠、不兼容的錯誤點對；

（4）求解坐標變換。ICP 算法應用最小二乘法的思想迭代求解兩片點云之間的最優(yōu)坐標變換。

2 ICP算法的改進

依照ICP算法的實現步驟可以看出，在原始數據點云與待配準點云之間搜尋最近點對是整個算法的關鍵環(huán)節(jié)，也是最終快速、準確地得到配準結果的有力保證，傳統(tǒng)ICP算法在點云重疊區(qū)域內搜尋最近點的時間復雜度為O（Ai×Bj），假設Ai、Bj是兩點云各自的點集數目。顯然倘若可以有效改進尋找最近點的策略，算法效率將會得到非常大的提升，以此作為算法改進的出發(fā)點，引入盒子結構劃分點云，縮小最近點的查找范圍，按照相似原理對最近點的選取加以約束，計算其支持度保證對應點對的查找正確性。具體如下：

2.1 盒子結構

盒子結構的建立過程分為兩步：

（1）按照初始尺度x 對點集進行分割。以平行于X、Y、Z軸的棱邊為基準，完全包圍點云的最小長方體為盒子外圍，以1mm 的寬度根據每個點的坐標位置進行空間區(qū)域的等間隔分割，從而得到許多大小相等 （Mx×Mx×Mx）的小立方體。依次計算每個單元盒子內點的數目，如果大于50個點，要求對該盒子進行更細致的均等分割；

（2）對盒子結構進行排序。根據盒子劃分參數和每一點的坐標確定各點所在立方體的位置編號，先依次比較每個單元盒頂點坐標最小的點的Z 坐標，升序排序，對于Z坐標相同的，則比較Y 坐標，同理，對于Z坐標、Y 坐標均相同的，按其X 坐標大小重排點云。

容易看出，對海量點云數據進行盒子結構劃分且經過排序之后，最近點的查找范圍將更加明確，為大幅度提升對應點對的查找速度提供較大可能性。

2.2 相似原理

對于任一三角形，在經過比例放大、旋轉、平移變換之后與原三角形相似，將這一特性引入到點云匹配算法中，設法在源數據點云與目標點云中分別構建三角形，利用相似關系，確定點對對應關系。

假設在源數據點集P中找到3個特征點pi、ph、pu在目標點集Q 中的對應點分別是qj、qk、qv，通過上述原理可知，不管是否存在旋轉與平移比例變化，由pi、ph、pu三點構建的三角形與由qj、qk、qv組成的三角形相似。兩三角形的各邊分別表示如下

由相似原理，有下列對應關系

2.3 改進算法步驟

按照上述盒子結構的分割思想將源點云數據進行盒子結構的劃分，并重新排序。之后，以每一盒子為獨立分析單元，計算當前有限點云數目的盒子內各點的曲率值進行特征點的選取。其選取原則是：首先設定一個點云數據曲面的平均曲率閾值，依次計算每個點的平均曲率，并將平均曲率大于預設閾值的點作為候選特征點，然后計算各候選特征點的曲率，將求得的曲率極值點作為真正特征點。

在源數據點集P已提取出的特征點中任選不共線的三點pi、ph、pu構建三角形，任選兩點，譬如為pi、ph，首先找到其在目標點集Q 中的最近點分別為qj、qk。而對于另一點pu對應點的確定，則利用相似原理對其進行約束篩選，即在目標模型點云數據中找到一點qv，使得由qj、qk、qv組成的三角形與源點云數據中pi、ph、pu構建的三角形滿足相似性，并將qv作為pu在目標點集中的初始對應點。對于每一盒子結構單元內的其它點可以按照上述思想依次尋找其最近點，即以已選定三角形的任一條邊為基準，繼續(xù)找到該直線外某一點構成新的三角形，仍然對其進行相似性約束，如此循環(huán)往復下去，直到所有的源數據點云與目標數據點云的對應點對關系都被確定。

對于每 一 六 元 組 （pi、ph、pu、qj、qk、qv），定 義 兩 三 角形的相似度。令δjhk（u，v）為當pi與qj匹配且ph與qk匹配時，由 （pi、ph、pu）組成的三角形與由 （qj、qk、qv）組成的三角形的相似度。令

則

若δjhk（u，v）為0，表示兩三角形完全相似，說明pu相對于pi、ph等同于qv相對于qj、qk，即點對（pu，qv）已經給予（pi，qj，ph，qk）最大的支持度。隨著δjhk（u，v）不斷增加，支持度減小。因此，由相似度可得到支持度的定義如下

使得當pi與qj配對、ph與qk配對時，pu只與和其相互聯(lián)系的、對（pi，qj，ph，qk）有著最大支持度的qv相匹配，此時可以確定 （pu，qv）為真正對應點對，從而保證利用相似原理約束后查找到的對應點對關系的正確性，即有

所有pu求和后 取平 均值，得到（pi，qj，ph，qk）的初始匹配度量值表示為

以及（pi，qj）的初始匹配度量值如下

進行第r次 （r＞0）迭代時，（pu，qv）對于（pi，qj，ph，qk）的匹配度量值依賴于pu、qv之間的位置差別及其Sr－1（pu，qv）的值。為體現出這兩個因素之間的相互作用，取二者中的最小值，則有

以及

為避免由于門限的選取不當帶來的問題，取

預先設定一個極小正數ε為判定閾值 （如0.000001），則當dr＜ε時，迭代終止。

綜上算法步驟可以看出，與傳統(tǒng)ICP 算法尋找對應點相比較，改進后的算法將兩片點云數據中對應點的查找加以盒子結構和相似原理下要求支持度達到最大的匹配條件的雙重約束，使得對應點的搜索速度大幅度提高，且出現錯誤匹配的概率很小。

3 實驗結果與分析

為了檢驗本文提出的改進ICP算法的有效性，在Windows 7 64位操作系統(tǒng)上，基于VS2010上的VC＋＋平臺，采用PCL1.6.0 版 本 點 云 庫［10］、CMAKE 3.0.0 跨 平 臺 編譯工具，搭載ASUS Xtion RRO LIVE體感攝像機，選擇與其兼容的OPENNI第三方庫作為點云數據的輸入源，進行點云數據的配準實驗。實驗所用測試數據來源于PCL 點云庫官網［11］。

在CMAKE中建立工程文件，生成相應的可執(zhí)行文件并運行。將點云數據轉換為PCD 文件格式，確定輸入的待匹配點云數據量大小，分別用不同顏色表示源點云、目標點云，并建立不同視口的可視化對象顯示未配準和配準后的對比效果。經過多次實驗效果對比，當手動迭代次數設置為10次時，其配準效果最好，精度最高，每迭代一次，在配準結果視口中刷新顯示最新迭代效果，在算法運行迭代次數小于預設次數之前，不斷刷新最新的配準結果，直至收斂。

利用傳統(tǒng)ICP算法、文獻 ［9］提出的鄰近關系對應準則配準算法以及本文算法分別對Michael人物模型點云測試數據進行配準實驗。獲取到待配準的兩片點云數據量分別為：5215和4918。實驗結果如圖1所示，其中圖1 （a）表示待配準的兩片點云，圖1 （b）為經典ICP 算法配準后的結果，容易看出，傳統(tǒng)點云數據配準算法遠遠達不到良好的匹配精度，不能滿足實際配準要求。由圖1 （c）可知，作為改進算法，文獻 ［9］提出的某算法在一定程度上提高了配準精度，取得了較好配準效果，然而對比配準結果明顯看到圖1 （d）的配準精度和效果得到了進一步的改善，對實驗所用點云模型中人物的頭部，胳膊，腿部以及雙腳等一些線條較復雜的細節(jié)區(qū)域的配準效果更為完整和精確，即應用本文提出的改進ICP算法可以大幅度提高點云配準效率。

本文提出的改進算法不僅在配準精確度上有較好體現，在配準速度方面也取得令人滿意的實驗結果，對照比較最原始的ICP算法以及文獻 ［9］提出的改進算法，其在海量點云數據的匹配效率最高。為判別算法的配準誤差，現定義誤差參數μ，則有

圖1 Michael點云數據配準效果

其中，H（p1i，q2i）表示經過精確配準之后兩片點云中各對應點對（p1i，q2i）之間的距離，τ為設定的精度閾值，N 為點云數據點數，S（p1i，q2i）表示當前點對已達到配準精度要求，則μ表示點云中未達配準精度要求的數據點所占比率。

如表1所示上述3 種算法實現Michael點云配準過程中，在配準速度和精度方面的對比體現。分析可知，文獻［9］算法在找到良好匹配初值情況下，縮小了配準誤差，但就匹配速度而言，其所需配準時間較傳統(tǒng)ICP 算法減小甚微；而本文提出的改進算法不僅大幅縮短配準時間，而且將配準誤差縮減到0.03%。由此可見，通過對待配準點云數據劃分盒子結構并排序，縮小對應點對的查找范圍，提高了配準速度，同時引入三角形相似原理，驗證當前查找到的對應點對對其余匹配點對的支持度是否為最大，從而保證對應點對的查找正確性，進而減小算法配準誤差。

表1 點云配準比較

運用本文算法對另一組點云測試數據Wolf進行配準，其中圖2 （a）為待配準的且有較大重合區(qū)域的兩片點云，圖2 （b）為配準后結果，配準后所得Wolf模型的頭部、前后腳以及尾巴部分均顯示出較明顯的輪廓特征，可以看出同樣能達到較好的匹配效果，如圖2所示。

圖2 改進ICP算法Wolf點云配準

如表2所示對于不同數據量規(guī)模的點云數據，采用上述3種算法進行配準實驗的結果對比，由表可見，在不同規(guī)模的點云數據量下，文獻 ［9］算法的效率優(yōu)于傳統(tǒng)ICP算法，而本文的改進算法效率則遠遠優(yōu)于以上兩種算法。

表2 不同規(guī)模的點云數據配準對比

按照表2中數據，在點云配準中本文算法效率與傳統(tǒng)ICP、文獻 ［9］算法效率相比較所提高的百分比。可以看出，較傳統(tǒng)ICP算法效率提高了65%以上，較文獻 ［9］算法效率提高了60%以上，而且隨著點云數據量的增大，本文算法體現出的優(yōu)勢愈加明顯，具體如圖3所示。

圖3 改進算法的效率提高比較

4 結束語

本文提出的對點云數據進行盒子結構劃分并排序，在縮小對應點查找范圍的基礎上，引入相似原理對最近點的選擇加以約束，通過計算已構成的對應點對，對其余兩匹配點對的支持度是否達到最大來判斷當前點對的查找正確性，從而保證算法的配準精度。依照上述實驗結果和數據對比分析可知，改進后的算法在配準效率、配準精度上較傳統(tǒng)算法都有大幅度地提高，尤其對于數據量龐大的帶匹配點云模型，表現出的配準效率具有明顯優(yōu)勢。

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

［2］Senin N，Colosimo BM，Pacella M.Point set augmentation through fitting for enhanced ICP registration of point clouds in multisensor coordinate metrology ［J］.Robotics and Computer－Integrated Manufacturing，2013，29 （1）：39－52.

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

［4］DU SY，ZHENG NN，YING SH，et al.Affine iterative closest point algorithm for point set registration ［J］.Pattern Recognition Letters，2010，31 （9）：791－799.

［5］LAURA EW，JOHN FP.A bounding box search algorithm for DEM simulation ［J］.Computer Physics Communications，2011，182 （2）：281－288.

［6］YUAN Jianying，LIU Xianyong，LIU Wei，et al.Research on precise registration of multiviews based on improved ICP algorithm ［J］.Transducer and Micro System Technologies，2008，27 （5）：27－30 （in Chinese）. ［袁建英，劉先勇，劉偉，等.改進ICP算法實現多視點云精確配準研究 ［J］.傳感器與微系統(tǒng)，2008，27 （5）：27－30.］

［7］YANG Xianhui，WANG Huinan.Application research of ICP algorithm in 3Dpoint cloud alignment［J］.Computer Simulation，2010，27 （8）：235－238 （in Chinese）. ［楊現輝，王惠南.ICP算法在3D點云配準中的應用研究 ［J］.計算機仿真，2010，27 （8）：235－238.］

［8］WANG Rui，LI Junshan，LIU Lingxia，et al.Registration of point clouds based on gemotric properties ［J］.Journal East China University of Science and Technology （Natural Science Edition），2009，35 （5）：768－773 （in Chinese）. ［王蕊，李俊山，劉玲霞，等.基于幾何特征的點云配準算法 ［J］.華東理工大學學報 （自然科學版），2009，35 （5）：768－773.］

［9］ZHANG Xiaojuan，LI Zhongke，WANG Xianze，et al.Research of 3Dpoint cloud data registration algorithms based on feature points and improved ICP ［J］.Transducer and Micro System Technologies，2012，3l（9）：116－118 （in Chinese）.［張曉娟，李忠科，王先澤，等.基于特征點和改進ICP 的三維點云數據配準算法 ［J］.傳感器與微系統(tǒng)，2012，3l（9）：116－118.］

［10］ZHU Dehai，GUO Hao，SU Wei.Point cloud Library PCL tutorial［M］.Beijing：Beihang University Press，2012 （in Chinese）. ［朱德海，郭浩，蘇偉.點云庫PCL 學習教程［M］.北京：北京航空航天大學出版社，2012.］

［11］Point cloud library website.Point cloud data［EB／OL］.［2014－10－06］.http：／／www.pointclouds.org／（in Chinese）.［PCL點云庫官網.點 云 數 據［EB／OL］.［2014－10－06］.http：／／www.pointclouds.org／.］