摘  要： 在三維激光點(diǎn)云數(shù)據(jù)配準(zhǔn)的過(guò)程中，利用傳統(tǒng)Iterative Closest Point（ICP）算法搜索對(duì)應(yīng)點(diǎn)對(duì)時(shí)速度慢，而且配準(zhǔn)精細(xì)化程度低，遠(yuǎn)達(dá)不到三維建模后期處理的要求。針對(duì)這一問(wèn)題，提出一種基于KDTree改進(jìn)的ICP算法以實(shí)現(xiàn)激光點(diǎn)云數(shù)據(jù)的快速精細(xì)化配準(zhǔn)。通過(guò)實(shí)驗(yàn)驗(yàn)證算法的有效性和合理性，為后期模型重建過(guò)程中的三角網(wǎng)格化、曲面化、紋理映射提供強(qiáng)有力的理論和實(shí)踐基礎(chǔ)。

　　關(guān)鍵詞： 激光點(diǎn)云；ICP算法；KDTree；曲面化

0 引言

　　在地面三維激光掃描過(guò)程中，受物體尺寸、物體間的遮蔽以及掃描儀視場(chǎng)角等因素的影響，每站掃描只能獲得本站掃描儀坐標(biāo)系下的點(diǎn)云數(shù)據(jù)。對(duì)于大型立體模型而言，大多數(shù)情況下不太可能只通過(guò)一次掃描就獲得全部的物體表面坐標(biāo)及屬性數(shù)據(jù)。因此，為了獲得完整的物體表面坐標(biāo)及屬性數(shù)據(jù)，必須從不同的視角來(lái)掃描場(chǎng)景。在點(diǎn)云數(shù)據(jù)處理階段，點(diǎn)云配準(zhǔn)是十分關(guān)鍵的問(wèn)題之一，配準(zhǔn)的精細(xì)化程度直接影響后續(xù)操作。

1 掃描設(shè)備及作業(yè)流程

　　對(duì)物體進(jìn)行點(diǎn)云數(shù)據(jù)采集時(shí)，采用的三維激光掃描儀是ScanStation C10全站式三維激光掃描儀。掃描參數(shù)設(shè)置情況如下：全景掃描，掃描視角為360°×270°，掃描速度為  50 000點(diǎn)/s，掃描距離為300 m，點(diǎn)位標(biāo)稱精度為±2 mm。

　　設(shè)置好參數(shù)以后，分別在兩位置坐標(biāo)系下對(duì)只有4個(gè)面的長(zhǎng)方體實(shí)物進(jìn)行掃描采集。然后，依次進(jìn)行點(diǎn)云數(shù)據(jù)的去噪、稀疏采樣，由此獲得能足夠表達(dá)物體模型的點(diǎn)云數(shù)據(jù)。

　　圖1為使用ScanStation C10掃描儀的作業(yè)流程，圖2為經(jīng)過(guò)去噪采樣處理過(guò)的數(shù)據(jù)并可視化的結(jié)果。

2 配準(zhǔn)定義

　　點(diǎn)云配準(zhǔn)簡(jiǎn)單來(lái)說(shuō)就是將從多個(gè)站點(diǎn)獲得的點(diǎn)云數(shù)據(jù)進(jìn)行拼接，得到一個(gè)統(tǒng)一坐標(biāo)系下的三維數(shù)據(jù)點(diǎn)集。它類似于數(shù)學(xué)上的映射問(wèn)題，也就是說(shuō)要先找到兩個(gè)點(diǎn)云數(shù)據(jù)集間的對(duì)應(yīng)關(guān)系，然后將一個(gè)坐標(biāo)系下的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到另一個(gè)坐標(biāo)系下。

　　配準(zhǔn)過(guò)程主要有以下兩個(gè)步驟：（1）尋找對(duì)應(yīng)關(guān)系；（2）解算變換參數(shù)。即首先確定同名點(diǎn)對(duì)，然后解算旋轉(zhuǎn)矩陣R和平移矩陣T。

