摘  要： 在三維激光點云數據配準的過程中，利用傳統Iterative Closest Point（ICP）算法搜索對應點對時速度慢，而且配準精細化程度低，遠達不到三維建模后期處理的要求。針對這一問題，提出一種基于KDTree改進的ICP算法以實現激光點云數據的快速精細化配準。通過實驗驗證算法的有效性和合理性，為后期模型重建過程中的三角網格化、曲面化、紋理映射提供強有力的理論和實踐基礎。

　　關鍵詞： 激光點云；ICP算法；KDTree；曲面化

0 引言

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

1 掃描設備及作業流程

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

　　設置好參數以后，分別在兩位置坐標系下對只有4個面的長方體實物進行掃描采集。然后，依次進行點云數據的去噪、稀疏采樣，由此獲得能足夠表達物體模型的點云數據。

　　圖1為使用ScanStation C10掃描儀的作業流程，圖2為經過去噪采樣處理過的數據并可視化的結果。

2 配準定義

　　點云配準簡單來說就是將從多個站點獲得的點云數據進行拼接，得到一個統一坐標系下的三維數據點集。它類似于數學上的映射問題，也就是說要先找到兩個點云數據集間的對應關系，然后將一個坐標系下的點云數據轉換到另一個坐標系下。

　　配準過程主要有以下兩個步驟：（1）尋找對應關系；（2）解算變換參數。即首先確定同名點對，然后解算旋轉矩陣R和平移矩陣T。

　　同名點對：同一個點在不同坐標系下的表達。

　　圖3所示為兩站掃描示意圖，在A、B兩處分別安放掃描儀對同一個物體進行掃描。在A處獲得坐標O1-x1y1z1下的點云數據M，在B處獲得坐標系O2-x1y1z1下的點云數據N，配準的目的就是將兩個坐標系O1-x1y1z1、O2-x1y1z1下的點云數據M和N轉換到同一個坐標系下。

　　對于從兩站采集到的點云集合M和N，Mi（X，Y，Z），Ni（x，y，z），且Mi、Ni為在不同坐標系下的同一點，嚴格來說，點云配準就是將全部來自兩個不同坐標系下的同名點對（Mi，Ni）滿足剛體變換（R，T），即：

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

　　式（1）稱作空間相似變換公式，它是點云配準的基本公式。由式（1）可解出同名點轉換參數，而后進行點云數據配準。

3 點云配準算法

　　目前，點云配準算法依據其采用的配準基元可將其分為無特征的配準和基于特征的配準[1]兩大類。

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

　　無特征的配準就是直接利用原始數據進行配準。此類算法中最為著名的是ICP（Iterative Closest Point）算法[6]，但該算法只適用于存在明確對應關系的點集，并且計算速度慢。為此，在其他傳統ICP算法[7]的基礎之上，提出基于KDTree[8]的改進ICP算法，包括基于KDTree搜索對應點對和矩陣變換參數的計算兩方面的內容。

　　3.1 傳統ICP配準算法

　　基本思路：在對應點云中搜尋最鄰近點對，利用此最鄰近點對求解剛體變換參數R、T，在這個過程中點對的搜尋和變換參數的求解都是迭代計算的。

　　算法步驟如下：

　　（1）令Ω為點云M和N的重疊域，設在Ω然數集N及其擴展情況，如正整數集Z+、n維實坐標中的任一點對應在M和N上的位置分別是Mi、Ni，初始迭代時兩個點集的初始變換參數是R0，T0。

　　（2）點集M中的每個點Mi，由初始變換參數最小為標準，求出新的變換參數R、T。

　　（3）根據找到的全部最近點對（mi，ni），求出兩個點集的變換參數R、T，并且以全部點對距離的平方和最小為標準，求出新的變換參數R、T。

　　（4）在相鄰兩次計算所得的距離平方和的差值小于給定的閾值時結束迭代，否則重復步驟（2）和（3）直至小于給定的閾值。

　　（5）根據最終得到的R、T將點云M映射變換到點云N的坐標系下，完成配準。

　　3.2 改進的基于KDTree的ICP算法

　　3.2.1 算法準備工作

　　由KDTree的算法原理可知，當鄰域點集中點數k為1時，搜尋點與鄰域點間建立一一映射關系。此時，搜索到的鄰域點是搜尋點與鄰域點集中距離最小的點。

　　該算法中要用到的變換矩陣利用四元素法[9]求解，過程如下：

　　（1）求解點集M、N的重心坐標O1、O2。

　　（2）點集M、N的重心化：

　　（3）構建矩陣Q：

　　（4）求解Q的最大特征值以及最大特征值對應的特征向量（w，m，n，p）。

　　（5）構造旋轉矩陣：

　　（6）解算平移向量T：

　　T=O2-R′O1(4)

　　3.2.2 算法實現步驟

　　（1）設點集M、N的部分區域分別為目標點集M′和參考點集N′。

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

　　（3）利用設置好的最小閾值距離Di，刪除Mm、Nm中錯誤的點對，并完成Mm、Nm的更新。

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

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

　　（6）重復步驟（2）~（5），求出Mm中每一點到Nm中的映射點對，以及相應的R、T。

　　（7）當最后的R、T滿足配準后，對應點對坐標間差值的閾值收斂條件|xm-xn|or|ym-yn|or|zm-zn|<ε時，結束循環，匹配成功；如果不滿足收斂條件，進行第m+1次迭代計算。

　　算法設計流程如圖4所示。

　　3.2.3 主要函數代碼介紹

　　最小閾值Di設定函數：

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

　　坐標差閾值設定函數：

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

4 實驗結果與結論

　　根據以上提出的算法，利用斯坦福大學實驗室在不同坐標系下獲得的兔子點云數據和實測的只有4個面數據的長方體的點云數據進行實驗。

　　實驗平臺為Windows 8.1 64位操作系統，VS2010 32位，PCL點云庫1.7.1。

　　如圖5、圖6所示，左上角和右上角為兩個不同坐標系下的點云數據；圖5左下角的右上方為利用傳統ICP算法獲得的實驗結果，右下角的右上方為基于KDTree改進的ICP算法的實驗結果；圖6左下角的上方圖為利用傳統ICP算法獲得的實驗結果，右下角的上方圖為基于KDTree改進的ICP算法的實驗結果。

　　由以上比對可以明顯看出，傳統ICP算法獲得的結果有著明顯的匹配不到的地方，而利用改進的ICP算法獲得的精細化匹配結果趨于完美，能夠實現兩坐標系下點云數據的精細化匹配。

　　參考文獻

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

　　[2] 鄭德華，岳東杰，岳建平.基于幾何特征約束的建筑物點云配準算法[J].測繪學報，2008，37（4）：464-468.

　　[3] 張政.點云數據配準算法研究[D].濟南：山東大學，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] 路銀北，張蕾，普杰信，等.基于曲率的點云數據配準算法[J].計算機應用，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.