改進的ICP算法在三維模型配準中的研究*

楊 軍，張 瑤，黃 亮

1.蘭州交通大學 電子與信息工程學院，蘭州 730070

2.蘭州交通大學 自動化與電氣工程學院，蘭州 730070

1 引言

配準技術(shù)是當今數(shù)字化檢測領(lǐng)域的一項關(guān)鍵技術(shù)，最早出現(xiàn)于醫(yī)療診斷和圖像處理領(lǐng)域[1]，目前已廣泛應用于逆向工程[2]、虛擬現(xiàn)實[3]、機器人[4]和柔性裝配[5]等領(lǐng)域。

模型配準是指通過在源模型和目標模型之間構(gòu)造一個三維空間變換，使源模型在此變換作用下能夠最大限度地和目標模型重合。目前主要有三類方法：第一類是基于標記的配準方法，通過確定兩模型中對應的特征點，并由對應特征點計算目標模型和源模型之間的空間變換。該方法主要應用于醫(yī)學領(lǐng)域。第二類是基于表面的配準方法，在模型表面上定義并計算突出的特征，根據(jù)模型表面的共同特征在重疊區(qū)域內(nèi)搜索兩個表面之間的唯一匹配模式。該方法通常應用于工業(yè)領(lǐng)域的模型配準。第三類是基于體素相似性的配準方法，不需要對像素做預處理（去噪或者分割），直接作用于圖像，但該方法計算量太大，很少用于三維模型配準。模型配準一般分為兩步：初始配準和精確配準。初始配準是通過縮小兩模型旋轉(zhuǎn)和位移誤差將兩模型大致調(diào)整到同一位置，為迭代最近點（iterative closest point，ICP）配準提供最優(yōu)初始值；精確配準則是利用初始匹配值不斷地執(zhí)行迭代變換，并使迭代收斂到全局最優(yōu)的配準結(jié)果。

1992年，Besl和Mckay提出了ICP算法[6]，由于其操作簡單和良好的配準效果而廣泛應用于各種領(lǐng)域的配準問題，然而ICP算法對于初始值的敏感性極高且運行效率低。針對經(jīng)典ICP算法的缺點，中外許多學者對尋找和選取對應點對設(shè)計了各種算法，以保證能夠得到最優(yōu)的初始匹配點對，提高ICP算法的速度和精確度。文獻[7]結(jié)合點集增強技術(shù)與經(jīng)典ICP算法，將一些新的采樣點加入到已有的點集中并作為標記點，用于解決由多傳感器掃描獲取的三維點云數(shù)據(jù)的坐標融合問題，提高了配準的準確度和魯棒性，然而該算法太依賴新加入的標記點，而標記點對應位置準確與否直接影響配準結(jié)果。文獻[8]利用模型表面曲率變化和法線矢量之間的夾角來約束對應點對，這種方法雖然提高了配準效率和準確度，但仍然需要用戶給定初始配準點。文獻[9]除了用模型表面一點和其鄰近點組成的切平面與此點處法向量的夾角閾值約束對應點對外，還加入了點云數(shù)據(jù)的對應點對間的歐氏距離閾值來選取最合適的對應點對，這種方法對于小型零件檢測效果很好，但當點云數(shù)據(jù)規(guī)模超過8 000點之后，其精確度不如經(jīng)典ICP算法。文獻[10]認為錯誤的對應點對對配準結(jié)果影響較大，于是將圖像信息融合到改進的ICP算法中，用SIFT特征點減少錯誤對應點對，提高了配準的準確度和效率，但此方法除了必須的點云外還需要模型在真實環(huán)境中的二維圖像信息。文獻[11]提出了基于點云邊界特征點的改進ICP算法。首先，對點云最小包圍盒進行三維空間劃分，運用邊界種子網(wǎng)格識別及生長算法，從點云邊界提取特征點，運用奇異值矩陣分解法求出點云的變換矩陣，得到初始配準結(jié)果。其次，對點云對應點賦予權(quán)重，剔除權(quán)重大于閾值的點，通過對目標函數(shù)引入M-估計，剔除異常點。最后，在初始配準的基礎(chǔ)上，運用改進的ICP方法精確配準。算法效率提高了，但準確度并不是很高。

