基于KDTree改進的Super-4PCS+ICP算法在點云配準中的應用研究

夏軍勇,高睿杰,鐘 飛

(湖北工業大學機械工程學院,湖北 武漢 430068)

1 引 言

點云配準是三維模型重建的關鍵環節。在工程實際測量中,安裝在掃描儀內部的相機角度固定不動,要想獲得待測物體完整的點云數據,需要多次轉動不同的角度掃描,其中不同視角多次掃描就是點云配準的過程,用最精確的點云配準算法,對多個視角下獲得的不同點云片段進行配準處理[1]。從二十世紀末期開始,國外研究人員對點云配準技術做了大量的實驗研究,最開始配準的是圖像,現在主要是配準三維點云數據。

早在20世紀90年代,國外學者就將四元數求解矩陣的思想運用到點云配準上,提出用四元數法進行點云配準實驗[2]。Besl[3]在1992年提出了最近點迭代算法ICP,這個算法至今還是使用最多、應用最廣的算法。Chen[4]的改進方法是將兩個對應點切平面的距離來代替直線距離,這樣就可以讓點云重疊區域變多,成功實現了兩塊點云的配準。文獻[5]提出了一種基于法向量與其K近鄰法向量夾角不變特征的點云配準方法,可以加快點對應關系的選擇。2010年,文獻[6]用最小二乘法匹配重疊曲面,先估計局部和模板坐標系的剛體變換參數。Milos Prokop提出用KDTree結構來提高搜索效率,用四元數法和最小二乘法計算配準結果,這個方法明顯提高了配準的速度和精度[7]。朱延娟等提出將曲率相似約束條件應用到對應點的構造中,成功實現了配準[8]。儲珺等利用二維SIFT提取特征點實現點云配準,這個方法可以有效排除無效數據的影響[9]。

針對于傳統的點云配準在配準精度上還是不能達到一些高精度設備的要求。Super_4PCS算法常用于粗配準,ICP算法常用于精配準,本文將對Super_4PCS算法和ICP算法進行改進,通過對點云數據進行“先粗后精”處理,提高點云數據的配準精度。本文提出的“先粗后精”點云配準流程圖如圖1所示。

圖1 點云配準流程

2 點云粗配準

對于需要進行配準的兩片初始掃描點云,獲取手法及方式的差異會導致它們的初始位姿區別很大。而點云精配準的前提是兩片點云初始位姿良好,故在精配準前需要進行粗配準[10],使兩片點云的初始位姿大致對齊。粗配準使用的算法是基于隨機采樣一致性思想,其中使用比較多的就是SAC-IA[11]算法以及Super-4PCS[12]算法。

2.1 Super-4PCS算法

4PCS是將同一個平面內的四點作為基本單位點來尋找對應點,然后進行RANSAC迭代。其原理是根據同一平面內四點之間的仿射不變關系來確定對應點。假定其共面的四點B={a,b,c,d}可以確定滿足下列關系的兩個仿射不變量r1和r2:

(1)

(2)

得到了兩個仿射不變量后尋找對應的四點集,取目標點云里面的任意兩個點,計算可能會存在的交點e1,e2:

e1=q1+r1(q2-q1)

(3)

e2=q1+r2(q2-q1)

(4)

假如任意選取的一對點所計算出來的e1與另一對點計算出來的e2近似相等,如下圖3所示,就可以確定這兩對點之間構成的四點集U1是和基B對應的一個四點集。而對于剛體而言,剛體變換時對應點對之間的距離保持不變,故在點云旋轉平移變換過程中四點集還應該滿足下式條件:

(5)

(6)

Super-4PCS算法降低了4PCS的計算復雜度,該算法通過角度的約束手段減少了4PCS算法中無效對的產生。同時采用了一種柵格化的方法,將計算距離的二次復雜度降低了,從而極大地提高算法的效率。

2.2 基于KDTree樹改進的Super-4PCS點云粗配準

