基于K-D 樹加速的大型點云配準(zhǔn)算法

吳振慧，王彩余

（1. 揚州市職業(yè)大學(xué) 電子工程學(xué)院， 江蘇 揚州 225009； 2. 揚州廣潤機械有限公司 技術(shù)部， 江蘇 揚州 225000）

在工業(yè)生產(chǎn)領(lǐng)域，尤其是自動化裝配和生產(chǎn)領(lǐng)域，受測量方式和被測物體集合形狀的限制，對于同一物體，需要在不同視角下掃描得到兩組或多組不同的點云數(shù)據(jù)。為了計算在此過程中物體或設(shè)備的運動，或是進行點云拼接，都需要找到兩個點云之間的對應(yīng)空間位置關(guān)系，進行點集與點集坐標(biāo)系間的轉(zhuǎn)換，實現(xiàn)配準(zhǔn)，可見點云配準(zhǔn)至關(guān)重要。

從物理學(xué)角度分析，點云配準(zhǔn)即為計算源點云和目標(biāo)點云之間的旋轉(zhuǎn)矩陣R 和平移矩陣T，通過這兩個矩陣即可描述三維空間中兩個點云的相對位置關(guān)系[1]。為了提高點云對于物體的描述精度，目前的掃描儀通常采用小點間距、小線間距的陣列式掃描。對于同一物體，越小的點、線間距意味著掃描出的點云中包含的點數(shù)據(jù)越多。然而，針對大型點云數(shù)據(jù)，其配準(zhǔn)算法要在保證精度的基礎(chǔ)上，提高其速度，若采用一般的配準(zhǔn)算法，計算耗時非常大，難以在工業(yè)界使用。因此，本文提出基于迭代最近點（Iterative Closest Point，ICP），結(jié)合預(yù)處理和K-D 樹的大型點云配準(zhǔn)算法，以期在滿足精度要求的同時，提高計算速度。

1 迭代最近點算法

1.1 迭代最近點算法步驟

迭代最近點（ICP）算法[2]是目前使用最廣泛的點云精配準(zhǔn)算法之一。ICP 算法的核心步驟為：在目標(biāo)點云（Target）中尋找源點云（Source）的最近點，通過找到的對應(yīng)最近點，使用最小二乘法構(gòu)建目標(biāo)函數(shù)，計算參數(shù)矩陣R 和T，經(jīng)過一步計算后，將源點云進行R 和T 的位移變化，再次尋找最近點，如此迭代直到目標(biāo)點云和源點云的距離小于閾值。

ICP 算法的目標(biāo)函數(shù)如下：

ICP 算法的步驟如下：

第一步，在目標(biāo)點云 Q 中計算源點云 P（k）中每一點的最近點，k 表示迭代次數(shù)。

第二步，計算能使得P（k）和其對應(yīng)最近點標(biāo)準(zhǔn)誤差E 最小的旋轉(zhuǎn)矩陣R 和平移矩陣T，其標(biāo)準(zhǔn)誤差計算依據(jù)公式（1）。

第三步，使用第二步中計算得到的旋轉(zhuǎn)矩陣R 和平移矩陣 T，移動 P（k）使其成為 P（k+1）。

第四步：如果 P（k+1）和 Q 之間的標(biāo)準(zhǔn)誤差 E 小于閾值，則迭代停止，否則重復(fù)第一步至第三步。

1.2 迭代最近點算法計算方法

迭代最近點算法的主要問題在于：（1）如何在目標(biāo)點云Q 中尋找源點云P（k）中某一點對應(yīng)的最近點；（2）如何根據(jù)一一對應(yīng)的最近點，計算得到旋轉(zhuǎn)矩陣R 和平移矩陣T；（3）如何將每一步計算得到的旋轉(zhuǎn)矩陣和平移矩陣合成為一個參數(shù)矩陣。

1.2.1 最近點的計算

針對問題（1），如果點云數(shù)據(jù)量較小，可以采用暴力搜索的方式尋找最近點。然而，針對大型點云該方法會消耗大量時間，并不適用。針對此，本算法采用了去質(zhì)心和降采樣的預(yù)處理方式。去質(zhì)心即在計算開始時，將點云的坐標(biāo)分別減去其質(zhì)心坐標(biāo)，保留相對位置，以大大減少搜尋最近點的時間。過多的對應(yīng)點對于計算準(zhǔn)確性的提高作用甚微，只需采集少部分的特征點即可進行計算，故本文采用讀取點云后即進行30 %的降采樣操作方式，隨機均勻刪除70 %的點，在進行計算時，還可人為或隨機選取更小一部分的特征點作為迭代依據(jù)。因此，公式（1）可轉(zhuǎn)化為：

