基于幾何一致性加權(quán)的二部圖點云配準(zhǔn)方法

夏坎強

(上海理工大學(xué)機(jī)械工程學(xué)院，上海 200093)

0 引言

近年來，隨著3D 采集技術(shù)的迅猛發(fā)展，三維傳感器逐漸從實驗室走進(jìn)了大眾的視野，例如各種類型的3D 掃描儀、激光雷達(dá)與RGB-D 相機(jī)。與2D 圖像相比，三維數(shù)據(jù)可以表達(dá)更加豐富的幾何、形狀等信息并能夠以更好的視角描繪物體整體面貌。點云配準(zhǔn)是將具有重疊部分、在不同視角下采集到的點云，通過變換矩陣的轉(zhuǎn)化，統(tǒng)一到同一坐標(biāo)系的算法。點云配準(zhǔn)在不同領(lǐng)域有很多應(yīng)用，如三維重建[1]、三維定位[2]與姿態(tài)估計[3]等。

傳統(tǒng)的點云配準(zhǔn)算法是由Besl 等[4]提出的迭代最近點(ICP)算法，該算法采用點對點的距離度量，通過迭代的方式最小化兩點云對應(yīng)點間的歐式距離解決配準(zhǔn)問題。李繞波[5]等提出一種基于對偶四元素描述的配準(zhǔn)方法，利用線面的幾何約束關(guān)系構(gòu)建空間變換函數(shù)并利用最小二乘法計算變換矩陣，該方法對面特征較少的點云效果不佳。Segal[6]等提出一種廣義的ICP，允許點到點和點到面變量中包含任意協(xié)方差矩陣，其基本思想是將點云看作一個采樣的二維流形，用局部表面法線來表示點云。王珊[7]等提出一種基于特征點的匹配算法，對特征點使用雙向最近鄰距離比匹配并根據(jù)不同點云設(shè)置收斂閾值，但算法對收斂閾值較為敏感。李新春[8]等提出一種基于鄰域特征點提取與匹配的配準(zhǔn)方法，提高了特征點的分辨率，但該算法配準(zhǔn)時間較長。陳強[9]等提出一種基于特征空間匹配的點云配準(zhǔn)算法，利用PointNet 模型提取特征空間，通過RANSAC 與ICP 完成粗配準(zhǔn)與精配準(zhǔn)，但該算法設(shè)計參數(shù)較多，配準(zhǔn)流程較繁瑣。張旭春等[10]提出一種基于多尺度集距離約束的配準(zhǔn)方法，通過在FPFH 算法提取不同尺度下的特征點并結(jié)合距離約束條件完成點云配準(zhǔn)，但該算法運行速度較慢。

本文提出一種基于圖的優(yōu)化配準(zhǔn)方法，實現(xiàn)了對點云中錯誤對應(yīng)關(guān)系的篩除以及變換矩陣的求解。結(jié)合點云對應(yīng)點對的幾何一致性，在幾何空間與特征描述符的復(fù)合空間上度量特征點對的相似性。同時，從特征點對全局配準(zhǔn)的角度出發(fā)，將點云配準(zhǔn)問題轉(zhuǎn)化為二部圖的匹配任務(wù)并應(yīng)用Kuhn-Munkres(KM)[11]算法求解正確的對應(yīng)關(guān)系，最后通過SVD 分解計算變換矩陣。

1 方法原理

1.1 方法框架

本文算法流程如圖1 所示。首先，利用內(nèi)部形狀描述子(ISS)算法檢測待配準(zhǔn)點云特征點并計算其3DSC 特征描述符以編碼特征點局部信息。同時，利用快速點對特征直方圖(FPFH)構(gòu)建點云初始候選對應(yīng)點對并利用幾何空間一致性確定置信度較高的基準(zhǔn)點對，以此計算后續(xù)匹配點對的幾何一致性系數(shù)。接著，將點云對應(yīng)點匹配任務(wù)描述為一個代價函數(shù)，它在3DSC 特征和歐式幾何空間上對特征點的全局相似性進(jìn)行建模，然后，利用KM 算法優(yōu)化全局代價函數(shù)用以確定特征點對應(yīng)關(guān)系，最后，利用奇異值分解(SVD)估計最優(yōu)變換，實現(xiàn)點云配準(zhǔn)。

圖1 算法流程圖

1.2 幾何空間一致性系數(shù)

傳統(tǒng)特征點對使用歐氏距離確立對應(yīng)關(guān)系，不僅忽略了3D剛性變換的獨特性，而且會在配準(zhǔn)中帶來錯誤的匹配。由圖2 中可知，基礎(chǔ)對應(yīng)點對間的距離是固定的，但錯誤匹配點對P3與Q3間的距離與基礎(chǔ)匹配點對(P1,Q1)的距離相同。故單純依靠兩點云間的歐氏距離會給匹配帶來歧義，進(jìn)而影響最終的匹配效果。幾何空間一致性則是利用點云自身特征點間的距離恒定(如P1P2)，其在經(jīng)過變換后仍滿足自身特征點對距離恒定的特點(如Q1Q2)。