　　同名點(diǎn)對(duì)：同一個(gè)點(diǎn)在不同坐標(biāo)系下的表達(dá)。

　　圖3所示為兩站掃描示意圖，在A、B兩處分別安放掃描儀對(duì)同一個(gè)物體進(jìn)行掃描。在A處獲得坐標(biāo)O1-x1y1z1下的點(diǎn)云數(shù)據(jù)M，在B處獲得坐標(biāo)系O2-x1y1z1下的點(diǎn)云數(shù)據(jù)N，配準(zhǔn)的目的就是將兩個(gè)坐標(biāo)系O1-x1y1z1、O2-x1y1z1下的點(diǎn)云數(shù)據(jù)M和N轉(zhuǎn)換到同一個(gè)坐標(biāo)系下。

　　對(duì)于從兩站采集到的點(diǎn)云集合M和N，Mi（X，Y，Z），Ni（x，y，z），且Mi、Ni為在不同坐標(biāo)系下的同一點(diǎn)，嚴(yán)格來(lái)說(shuō)，點(diǎn)云配準(zhǔn)就是將全部來(lái)自兩個(gè)不同坐標(biāo)系下的同名點(diǎn)對(duì)（Mi，Ni）滿足剛體變換（R，T），即：

　　其中，R為旋轉(zhuǎn)矩陣，T為平移矩陣，α、β、γ表示沿X、Y、Z軸的旋轉(zhuǎn)角，tx、ty、tz表示位移量。

　　式（1）稱作空間相似變換公式，它是點(diǎn)云配準(zhǔn)的基本公式。由式（1）可解出同名點(diǎn)轉(zhuǎn)換參數(shù)，而后進(jìn)行點(diǎn)云數(shù)據(jù)配準(zhǔn)。

3 點(diǎn)云配準(zhǔn)算法

　　目前，點(diǎn)云配準(zhǔn)算法依據(jù)其采用的配準(zhǔn)基元可將其分為無(wú)特征的配準(zhǔn)和基于特征的配準(zhǔn)[1]兩大類。

　　基于特征的配準(zhǔn)是指利用角點(diǎn)、邊緣、面等幾何特征[2]來(lái)解算變化參數(shù)。這類算法主要有以下幾種：基于控制點(diǎn)的配準(zhǔn)算法[3]、基于線特征的配準(zhǔn)算法[4]以及基于曲率[5]的點(diǎn)云配準(zhǔn)算法。

　　無(wú)特征的配準(zhǔn)就是直接利用原始數(shù)據(jù)進(jìn)行配準(zhǔn)。此類算法中最為著名的是ICP（Iterative Closest Point）算法[6]，但該算法只適用于存在明確對(duì)應(yīng)關(guān)系的點(diǎn)集，并且計(jì)算速度慢。為此，在其他傳統(tǒng)ICP算法[7]的基礎(chǔ)之上，提出基于KDTree[8]的改進(jìn)ICP算法，包括基于KDTree搜索對(duì)應(yīng)點(diǎn)對(duì)和矩陣變換參數(shù)的計(jì)算兩方面的內(nèi)容。

　　3.1 傳統(tǒng)ICP配準(zhǔn)算法

　　基本思路：在對(duì)應(yīng)點(diǎn)云中搜尋最鄰近點(diǎn)對(duì)，利用此最鄰近點(diǎn)對(duì)求解剛體變換參數(shù)R、T，在這個(gè)過(guò)程中點(diǎn)對(duì)的搜尋和變換參數(shù)的求解都是迭代計(jì)算的。

　　算法步驟如下：

　?。?）令Ω為點(diǎn)云M和N的重疊域，設(shè)在Ω然數(shù)集N及其擴(kuò)展情況，如正整數(shù)集Z+、n維實(shí)坐標(biāo)中的任一點(diǎn)對(duì)應(yīng)在M和N上的位置分別是Mi、Ni，初始迭代時(shí)兩個(gè)點(diǎn)集的初始變換參數(shù)是R0，T0。