其中：p 表示源點云P 中被選擇作為特征點的點云，稱為特征源點云；q 表示目標(biāo)點云Q 中與p 對應(yīng)的最近點，稱為特征目標(biāo)點云。

要注意的是，在計算旋轉(zhuǎn)矩陣R 和平移矩陣T 時，可以僅使用很少一部分的特征點，但在判斷迭代是否停止時，需要對所有點求其平均標(biāo)準(zhǔn)誤差E。公式（2）中的N 不是源點云所有點的個數(shù)，而是特征源點云所有點的個數(shù)。

1.2.2 參數(shù)矩陣的求解

奇異值分解算法計算效率高，計算結(jié)果準(zhǔn)確，是針對問題（2）的經(jīng)典計算方法。因此，本算法采用奇異值分解的方式并依據(jù)公式（2）求解目標(biāo)函數(shù)，具體計算過程如下：

第一步，計算兩部分特征點云的質(zhì)心：

第二步，去質(zhì)心化：

第三步，構(gòu)造協(xié)方差矩陣H：

第四步，使用奇異值分解矩陣H，得到U、S、V：

第五步，計算旋轉(zhuǎn)矩陣R：

第六步，計算平移矩陣T：

然后，更新特征源點云p 和源點云P 中所有點的坐標(biāo)：

至此，完成一步迭代。當(dāng)標(biāo)準(zhǔn)誤差E 小于閾值時，迭代停止，配準(zhǔn)結(jié)束。根據(jù)工業(yè)現(xiàn)場定位要求，閾值設(shè)置為0.5 mm。

1.2.3 點云坐標(biāo)的齊次化

針對問題（3），參考運動學(xué)原理[3]，可以引入齊次坐標(biāo)，通過齊次變換矩陣M 統(tǒng)一表示旋轉(zhuǎn)矩陣R 和平移矩陣T，

此時，一個三維空間的坐標(biāo)表示為齊次坐標(biāo)的形式，

可以發(fā)現(xiàn)齊次坐標(biāo)變化公式（15）與笛卡爾坐標(biāo)變化公式（12）等價，

根據(jù)點云繞固定坐標(biāo)系的齊次變換是齊次變換矩陣的順序左乘的定理，可以得到最終的齊次變換矩陣為：

這里Mi表示第i 次迭代計算得到的齊次變換矩陣。

2 最近點搜索加速方法

針對大型點云配準(zhǔn)的場景，傳統(tǒng)ICP 算法的弊端在于，即使采用了去質(zhì)心、降采樣的預(yù)處理方式，如果對于每一個特征源點云中的點，都采用暴力搜索的方式尋找最近點，也需要多次遍歷目標(biāo)點云的所有點。針對這一弊端，楊秋翔等人基于法矢夾角對ICP 算法進行了改進[4]，楊小青等人基于法向量進行了改進[5]，朱玉梅等人提出基于DNSS 與點到平面的ICP 結(jié)合的方法[6]?？紤]大型點云配準(zhǔn)這一場景，本文采用K-D 樹方法加速最近點的搜索。

2.1 K-D 樹算法原理

K-D 樹是K-Dimensional 樹的簡稱，即為K維空間的樹，是一種對k 維空間的點儲存一遍從而可以進行快速搜索的樹形數(shù)據(jù)結(jié)構(gòu)[7]。K-D 樹主要應(yīng)用于多維空間中的最鄰近搜索和范圍搜索。K-D 樹中每個結(jié)點數(shù)據(jù)類型描述見表1。

表1 K-D 樹結(jié)點數(shù)據(jù)類型

2.1.1 K-D 樹的構(gòu)建

K-D 樹的構(gòu)建過程如下：

輸入：k 維空間數(shù)據(jù)集 T={x1，x2，…，xN}，其中

輸出：K-D 樹

（1）開始：構(gòu)造根結(jié)點，根結(jié)點對應(yīng)于包含T的k 維空間的超矩形區(qū)域。選擇x（1）作為切分的坐標(biāo)軸，以T 中所有數(shù)據(jù)x（1）坐標(biāo)的中位數(shù)作為切分點。分割超平面通過切分點并與坐標(biāo)軸x（1）垂直，將根結(jié)點對應(yīng)的超矩形區(qū)域切分為兩個子區(qū)域，即根結(jié)點的左樹Left 和右樹Right。左樹對應(yīng)坐標(biāo)x（1）小于切分點的左子空間，右樹對應(yīng)于坐標(biāo) x（1）大于切分點的右子空間。