常用的初始配準方法有重心重合法[12]、標簽法[13]、提取特征法[14-15]。重心重合法計算出兩模型的重心坐標，然后將重心重合，這種方法只能縮小平移錯位而無法縮小旋轉(zhuǎn)錯位；標簽法在測量時人為地貼上一些特征點，然后利用這些特征點進行定位，根據(jù)這些對應的特征點將兩模型大致調(diào)整到同一位置，這種方式仍然是依賴于測量和儀器的；提取特征法，如提取平面特征、輪廓曲線等，通過將模型表面特征相同部分重合以達到初始配準的目的，但這種方式要求點云有比較明顯的突出特征。

本文針對ICP算法對于初始值的敏感性以及整體與部分模型的配準精確度兩方面進行了改進。首先，采用主成分分析法（principal component analysis，PCA）對模型進行初始配準，使用三維縮放變換將兩模型調(diào)整為同等大小。然后，用KD-tree進行最近點搜索提高搜索速度。由于本文主要針對整體與部分模型間的配準，對配準精度要求更高，故利用自適應最優(yōu)閾值優(yōu)化ICP算法，提高模型配準的準確度。最后，為了防止在上下或前后顛倒的情況下進行三維模型重合度檢測，做最后的反轉(zhuǎn)調(diào)整。

2 模型的初始配準

在運用ICP算法對模型進行精確配準之前，兩模型的點集有可能在不同的坐標系下，也有可能處于同一坐標系的任意位置，那么點集的迭代初始值就不能滿足精確配準的要求，因此需要通過搜索最近點，或者使用三維幾何特征來計算兩組點集的匹配條件。然而，算法的復雜度和計算量會劇增，影響執(zhí)行效率。因此，在精確配準前先進行兩模型點集間的初始配準，為精確配準提供初始值以提高最終的配準精確度和效率。

文中使用了一種全局配準方法。由于兩模型點集的主軸形狀相似度很高，可以通過計算兩組點集的主軸以得到點集之間的剛體變換矩陣，此方法稱為主成分分析法。給定一組點集P={P1,P2,…,Pn},計算其均值和協(xié)方差矩陣cov,對矩陣cov進行特征向量分解，得到的正交特征向量作為點集P的三坐標軸X、Y、Z,以均值為坐標系原點，建立這組點集的坐標系?？梢杂猛瑯拥姆绞教幚碓茨Ｐ秃湍繕四Ｐ偷狞c集，并將兩模型的坐標系進行位置調(diào)整，以滿足初始配準的要求。

然而，本文并沒有將兩模型新的坐標系調(diào)整至同一位置，而是先將兩模型進行三維縮放調(diào)整為同等大小。

給定包含n個采樣點的源模型S={s1,s2,…,sn}和包含m個采樣點的目標模型T={t1,t2,…,tm},用矩陣（1）和（2）分別表示源模型和目標模型點云數(shù)據(jù)的坐標。其中，下標s和t分別表示源模型和目標模型。

分別計算源模型和目標模型的點集在X、Y、Z坐標軸上的平均值分別表示點在X軸、Y軸、Z軸上的坐標。

分別計算源模型和目標模型所有點坐標值xi、yi、zi和相應均值之間的差值，如式（5）和（6）所示。

用Bs和Bt分別表示經(jīng)過PCA分析后形成的新坐標系下源模型和目標模型各點的新的坐標矩陣。

分別計算源模型和目標模型每一列的最大值和最 小 值 max(Bs)、 min(Bs)、 max(Bt)、 min(Bt),并 計 算最大值和最小值之間的差值Ds、Dt。

構(gòu)造新的矩陣D,其中元素d是矩陣Ds和Dt對應元素相除得到的一行三列矩陣，d(1,1)、d(1,2)、d(1,3)分別表示矩陣d的第一行一列、一行二列和一行三列元素。

D即為源模型到目標模型的縮放矩陣，因此模型的縮放調(diào)整由下式完成：

再將源模型依次繞X軸、Y軸、Z軸、XOY面、XOZ面、YOZ面反轉(zhuǎn)并求出每次反轉(zhuǎn)后的誤差，選取誤差最小的反轉(zhuǎn)方式來調(diào)整源模型的方向。這樣做既節(jié)省時間又不影響結(jié)果。