　?。?）點(diǎn)集M中的每個(gè)點(diǎn)Mi，由初始變換參數(shù)最小為標(biāo)準(zhǔn)，求出新的變換參數(shù)R、T。

　?。?）根據(jù)找到的全部最近點(diǎn)對(duì)（mi，ni），求出兩個(gè)點(diǎn)集的變換參數(shù)R、T，并且以全部點(diǎn)對(duì)距離的平方和最小為標(biāo)準(zhǔn)，求出新的變換參數(shù)R、T。

　?。?）在相鄰兩次計(jì)算所得的距離平方和的差值小于給定的閾值時(shí)結(jié)束迭代，否則重復(fù)步驟（2）和（3）直至小于給定的閾值。

　　（5）根據(jù)最終得到的R、T將點(diǎn)云M映射變換到點(diǎn)云N的坐標(biāo)系下，完成配準(zhǔn)。

　　3.2 改進(jìn)的基于KDTree的ICP算法

　　3.2.1 算法準(zhǔn)備工作

　　由KDTree的算法原理可知，當(dāng)鄰域點(diǎn)集中點(diǎn)數(shù)k為1時(shí)，搜尋點(diǎn)與鄰域點(diǎn)間建立一一映射關(guān)系。此時(shí)，搜索到的鄰域點(diǎn)是搜尋點(diǎn)與鄰域點(diǎn)集中距離最小的點(diǎn)。

　　該算法中要用到的變換矩陣?yán)盟脑胤╗9]求解，過(guò)程如下：

　?。?）求解點(diǎn)集M、N的重心坐標(biāo)O1、O2。

　　（2）點(diǎn)集M、N的重心化：

　?。?）構(gòu)建矩陣Q：

　　（4）求解Q的最大特征值以及最大特征值對(duì)應(yīng)的特征向量（w，m，n，p）。

　?。?）構(gòu)造旋轉(zhuǎn)矩陣：

　?。?）解算平移向量T：

　　T=O2-R′O1(4)

　　3.2.2 算法實(shí)現(xiàn)步驟

　　（1）設(shè)點(diǎn)集M、N的部分區(qū)域分別為目標(biāo)點(diǎn)集M′和參考點(diǎn)集N′。

　?。?）令k=1，在N′中通過(guò)KDTree加速搜索為M′中的任意點(diǎn)搜索最近鄰域點(diǎn)，由此找出M′中任意一點(diǎn)的映射點(diǎn)，也就是找出M′中點(diǎn)集合Mm={M1m，M2m，…，Mnm}在N′上的映射點(diǎn)集Nm={N1m，N2m，…，Nnm}，m代表迭代次數(shù)，n代表點(diǎn)個(gè)數(shù)。

　　（3）利用設(shè)置好的最小閾值距離Di，刪除Mm、Nm中錯(cuò)誤的點(diǎn)對(duì)，并完成Mm、Nm的更新。

　　（4）利用四元素法計(jì)算Mm、Nm的變換矩陣R和平移量T。

　?。?）由得到的R、T變換Mm，得到最新的Mm。

　　（6）重復(fù)步驟（2）~（5），求出Mm中每一點(diǎn)到Nm中的映射點(diǎn)對(duì)，以及相應(yīng)的R、T。

　?。?）當(dāng)最后的R、T滿足配準(zhǔn)后，對(duì)應(yīng)點(diǎn)對(duì)坐標(biāo)間差值的閾值收斂條件|xm-xn|or|ym-yn|or|zm-zn|<ε時(shí)，結(jié)束循環(huán)，匹配成功；如果不滿足收斂條件，進(jìn)行第m+1次迭代計(jì)算。

　　算法設(shè)計(jì)流程如圖4所示。

　　3.2.3 主要函數(shù)代碼介紹

　　最小閾值Di設(shè)定函數(shù)：