圖2 幾何空間一致性示意圖

幾何空間一致性加權(quán)依賴于匹配點云的預(yù)對應(yīng)假設(shè)，本文選用快速特征點直方圖(FPFH)描述子進(jìn)行預(yù)匹配點云匹配對(p,q)，然后通過幾何空間一致性篩選候選正確對應(yīng)點對，如圖2 所示。當(dāng)兩組匹配點對(p1,q1),(p2,q2)滿足式(1)時，將其列為候選匹配點對。式⑴中，p1,p2表示源點云中的特征點，q1,q2是目標(biāo)點云中的特征點?！?* ‖2表示向量2范數(shù)。

對于三維空間而言，滿足式⑴的候選匹配點對可能會有歧義性，因此需要統(tǒng)計滿足公式的對應(yīng)點對的次數(shù)。由于正確的匹配對在計算過程中會被多次使用形成穩(wěn)定的多邊形共面幾何結(jié)構(gòu)，所以當(dāng)統(tǒng)計計數(shù)大于計數(shù)閾值時，將其定義為基礎(chǔ)對應(yīng)點對。

通過計算待匹配點對(pi,qj)與置信度較高的基礎(chǔ)對應(yīng)點對(pt,qt)之間的幾何一致性，可以得到該對匹配點的內(nèi)在幾何一致性度量。具體來說，通過測量源點云中的特征點與基準(zhǔn)點的距離與其在目標(biāo)點云對應(yīng)的點對距離的均值差來計算幾何空間相關(guān)性gpq，如式⑵。

其中，gpq表示點云匹配對(p,q)的幾何空間相關(guān)性，|c|是基準(zhǔn)對應(yīng)點對的個數(shù)，(xk,yk)是源點云與目標(biāo)點云的基礎(chǔ)對應(yīng)點對，dpqk是源點云中的點對(xk,yk)與目標(biāo)點云中的點對(yp,yk)的長度差。

由于幾何相關(guān)性與3DSC 的特征描述符量綱不一致，本文選用威爾式公式將特征點p與q的幾何相關(guān)度量映射至0-1 區(qū)間以此對特征點3DSC 描述符歐氏距離進(jìn)行加權(quán)表示，幾何一致性系數(shù)ψγ(p,q)如式⑶。其中，ψγ(p,q)表示點云匹配對(p,q)的幾何一致性系數(shù)，γ代表威爾式公式核寬。

1.3 代價函數(shù)構(gòu)建

從點云特征點對的整體對應(yīng)關(guān)系出發(fā)，構(gòu)造了一個用于特征點匹配的全局代價函數(shù)Ecos t來表征對應(yīng)點匹配的正確性，該函數(shù)主要由匹配代價與懲罰代價組成，如式⑷。匹配代價表征源點云與目標(biāo)點云對應(yīng)關(guān)鍵點相似性度量，而懲罰代價表征未匹配點對的數(shù)量。

對于匹配代價LMatch_cos t，其由特征點描述符歐式距離與幾何空間一致性加權(quán)和構(gòu)成，如式⑸所示。

其中，M是兩點云的對應(yīng)匹配集。ED3dsc(p,q)表示匹配點對(p,q)的3DSC 描述符歐氏距離。懲罰代價LPenalty_cos t由未匹配點個數(shù)以及懲罰權(quán)重表示，|φ|是未匹配特征點個數(shù)，Wp是懲罰權(quán)重，如式⑹。通過最小化代價函數(shù)Ecos t，可以得到匹配集{M,φ}*，如式⑺。

1.4 基于KM算法的代價函數(shù)優(yōu)化

尋找源點云與目標(biāo)點云特征點對的正確對應(yīng)關(guān)系可以建模為二部圖的最優(yōu)匹配問題。在加權(quán)二部圖G=(S,T,E)中，兩個不相交的點集結(jié)點視為源點云S與目標(biāo)點云T的特征點，結(jié)點間的權(quán)值大小E可以由特征點描述符歐式距離與幾何空間一致性加權(quán)表征。KM算法的求解需要滿足兩點集結(jié)點個數(shù)一致，故假設(shè)源點云中與目標(biāo)點云中分別檢測到m與n個特征點，在特征點個數(shù)m與n不一致時，需要添加|m-n| 個虛擬節(jié)點Nv來滿足算法的運行。每條邊e(p,q) 的大小表示源點云S中的p特征點與目標(biāo)點云T中的q特征點的復(fù)合距離即幾何一致性加權(quán)的3DSC 歐式距離。為防止異常值對算法的影響，設(shè)置未匹配點對閾值Tcd，Tcd的計算公式如式⑻。通過計算源點云S與目標(biāo)點云T中所有特征點對的匹配矩陣Mcd的均值μcd與標(biāo)準(zhǔn)差σcd來獲得Tcd，t設(shè)置為1。確定未匹配點對閾值Tcd后，對應(yīng)邊權(quán)值e(p,q)計算如下：