3 模型的精確配準

初始配準只是將源模型和目標模型縮放至同等大小，并調(diào)整至方向基本一致，其精度太低，遠不能達到完全配準的要求。為了提高配準精度，還需在初始配準的基礎(chǔ)上進行精確配準。本文采用ICP算法進行精確配準，其關(guān)鍵是對應點集配準（corresponding point set registration）。

對應點集配準算法的目的是利用最小二乘法計算模型間最優(yōu)的坐標變換矩陣。對兩個點集，可以采用單位四元數(shù)法[1]得到它們之間的變換矩陣。

用S和T分別表示源模型和目標模型的點云數(shù)據(jù)，它們應滿足以下條件：

（1）T中有m個點和S中有n個點，且m=n;

（2）對于T中的任意點ti都對應于S中具有相同下標的si,即ti=(sisi、ti分別表示源模型和目標模型上的點）。

設(shè)旋轉(zhuǎn)變換向量為單位四元數(shù)qrotat=[q0q1q2q3]T,其中q0≥0,并且,則可以得到3×3旋轉(zhuǎn)變換矩陣R(qrotat)。設(shè)平移變換向量為qtrans=[q4q5q6]T,則求解對應點集間的最佳坐標變換向量問題可轉(zhuǎn)化為求解qrotat和qtrans使得函數(shù)f(qtrans,qrotat)最小化的問題：

算法流程如下：

步驟1給定源模型和目標模型的對應點集S和T,計算目標模型點集T的重心μT和源模型點集S的重心μS:

步驟2由點集T和S構(gòu)造協(xié)方差矩陣：

步驟3由協(xié)方差矩陣構(gòu)造4×4對稱矩陣：

其中，I3是 3×3單位矩陣；tr(ΣT,S)是矩陣 ΣT,S的跡，Δ=[A23A31A12]T,Ai,j是ΣT,S的反對稱矩陣,Ai,j=(ΣT,S-ΣTT,S)i,j。

步驟4計算Q(ΣT,S)的特征值和特征向量，其最大特征值對應的特征向量即為最佳旋轉(zhuǎn)向量qrotat=[q0q1q2q3]T,最佳旋轉(zhuǎn)矩陣R(qrotat)為：

步驟5計算最佳平移向量為：

步驟6利用已得到的平移向量和旋轉(zhuǎn)向量，計算dl=f(qtrans,qrotat),即迭代后的最小誤差。

4 改進的ICP算法

ICP算法是目前應用最普遍的配準方法，其配準效果良好，但是算法本身計算效率不高。而KD-tree[12,16]是一種分割k維數(shù)據(jù)空間的數(shù)據(jù)結(jié)構(gòu)，主要應用于多維空間關(guān)鍵數(shù)據(jù)的搜索（如本文中的最近鄰搜索），并對搜索空間進行層次劃分，具有搜索速度快的優(yōu)點，故本文采用KD-tree搜索最近點來提高搜索效率。此外，整體與部分模型間的配準對于配準精度的要求更高，故本文采用自適應的最優(yōu)閾值來優(yōu)化迭代結(jié)果，提高了經(jīng)典ICP算法的配準精度。

在兩模型初始配準之后，不直接執(zhí)行ICP算法，而是給定不同的閾值K，計算不同閾值時對兩模型執(zhí)行ICP算法所對應的配準誤差，記錄最小誤差所對應的閾值并記為Kbest,用Kbest作為閾值，重新執(zhí)行迭代最近點算法，直至算法收斂。

在執(zhí)行ICP算法之后有可能會出現(xiàn)模型上下或者前后顛倒的現(xiàn)象，這是由于算法在執(zhí)行過程中，可能會出現(xiàn)由于初始配準的不夠準確導致執(zhí)行ICP算法時不能找到正確的對應點對，故目前通常的做法是在初始配準后用最小包圍盒的方法進行校正。但在實驗中發(fā)現(xiàn)即使在初始配準后用最小包圍盒進行矯正，最終還是可能會出現(xiàn)這種狀況。

因此，本文算法在執(zhí)行完ICP配準后，增加了三維目標重合度檢測算法，根據(jù)兩模型重合度再進行最后的反轉(zhuǎn)調(diào)整，這里也要用到最小包圍盒。根據(jù)包圍盒立方體的三條邊與三個坐標軸間的位置關(guān)系，包圍盒分為沿坐標軸的軸向包圍盒AABB（axisaligned bounding box）和有向包圍盒OBB（oriented bounding box）[17]兩種，如圖1所示。本文選用OBB包圍盒。

Fig.1 Axis-aligned bounding box(left)and oriented bounding box(right)圖1 AABB包圍盒（左）和OBB包圍盒（右）

設(shè)Vs為源模型點云包圍盒體積，Vt為目標模型點云包圍盒體積，Vi為源模型點云和目標點云包圍盒相交得到的包圍盒的體積，則包圍盒重合系數(shù)為：

如果τ＞thr,thr為設(shè)定閾值，則兩點云模型大致重合。否則兩模型不重合，那么反轉(zhuǎn)坐標系的X軸或Y軸，將X軸和Y軸是否反轉(zhuǎn)的4種情況全部測試完，并且沒有任何一個τ大于thr,則說明兩模型間差異較大，不能配準。

算法流程如圖2所示，具體步驟說明如下。

步驟1輸入源模型和目標模型的點云數(shù)據(jù)。

步驟2經(jīng)過縮放，使兩模型大小相同，再用矩陣C調(diào)整模型的方向。

步驟3用KD-tree來搜索目標模型最鄰近點為對應點對，計算源模型上一點到目標模型上對應點之間距離的平均值aver和標準差sde。

步驟4設(shè)變量k,且1≤k≤3。k從1開始每隔0.05取一個值，得到不同的閾值K=k×aver+sde,對對應點對進行篩選，對應點對間的距離小于閾值的保留，否則剔除。計算得到不同閾值K執(zhí)行ICP算法后對應的配準誤差，記錄最小誤差對應的閾值為Kbest。

步驟5以Kbest為閾值執(zhí)行ICP算法，并計算每次執(zhí)行ICP算法后的配準誤差。

步驟6判斷誤差是否收斂，若收斂，則兩模型完成配準。若不收斂，判斷迭代次數(shù)是否超出預設(shè)值，如超出，則強制結(jié)束迭代，兩模型無法完全配準；如迭代次數(shù)沒有超出預設(shè)值，返回步驟4。

Fig.2 Flow chart of improved ICP algorithm圖2 算法流程圖

步驟7ICP算法結(jié)束后，執(zhí)行三維目標重合度檢測算法，分析兩個目標的重合度情況，根據(jù)重合度再進行最后的旋轉(zhuǎn)調(diào)整。

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

在基于極點譜植入的初始化過程中，由于優(yōu)化策略采用了貪婪優(yōu)化算法，從而建立了較好的初始對應關(guān)系。對已建立的初始對應關(guān)系再通過貪婪優(yōu)化算法進行優(yōu)化，就可得到最終的稀疏對應關(guān)系。

本文算法采用Matlab編程實現(xiàn)，實驗中迭代次數(shù)預設(shè)最大值為300，針對部分模型（與源模型相比，目標模型缺少了某一部件）與完整模型進行實驗，并將本文算法的誤差與經(jīng)典ICP算法進行對比。

圖3是缺少兩只小臂且沒有穿衣服的人體模型（綠色）與完整模型（黃色）在只執(zhí)行初始配準算法后的結(jié)果，可以看出配準精確度很差，沒有對齊。同樣的模型，圖4所示實驗表示只用精確配準的結(jié)果，配準效果依然不好。由此可見，如果沒有初始配準為ICP算法計算出一個最優(yōu)的初始值，則配準效果較差，這是由于ICP算法對初始值的敏感性所致。圖5所示為本文算法的配準結(jié)果和迭代誤差圖，基本完全配準，最終配準誤差為4.217×10-5。圖6為缺少一條腿的馬模型（綠色）和完整馬模型（黃色）使用本文算法的配準結(jié)果和迭代誤差?？梢钥闯?，在迭代過程的初期誤差曲線有曲折，當?shù)螖?shù)超過20次時，算法收斂，基本達到完全配準。最終配準誤差為11.981×10-5。圖7所示為缺少兩只胳膊的人模型（綠色）與完整模型（黃色）的配準結(jié)果，此組模型中源模型（綠色）與目標模型（黃色）在小腿部分不完全相同，從配準結(jié)果看小腿部分綠色多而其他部分黃色多，在迭代過程中不斷地前后調(diào)整導致誤差不停波動，最終配準誤差為14.139×10-5。圖8為缺少一只胳膊且穿衣服的人體模型（綠色）與完整模型（黃色）使用本文算法的配準結(jié)果。與前三組模型相比，此模型的點云規(guī)模較大，且表面紋理復雜（因為衣服的皺褶）?？梢钥闯?，配準效果良好，但是在模型的腳腿部由于有衣服浮動的影響，兩模型有差別，在迭代過程中不斷地調(diào)整，最終配準誤差在24.975×10-5附近波動收斂。當兩模型姿態(tài)完全相同時，本文算法會完全配準，但當兩模型姿態(tài)有所區(qū)別時，本文算法會在不同的部分不斷調(diào)整迭代直到達到預設(shè)迭代值。