本文點云數據由三維激光掃描儀FreeScan_UE自動掃描輪廓獲得,數據量很大,在對點云處理過程中會耗費大量的時間,極大地影響了點云配準的效率。故本文先對掃描得到的數據進行去噪處理,在保留特征點的前提下進行點云精簡,極大地減少了點云數量。然后提出KDTree樹搜索對應點,結合提取特征點[13]方法對點云的粗配準進行改進,解決計算量巨大的問題。

本文將選取局部區域法向量變化較大的點集來選為特征點集。假定點云區域內的一點pi處法向量的特征度用k近鄰點法向量和此局部區域本身的法向量夾角的算術平均值來表示:

(7)

式中,θij是近鄰點pj處的法向量與點pi之間的夾角。

選取合適的閾值ε1,因為fi<ε1時會出現局部區域特征不明顯的問題,故選取所有fi>ε1的點,而fi<ε1的全部不考慮,去除這不部分無用點。對于其中任意一點pm,若滿足:

f(pm)=max[f(pm1),f(pm2),…f(pmk)]

(8)

其中,點pm就被設定為局部區域的一個特征點。

本文改進方法的具體步驟為:

1)對初始散亂點云建立KDTree,加速鄰域點的查找。通過三維激光掃描儀獲得的點云數據是散亂的,點云的分布沒有規律,無法用數學模型來描述,故無法對點云坐標信息進行查詢,所以需要建立拓撲信息使得掃描得到的點云數據變得更有規律,從而實現可查詢的功能和要求,而KDTree就是建立拓撲關系中應用最為廣泛的一種方法。

2)對散亂點云數據按照設置的邊長l mm進行分塊,使其在邊長為l mm的正方形小局部區域內都能獲得相應的法向量。

3)通過計算指定的夾角算術平均值得出各分塊區域的特征度,并將特征度大于給定閾值的點保留下來。

4)選取任意保留點計算其鄰近點的特征度,若保留點的特征度為最大值,則此點即為此局部區域的特征點,直至選完所有的特征點。

5)再使用Super-4PCS算法中的隨機采樣一致性算法RANSAC進行迭代來確定匹配關系,實現點云粗配準,求解旋轉平移矩陣。

2.3 點云粗配準實驗及分析

為驗證本文方法改進的有效性,本文將在AMD Ryzen 7 2700,機帶RAM為16GB的Windows10操作系統上基于MATLAB平臺對標準的點云測試實例斯坦福兔點云、龍點云進行算法有效性驗證實驗。其中源點云為深灰色顯示,目標點云淺灰色顯示。各實例粗配準實驗前初始位姿如圖2所示。

圖2 粗配準實驗前初始位姿

對其使用Super-4PCS算法及本文改進算法進行粗配準,配準后得到的效果圖如圖3所示。

圖3 粗配準結果

本文方法成功完成了源點云與目標點云的粗配準。兔點云和龍點云的點云數據殘缺較大,輪廓特征不明顯,粗配準效果一般。但相對于Super-4PCS算法,明顯可以看出本文方法配準效果更優。

Super-4PCS與本文方法的粗配準實驗數據結果如上表1所示,分別列出了粗配準時間、粗配準誤差。從表中可以看出,兩種方法在配準時間相差無幾的情況下,應用本文方法的兩個實例兩片初始點云已經大部分重疊,成功實現了源點云與目標點云的粗配準,且配準誤差相對于Super-4PCS算法明顯降低,配準精度有所提高。本文方法對標準數據庫實例斯坦福兔點云進行點云配準時,其配準精度相對于Super-4PCS算法最大提高了38.71 %,而龍點云的配準精度提高了11.8 %。數據證明本文改進方法的可行性。滿足后續使用精配準算法對于初始位姿的要求。

3 點云精配準

上一節介紹了兩片散亂點云的粗配準,從實驗結果可以看出,散亂點云經過粗配準后,兩片點云的初始位姿發生了很大變化,距離變近,方向對齊,基本實現重疊。但是對于初始位姿很差的點云,錯位等問題依然存在,粗配準的精度還遠遠達不到我們的要求。故在此基礎上需要進行精配準來提升配準精度。

3.1 ICP算法