當(dāng)Tcd=2Wp時，所有邊的最小權(quán)值和與代價函數(shù)相差常數(shù)值，如式⑾。因此，代價函數(shù)最小值的求解可以通過加權(quán)二部圖的KM 算法得出。其中，被選中邊的權(quán)重e*(p,q)＜Tcd最終構(gòu)成源點云與目標(biāo)點云的對應(yīng)匹配集合M*，同時未匹配點集φ*也被確定。圖3 展示了KM 算法求解過程的簡單示例，其中Mgd表示所有特征點對的幾何一致性系數(shù)矩陣；M3dsc表示所有特征點對的3DSC 特征描述符歐式距離矩陣；Mbg表示特征點對的二部圖矩陣，圖3(c)中的綠線連接即為求解的對應(yīng)特征點對。

圖3 K-M算法示意圖

以上證明了在給定一個點云對和一個固定閾值的情況下,可以通過求解來計算。給定如上所述的加權(quán)二部圖G=(S,T,E)，采用KM 算法將尋找最小權(quán)重匹配的優(yōu)化問題轉(zhuǎn)化為尋找點云匹配中對應(yīng)特征點的匹配問題，輸出點云特征點的對應(yīng)關(guān)系。一旦確定對應(yīng)關(guān)系，就可以通過奇異值分解(SVD)來計算變換矩陣，如式⑿。

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

實驗為：實際點云的算法配準(zhǔn)實驗。

為驗證配準(zhǔn)算法在實際場景中的配準(zhǔn)效果，使用RealSense 設(shè)備采集零件點云，并應(yīng)用該算法進(jìn)行點云配準(zhǔn)。如圖4 所示。本文共采集三個零件點云，對實際工件點云應(yīng)用本文算法時，將原始工件點云與目標(biāo)工件點云的ISS 特征點檢測算法的球鄰域半徑設(shè)置為0.01，非極大抑制半徑設(shè)置為0.01。為直觀地顯示配準(zhǔn)算法對實際場景中的工件的配準(zhǔn)效果，對配準(zhǔn)過程進(jìn)行可視化顯示。圖4(a)顯示了零件點云的初始位姿；圖4(b)顯示了點云經(jīng)過粗配準(zhǔn)后的結(jié)果，從工件在粗配準(zhǔn)中的位姿關(guān)系可以看出兩者已經(jīng)處于較近的距離，工件場景點云與模板點云已經(jīng)建立正確的位姿關(guān)系，表明算法較好地完成了粗配準(zhǔn)的任務(wù)。為進(jìn)一步細(xì)化兩點云之間的位姿差異，利用ICP 精配準(zhǔn)算法對上述模型進(jìn)行優(yōu)化迭代。對工件點云模型的最終配準(zhǔn)效果如圖4(c)所示，可以看到工件原始點云與工件目標(biāo)點云基本準(zhǔn)確配準(zhǔn)。

圖4 零件點云配準(zhǔn)圖

為驗證本文算法在粗配準(zhǔn)階段的運算時間與精度優(yōu)勢，將該算法與“基于SAC-IA 與ICP 的點云配準(zhǔn)算法[12]”和“利用特征點采樣一致性改進(jìn)ICP 算法點云配準(zhǔn)方法[13]”的粗配準(zhǔn)部分進(jìn)行對比。分別采用工件a、工件b 和工件c 的原始點云，以及目標(biāo)點云，點云配準(zhǔn)誤差由均方根誤差表征。對三種不同的配準(zhǔn)方法進(jìn)行比較，其實驗結(jié)果可視化如圖5 所示。實驗結(jié)果用表1表示不同算法對零件粗配準(zhǔn)的精度；用表2表示不同算法對零件的粗配準(zhǔn)時間。

圖5 不同算法工件點云粗配準(zhǔn)圖

表1 不同算法對零件的粗配準(zhǔn)精度(mm)

表2 不同算法對零件的粗配準(zhǔn)時間(s)

3 結(jié)論

針對傳統(tǒng)點云配準(zhǔn)ICP需要兩點云具有相對良好的初始位姿，否則算法容易陷入局部最優(yōu)而導(dǎo)致配準(zhǔn)失敗的問題。本文從特征點對全局匹配的思路出發(fā)，將對應(yīng)點對匹配與二部圖完備匹配相結(jié)合。通過引入幾何一致性賦權(quán)的特征點對3DSC 描述符歐氏距離來定義二部圖的權(quán)值矩陣，利用二部圖匹配的KM 算法得出特征點對的對應(yīng)關(guān)系并應(yīng)用SVD 分解完成點云粗配準(zhǔn)。所提算法為精配準(zhǔn)ICP 提供優(yōu)化后的位姿，提高了精配準(zhǔn)的ICP的精度。實驗結(jié)果表明，與傳統(tǒng)配準(zhǔn)方法相比，來算法在保證精度的情況下，提高了點云粗配準(zhǔn)的速度。但仍需注意的是，該配準(zhǔn)算法對點云質(zhì)量有一定要求，重疊率較低的點云由于缺少足夠的關(guān)鍵特征點會影響算法的最終匹配效果，進(jìn)而對精配準(zhǔn)帶來誤差。