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

夏坎強(qiáng)

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

0 引言

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

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

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

1 方法原理

1.1 方法框架

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

圖1 算法流程圖

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖3 K-M算法示意圖

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

2 實(shí)驗(yàn)結(jié)果與分析

實(shí)驗(yàn)為：實(shí)際點(diǎn)云的算法配準(zhǔn)實(shí)驗(yàn)。

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

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

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

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

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

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

3 結(jié)論

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