（2）重復(fù)：對深度為 j 的結(jié)點，選擇 x（l）作為切分的坐標(biāo)軸，l=j（modk）+1 即為Split，以該結(jié)點的區(qū)域中所有數(shù)據(jù)x（l）坐標(biāo)的中位數(shù)作為切分點。分割超平面通過切分點并與坐標(biāo)軸x（l）垂直，將該結(jié)點對應(yīng)的超矩形區(qū)域切分為兩個子區(qū)域，即該結(jié)點的左樹Left 和右樹Right。左樹對應(yīng)坐標(biāo)x（l）小于切分點的左子空間，右樹對應(yīng)于坐標(biāo)x（l）大于切分點的右子空間。

（3）停止：直到結(jié)點的左樹和右樹沒有數(shù)據(jù)時，分割停止，K-D 樹形成。

K-D 樹構(gòu)建的時間復(fù)雜度為 O（log2n），并且對于迭代最近點算法，僅需要在第一次迭代之前構(gòu)建K-D 樹即可，無須在每次迭代中反復(fù)構(gòu)建，因此其構(gòu)建時間對于算法的整體時間影響很小。

2.1.2 K-D 樹查找最鄰近結(jié)點

K-D 樹查找最鄰近結(jié)點，即完成ICP 算法中在目標(biāo)點云中查找源點云中的點對應(yīng)最近點的過程，其查找過程如下：

（1）從根結(jié)點開始，判斷輸入點（即源點云中的點）與該根結(jié)點的位置關(guān)系，如果其x（l）坐標(biāo)小于根結(jié)點的x（l）坐標(biāo)，說明輸入點在根結(jié)點的左樹，則進入左子結(jié)點；如果其x（l）坐標(biāo)大于根結(jié)點的x（l）坐標(biāo)，說明輸入點在根結(jié)點的右樹，則進入右子結(jié)點。

（2）重復(fù)步驟（1）不斷下移，直至移動到葉結(jié)點，將該葉結(jié)點作為“當(dāng)前最近點” 。

（3）從“當(dāng)前最近點” 上移，對于每個經(jīng)過的結(jié)點，進行如下操作：①如果該結(jié)點比“當(dāng)前最近點” 更接近輸入點，則將其變?yōu)椤爱?dāng)前最近點” ；②如果該結(jié)點另一側(cè)子結(jié)點有更近的點，則進入其另一側(cè)子結(jié)點并不斷向下尋找。

（4）當(dāng)搜索回到根結(jié)點時，搜索完成，找到最鄰近結(jié)點。

2.2 K-D 樹算法舉例

為了直觀地展示K-D 樹搜索臨近點的過程，圖1 給出了一個二維平面上K-D 樹的構(gòu)建實例，其中包含 6 個點 （2，3），（5，4），（9，6），（4，7），（8，1），（7，2）。

圖1 K-D 樹的構(gòu)造

假設(shè)輸入的點為（2，4.5），該點標(biāo)注如圖2，需要在上述6 個點中尋找與其最近的點。

圖2 輸入點空間位置（星號位置）

首先，從根結(jié)點（7，2）開始搜索輸入點（2，4.5），其過程是：

（1）輸入點X 值2 小于根結(jié)點X 值7，因此訪問左樹，當(dāng)前結(jié)點是（5，4）。

（2）輸入點Y 值 4.5 大于當(dāng)前結(jié)點 Y 值 4，因此訪問右樹，當(dāng)前結(jié)點是（4，7）。

（3）當(dāng)前結(jié)點已經(jīng)是葉結(jié)點，向下搜索過程結(jié)束。

此時記錄搜索路徑為＜（7，2），（5，4），（4，7）＞，然后需要上移回溯搜索路徑：

（1）回溯到（4，7），“當(dāng)前最近點” 設(shè)置為（4，7）。計算（4，7）到輸入點（2，4.5）的距離為 d1，圖 2 中圓圈的半徑即為d1。

（2）回溯到（5，4），由于（5，4）到輸入點（2，4.5）的距離d2小于d1，因此“當(dāng)前最近點” 更新為（5，4）；另外，由于輸入點到（5，4）分割線的距離小于 d1，說明（5，4）的另一側(cè)也需要查找，添加其另一側(cè)的點（2，3）后，搜索路徑變?yōu)椋迹?，2），（2，3）＞。

（3）回溯到（2，3），由于（2，3）到輸入點（2，4.5）的距離 d3小于 d2，因此“當(dāng)前最近點” 更新為（2，3）。

（4）回溯到（7，2），由于（7，2）到輸入點（2，4.5）的距離d4大于d3，因此“當(dāng)前最近點” 無須更新；另外，由于輸入點到（7，2）分割線的距離大于d3，說明（7，2）的另一側(cè)無須查找。