Fig.3 Result of using initial registration separately to align point clouds圖3 僅用初始配準的結(jié)果

Fig.4 Result of using precise registration separately to align point clouds圖4 僅用精確配準的結(jié)果

Fig.5 Registration result and iterative error of two human models(complete human model vs partial human model lack of two forearms)圖5 完整與部分（缺少兩只小臂）人體模型的配準結(jié)果與迭代誤差

Fig.6 Registration result and iterative error of horse models(complete horse model vs partial horse model lack of right foreleg)圖6 完整與部分（缺少一條腿）馬模型的配準結(jié)果與迭代誤差

Fig.7 Registration result and iterative error of human models(complete human model vs partial human model lack of two arms)圖7 完整與部分（缺少兩只胳膊）人體模型的配準

Fig.8 Registration result and iterative error of two human models with clothes(complete human model vs partial human model lack of right arm)圖8 穿衣服的完整與部分（缺少右臂）人體模型配準與迭代誤差

本文算法對完整模型之間的配準也非常有效。圖9是4組完整模型的配準結(jié)果，可以看出，配準效果良好，骨頭、貓、狗、人體模型的最終配準誤差分別為10.224×10-5、0.380 42×10-5、2.569 8×10-5、6.842 4×10-5。

表1是本文算法和經(jīng)典ICP算法對不同點云數(shù)目的模型進行配準的誤差對比。本文算法根據(jù)每個模型的實際情況，搜索出最適合的閾值來篩選出最合適的對應點對，提高配準的準確度。且本文主要針對整體與部分模型間的配準，對配準精確度要求更高?？梢钥闯觯疚乃惴▽τ谌撕婉R模型（整體與部分模型）配準精確度的提高程度相對于貓和狗模型（兩個完整模型）高很多。配準誤差對比表明本文算法既能用于源模型是目標模型的某些部件這種情況，也適用于兩個完整模型的配準，同時提高了經(jīng)典ICP算法的模型配準精度。