　　inline void setTransformationEpsilon（double epsilon）{transformation_epsilon_=epsilon；}

　　坐標(biāo)差閾值設(shè)定函數(shù)：

　　inline void setEuclideanFitnessEpsilon（double epsilon）{euclidean_fitness_epsilon_=epsilon；}

4 實(shí)驗(yàn)結(jié)果與結(jié)論

　　根據(jù)以上提出的算法，利用斯坦福大學(xué)實(shí)驗(yàn)室在不同坐標(biāo)系下獲得的兔子點(diǎn)云數(shù)據(jù)和實(shí)測(cè)的只有4個(gè)面數(shù)據(jù)的長(zhǎng)方體的點(diǎn)云數(shù)據(jù)進(jìn)行實(shí)驗(yàn)。

　　實(shí)驗(yàn)平臺(tái)為Windows 8.1 64位操作系統(tǒng)，VS2010 32位，PCL點(diǎn)云庫(kù)1.7.1。

　　如圖5、圖6所示，左上角和右上角為兩個(gè)不同坐標(biāo)系下的點(diǎn)云數(shù)據(jù)；圖5左下角的右上方為利用傳統(tǒng)ICP算法獲得的實(shí)驗(yàn)結(jié)果，右下角的右上方為基于KDTree改進(jìn)的ICP算法的實(shí)驗(yàn)結(jié)果；圖6左下角的上方圖為利用傳統(tǒng)ICP算法獲得的實(shí)驗(yàn)結(jié)果，右下角的上方圖為基于KDTree改進(jìn)的ICP算法的實(shí)驗(yàn)結(jié)果。

　　由以上比對(duì)可以明顯看出，傳統(tǒng)ICP算法獲得的結(jié)果有著明顯的匹配不到的地方，而利用改進(jìn)的ICP算法獲得的精細(xì)化匹配結(jié)果趨于完美，能夠?qū)崿F(xiàn)兩坐標(biāo)系下點(diǎn)云數(shù)據(jù)的精細(xì)化匹配。

　　參考文獻(xiàn)

　　[1] 王蕊，李俊山，劉玲霞，等.基于幾何特征的點(diǎn)云配準(zhǔn)算法[J].華東理工大學(xué)學(xué)報(bào)（自然科學(xué)版），2009，35（5）：768-773.

　　[2] 鄭德華，岳東杰，岳建平.基于幾何特征約束的建筑物點(diǎn)云配準(zhǔn)算法[J].測(cè)繪學(xué)報(bào)，2008，37（4）：464-468.

　　[3] 張政.點(diǎn)云數(shù)據(jù)配準(zhǔn)算法研究[D].濟(jì)南：山東大學(xué)，2008.

　　[4] YANG R， ALLEN P K. Registering， integrating， and building CAD models from range data[C]. 1998 IEEE International Conference on Robotics and Automation IEEE， 1998，4：3115-3120.

　　[5] 路銀北，張蕾，普杰信，等.基于曲率的點(diǎn)云數(shù)據(jù)配準(zhǔn)算法[J].計(jì)算機(jī)應(yīng)用，2008，27（11）：2766-2769.

　　[6] BESL P J， MCKAY N D. Method for registration of 3-D shapes[C]. Robotics-DL Tentative， International Society for Optics and Photonics， 1992： 586-606.

　　[7] ZINBER T， SCHMIDT J， NIEMANN H. A refined ICP algorithm for robust 3-D correspondence estimation[C]. 2003 International Conference on Image Processing， ICIP 2003， IEEE， 2003，3（2）：695-698.

　　[8] Zhang Zhengyou. Iterative point matching for registration of free-form curves and surfaces[J]. International Journal of Computer Vision，1994，13（2）：119-152.

　　[9] HORN B K P， HILDEN H M， NEGAHDARIPOUR S. Closed-form solution of absolute orientation using orthonormal matrices[J]. Journal of the Optical Society of America A， 1988， 5（7）： 1127-1135.