(9)

然后求解使得上述誤差函數值最小的剛體變換向量q=[qR|qR]T,記作(q,d)=?(P,Y),上式中d為相應的均方誤差函數,即d=f(q)。算法將求解得到的剛體變換作用到第一片點云數據上,記作q(P)。ICP算法迭代地進行該操作直到滿足某一設定的收斂準則。

3.2 基于KDTree樹改進的ICP點云精配準

1)減少無用點集,本文在進行實驗之前,用三維掃描儀獲取點云時,會將旁邊環境物刷漆涂黑,使周圍物體不被掃出,避免出現不必要的噪聲點。

2)對應點的選擇采用的是KDTree結構搜索歐式距離最小的點。匹配點是選擇源點云中與其歐式距離最近的點,設定距離閾值,剔除錯誤點對。因為經過粗配準后的兩片點云初始很近,不需要再用特征點進行快速匹配,而使用KDTree臨近查找可以極大地提高搜索效率。

3)點加權采用所有點對常數加權均勻分配,并取權值為常數1,因為本文精確配準之前使用了粗配準,初始位姿保持極好,使用常數為1的加權可以滿足條件且便于計算。

4)去除算術平均值以下的錯誤點對,只保留處于閾值范圍內的對應點。誤差函數選取點到面的距離平方和。

5)將目標誤差函數最小化采用自適應閾值的方式,去中心化后對斜方差矩陣進行SVD分解[14]和ICP算法計算[15-17],求解當前旋轉矩陣和平移向量的誤差函數是否收斂,若誤差函數不收斂,則回到KDTree搜索對應點繼續計算,直至迭代收斂,算出最后的旋轉平移矩陣。

其中,SVD分解具體原理如下所示為:

對于任意的矩陣A∈Rm×n,存在矩陣U=[u1u2…un]∈Rm×n和V=[u1u2…un]∈Rm×n使得滿足:

(10)

式中,∑r=diag(σ1…σn)且σ1≤σ2≤…?σn。

基于SVD分解改進的ICP算法的計算步驟如下示:

首先計算目標數據點集P和源數據點集Q的重心坐標;

其次計算源數據點集和目標數據點集中點坐標相對于重心坐標的變化量;

(11)

(12)

然后利用其各自的重心變化量構造SVD分解矩陣;

(13)

最后計算旋轉平移矩陣。

(14)

R=VUT

(15)

3.3 點云精配準的實驗及分析

為驗證本文改進的點云精配準方法的有效性和可行性,繼續使用標準數據庫中的兔點云和龍點云及非標準實例籃球進行有效性驗證。其中深灰色點云為源點云,淺灰色點云為目標點云,使用ICP算法和本文改進精配準算法得到的實驗結果如圖4所示。

圖4 精配準后結果

從效果圖上看,相較于ICP算法,本文方法對點云精配準的效果更加好。但由于兩點云的配準誤差非常細微,故對兩種方法的實驗數據進行統計,作出詳細對比,如表2所示。

表2 精配準結果數據

由上述可知,本文方法進行精配準后,相較于ICP算法來說配準誤差更小,配準精度得到了提高,表明了本文方法精配準的必要性。使用本文方法配準標準例子兔點云和龍點云的精度相對于傳統ICP算法精度提高了41.77 %和68.12 %,證明了本文改進方法的有效性。

兔點云旋轉平移矩陣為:

龍點云旋轉平移矩陣為:

籃球點云旋轉平移矩陣為:

4 結 語

本文利用“先粗后精”的思想對兩點云進行配準。首先對Super-4PCS算法改進優化,進行粗配準。配準精度最大提高了38.71 %。然后在精配準時用KDTree臨近查找,常數加權及對誤差函數采用自適應閾值等方法對ICP算法進行優化,分別使用標準數據庫和非標準例子進行實驗驗證,實驗結果表明本文方法精配準的效果相較于ICP算法的配準效果有顯著提升,配準精度相較于ICP算法最大提升了68.12 %。因此本文的點云配準方法對后期點云模型重建有一定的參考價值。