（5）所有搜索路徑上的點均被回溯，回至根結(jié)點結(jié)束，（2，4.5）的最近點輸出為（2，3）。

最近點搜索結(jié)果如圖3 所示。

圖3 輸入點和其最近點

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

本文算法采用C++語言編寫，在Visual Studio 2015 中編譯和運行，采用OpenGL 三維圖像庫進行可視化交互。在此界面中，可以自由調(diào)整視角、縮放物體及框選物體上的點作為特征點等，并且程序可判斷所選取點是否可見。本算法認(rèn)為框選區(qū)域中僅正面可見的點為選擇的特征點。

本文選擇工業(yè)零部件的掃描點云作為實驗數(shù)據(jù)，目標(biāo)點云和源點云均以txt 文件形式存儲，存儲形式為：以v 開頭的行表示一個點（Vertex）的三維坐標(biāo)，以f 開頭的行表示一個三角面片（Face）頂點的編號。點云文件的存儲示例如圖4所示。

圖4 點云文件存儲形式

讀取點云文件后，以計算機圖形學(xué)中典型的Half-Edge 結(jié)構(gòu)進行點云模型的存儲。在Half-Edge 結(jié)構(gòu)下，可以很方便地遍歷一個點的臨近點和臨近邊，Half-Edge 結(jié)構(gòu)如圖5 所示，其中，每個“半邊” 有對應(yīng)的start（起始點）、twin（孿生邊）、next（下一條邊）、previous（前一條邊）、left face（左側(cè)邊）。通過這種數(shù)據(jù)結(jié)構(gòu)即可構(gòu)造完整的點云模型。

圖5 Half-Edge 數(shù)據(jù)結(jié)構(gòu)

讀取并顯示目標(biāo)點云和源點云模型如圖6 所示。圖6 顯示其形狀幾乎一致，但位置相差甚遠。

圖6 目標(biāo)點云和源點云模型

源點云信息輸出如圖7 所示，其中包括頂點個數(shù)（453 089）、邊個數(shù)（1 350 160）、面?zhèn)€數(shù)（895 959）等。由于其頂點個數(shù)多達45 萬余，屬于大型點云，符合本算法的使用范圍。

圖7 源點云相關(guān)信息

由系統(tǒng)隨機選取 3 000、1 000、500 個點作為特征點進行計算，或手動選取如圖8 所示的特征點，其實驗結(jié)果如表2 所示。表2 中序號4、5 對應(yīng)的特征點即為圖8 中左右選擇的區(qū)域。

圖8 手動選取特征點（白色區(qū)域）

表2 實驗結(jié)果

從表2 不難發(fā)現(xiàn)，采用系統(tǒng)隨機選擇特征點的方式，可以在使用更少的特征點，減少計算時間的前提下，保證配準(zhǔn)誤差小于0.1 mm。若采用手動選取特征點，由于點云數(shù)目眾多，容易選取較多的特征點，因此計算時間較長。同時，因為選取的特征點分布較集中，所以誤差比隨機選擇特征點時大，與本算法原理相符。但不論是隨機還是手動選擇特征點，都能保證在30 s 內(nèi)完成誤差小于0.2 mm 的配準(zhǔn)，速度和精度均滿足工業(yè)要求。

為驗證采用K-D 樹加速的優(yōu)越性，本文將其與傳統(tǒng)的一一暴力搜索最近點的方法進行精度和速度比較。原點云數(shù)目高達45 萬多，本文降采樣后在僅有1 萬點左右的稀疏點云上進行傳統(tǒng)的ICP 配準(zhǔn)。選取所有點作特征點，完成配準(zhǔn)的計算時間為24.94 s，配準(zhǔn)誤差為0.075 mm。由此可見，基于K-D 樹加速的ICP 配準(zhǔn)算法，能夠在保證配準(zhǔn)精度的前提下，大大提高ICP 算法的配準(zhǔn)速度，是適用于大型點云配準(zhǔn)的可靠方法。

4 結(jié) 論

本文基于ICP 算法，結(jié)合各種預(yù)處理方式和K-D 樹數(shù)據(jù)結(jié)構(gòu)，提出了針對大型點云的配準(zhǔn)算法。該算法解決了傳統(tǒng)ICP 算法不適用于大量數(shù)據(jù)計算的弊端，在保證計算精度的前提下，大大提高了計算速度，并且可視化界面交互性強，可以實時顯示點云模型和配準(zhǔn)結(jié)果。大型點云數(shù)據(jù)的配準(zhǔn)實驗表明，該算法魯棒性強，速度靈敏，精確度高，可用于工業(yè)現(xiàn)場。