Fig.9 Registration results of 4 sets of complete models圖9 4組完整模型配準結(jié)果

6 結(jié)束語

隨著科技工業(yè)的快速發(fā)展，醫(yī)學和工業(yè)等領(lǐng)域?qū)Ω鞣N技術(shù)檢測的精確度要求越來越高，模型配準是這些檢測領(lǐng)域中的基本問題。本文提出了加入縮放功能的PCA算法，使用KD-tree搜索鄰近點提高搜索速度，并使用自適應最優(yōu)閾值優(yōu)化ICP算法提高其準確度。此外，還進行模型重合度檢測做最后調(diào)整避免出現(xiàn)模型上下或前后顛倒問題。實驗結(jié)果表明本文算法既可以配準整體與部分模型，也適用于兩個完整模型間的配準問題，同時提高了配準精度和效率。然而，在整體與部分模型的配準中，目標模型經(jīng)常比源模型多一些部件，當搜索目標模型的所有點以尋找源模型的對應點時，往往也會搜索在源模型中沒有對應點的部件。這屬于無用遍歷，增加了程序的運行時間，故在程序的運行速度方面可以再做進一步改善，比如利用三維網(wǎng)格分割技術(shù)，先將目標模型中多余的部件去除掉，然后再執(zhí)行本文改進的ICP算法來進一步提高速度，這將是下一步的研究方向。

Table 1 Comparison of registration error between traditional ICP algorithm and improved ICP表1 本文算法與傳統(tǒng)ICP算法配準誤差對比

[1]Zhang J,Yan C H,Chui C K,et al.Multimodal image registration system for image-guided orthopaedic surgery[J].Machine Vision&Applications,2011,22(5):851-863.

[2]Imbert M,Li Xiaoxing.Automatic registration of 3D point clouds for reverse engineering[J].Journal of Computational&Theoretical Nanoscience,2011,4(6):2431-2432.

[3]Lemeszenski D D A,Nakamura R.A marker-free calibration and registration process for multiple depth maps from structured light sensors and its application in video avatar systems[C]//Proceedings of the 15th Symposium on Virtual and Augmented Reality,Cuiabá-Mato Grosso,May 28-31,2013.Washington:IEEE Computer Society,2013:73-82.

[4]Schneider C,Guerrero J,Nguan C,et al.Intra-operative“pick-up”ultrasound for robot assisted surgery with vessel extraction and registration:a feasibility study[C]//LNCS 6689:Proceedings of the 2nd International Conference on Information Processing in Computer-Assisted Interventions,Berlin,Jun 22,2011.Berlin,Heidelberg:Springer,2011:122-132.

[5]Jacobs A G,Jung B,Ober C K,et al.Control of PS-b-PMMA directed self-assembly registration by laser induced millisecond thermal annealing[C]//Proceeding of the SPIE 9049,Alternative Lithographic Technologies VI,Bellingham,Mar 28,2014.San Jose:SPIE,2014:4177-4180.

[6]Besl P J,Mckay N D.A method for registration of 3-D shapes[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2):239-256.

[7]Senin N,Colosimo B M,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.

[8]Bae K H,Lichti D D.A method for automated registration of unorganized point clouds[J].ISPRS Journal of Photogrammetry and Remote Sensing,2008,63(1):36-54.

[9]Zhong Ying,Zhang Meng.Automatic registration of point clouds based on improved ICP algorithm[J].Control Engineering of China,2014,21(1):37-40.

[10]Zheng Zhongyang,Li Yan,Wang Jun.LiDAR point cloud registration based on improved ICP method and SIFT feature[C]//Proceedings of the 2015 International Conference on Progress in Informatics and Computing,Nanjing,Dec 18-20,2015.Washington:IEEE Computer Society,2015:588-592.

[11]Wang Xin,Zhang Mingming,Yu Xiao,et al.Point cloud data registration using improved iterative closest point method[J].Optics and Precision Engineering,2012,20(9):2068-2077.

[12]Liu Jiang,Zhang Xu,Zhu Jiwen.ICP three-dimensional point cloud registration based onK-D tree optimization[J].Engineering of Surveying and Mapping,2016,25(6):15-18.

[13]Meng Cai,Zhang Jun,Zhou Fugen,et al.New method for geometric calibration and distortion correction of conventional C-arm[J].Computers in Biology&Medicine,2014,52(3):49-56.

[14]He Bingwei,Lin Zeming,Li Y F.An automatic registration algorithm for the scattered point clouds based on the curvature feature[J].Optics&Laser Technology,2013,46(1):53-60.

[15]Lakehal A,Beqqali O E,Zemzami O A.Retrieval of similar shapes under affine transform using affine length paramete-rization[J].Journal of Computer Science,2010,6(10):1226-1232.

[16]Shakhnarovich G,Darrell T,Indyk P.Nearest-neighbors methods in learning and vision:theory and practice[J].Pattern Analysis andApplications,2008,11(2):221-222.

[17]Walizer L E,Peters J F.A bounding box search algorithm for DEM simulation[J].Computer Physics Communications,2011,182(2):281-288.

附中文參考文獻：

[9]鐘瑩,張蒙.基于改進ICP算法的點云自動配準技術(shù)[J].控制工程,2014,21(1):37-40.

[11]王欣,張明明,于曉,等.應用改進迭代最近點方法的點云數(shù)據(jù)配準[J].光學精密工程,2012,20(9):2068-2077.

[12]劉江,張旭,朱繼文.一種基于K-D樹優(yōu)化的ICP三維點云配準方法[J].測繪工程,2016,25(6):15-18.