基于特征點(diǎn)對(duì)優(yōu)化篩選的點(diǎn)云初始配準(zhǔn)算法

郭 敏， 孫 夢(mèng)， 呂源治， 李貞蘭

(1.長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春 130102;2.中國(guó)科學(xué)院長(zhǎng)春光學(xué)精密機(jī)械與物理研究所， 吉林 長(zhǎng)春 130033；3.吉林大學(xué)， 吉林 長(zhǎng)春 130012)

0 引 言

隨著物聯(lián)網(wǎng)技術(shù)和三維立體視覺的不斷發(fā)展，三維點(diǎn)云數(shù)據(jù)處理技術(shù)在人工智能、虛擬重建、機(jī)器視覺等領(lǐng)域得到廣泛應(yīng)用[1-3]。點(diǎn)云配準(zhǔn)技術(shù)是點(diǎn)云處理技術(shù)的關(guān)鍵部分，點(diǎn)云配準(zhǔn)質(zhì)量是影響三維重建效果的重要因素。

自動(dòng)配準(zhǔn)、手動(dòng)配準(zhǔn)和依賴儀器配準(zhǔn)是點(diǎn)云配準(zhǔn)方式的三種形式[4]。目前主要采用自動(dòng)配準(zhǔn)技術(shù)，最常用的方法是由 Besl P J等[5]提出的迭代最近點(diǎn)(Iterative Closest Point, ICP)算法，它是當(dāng)前點(diǎn)云配準(zhǔn)過程中應(yīng)用最廣泛的算法。雖然該迭代算法精確度較高，但對(duì)兩組點(diǎn)云的相對(duì)初始位置要求較高且容易陷入局部最優(yōu)解[6]。針對(duì)此問題，目前點(diǎn)云配準(zhǔn)主要采用先進(jìn)行初始配準(zhǔn)再進(jìn)行精配準(zhǔn)的方式[7]。初始配準(zhǔn)算法主要包括主成分分析法、基于特征的配準(zhǔn)算法以及基于投票機(jī)制的配準(zhǔn)方法。基于特征的配準(zhǔn)是指利用物體自身的點(diǎn)、線、面等幾何特征或紋理特征解計(jì)算變換參數(shù)[8]，它是目前應(yīng)用最為廣泛的初始配準(zhǔn)算法，主要分為特征檢測(cè)、特征描述和特征匹配三個(gè)步驟。特征描述步驟中應(yīng)用最為廣泛的是Rusu R B等[9]在2009年提出的一種FPFH用于描述點(diǎn)云特征。在特征匹配階段，RANSAC算法是一種應(yīng)用較為廣泛的特征匹配方法[10]，但存在有限次隨機(jī)性帶來(lái)的不穩(wěn)定性和計(jì)算量較大等弊端。當(dāng)輸入的點(diǎn)云中相似特征點(diǎn)較多時(shí)，僅依靠特征描述的相似程度搜索對(duì)應(yīng)點(diǎn)[11]，會(huì)出現(xiàn)較多的錯(cuò)誤匹配點(diǎn)對(duì)，導(dǎo)致后續(xù)的點(diǎn)云配準(zhǔn)結(jié)果較差。

針對(duì)上述問題，提出一種基于特征點(diǎn)對(duì)優(yōu)化的初始配準(zhǔn)算法，該算法能夠在優(yōu)化對(duì)應(yīng)點(diǎn)集的同時(shí)，規(guī)范化閾值的選取，減少了對(duì)初始配準(zhǔn)結(jié)果的影響，為點(diǎn)云的精配準(zhǔn)提供精確度較高的初始對(duì)應(yīng)點(diǎn)集，從而實(shí)現(xiàn)對(duì)三維點(diǎn)云數(shù)據(jù)配準(zhǔn)的目標(biāo)。同時(shí)文中又基于三維坐標(biāo)轉(zhuǎn)換的基本理論，羅德里格矩陣在迭代求解點(diǎn)云配準(zhǔn)參數(shù)中的應(yīng)用。

1 配準(zhǔn)原理

三維點(diǎn)云初始配準(zhǔn)的關(guān)鍵在于獲取較高精度的初始位置和精確的對(duì)應(yīng)點(diǎn)對(duì)。為此，文中采用基于點(diǎn)云特征實(shí)現(xiàn)初始配準(zhǔn)的算法流程如圖1所示。

圖1 點(diǎn)云初始配準(zhǔn)算法流程

首先，對(duì)輸入的兩組點(diǎn)云進(jìn)行法線估計(jì)；其次，基于點(diǎn)云的曲率實(shí)現(xiàn)特征點(diǎn)的提取；再次，根據(jù)特征點(diǎn)建立點(diǎn)云的快速特征直方圖；然后，基于已經(jīng)得到的直方圖實(shí)現(xiàn)初始對(duì)應(yīng)點(diǎn)集的建立，并進(jìn)行優(yōu)化選??；最后，利用羅德里格斯旋轉(zhuǎn)公式求解初始變換位姿，以實(shí)現(xiàn)點(diǎn)云初始配準(zhǔn)的目標(biāo)。

1.1 計(jì)算點(diǎn)云法向量

采用主成分分析(Principal Component Analysis, PCA)[12]算法實(shí)現(xiàn)點(diǎn)云的法向量估計(jì)，首先，構(gòu)建點(diǎn)云的KD樹并計(jì)算其鄰域內(nèi)的中心點(diǎn)坐標(biāo);其次，構(gòu)造點(diǎn)云的協(xié)方差矩陣,并計(jì)算該矩陣的特征值和特征向量;然后，將所求得的特征值按照從小到大的順序進(jìn)行排序，最小特征值對(duì)應(yīng)的特征向量就是點(diǎn)云的法向量；最后，對(duì)求得的法向量進(jìn)行檢驗(yàn)與調(diào)整，使得所有點(diǎn)云數(shù)據(jù)的法向量方向一致。

1.2 提取特征點(diǎn)

曲率能夠描述曲線或者曲面的彎曲程度，在三維點(diǎn)云中可以用來(lái)描述多個(gè)點(diǎn)所構(gòu)成曲面的局部性質(zhì)[13]，常用的曲率有高斯曲率、平均曲率和主曲率。三維空間中某點(diǎn)的平均曲率表示曲面在該點(diǎn)的平均彎曲程度，文中基于平均曲率實(shí)現(xiàn)特征點(diǎn)的提取。

首先，將點(diǎn)云中第c個(gè)點(diǎn)Pc(c=1,2,…,N)(N表示點(diǎn)的個(gè)數(shù))的曲率估計(jì)值為

(1)

式中：λ1,λ2,λ3----第c個(gè)點(diǎn)與其e個(gè)鄰域點(diǎn)所建立協(xié)方差矩陣的特征值，滿足λ1≤λ2且λ1≤λ3。

然后，設(shè)定曲率閾值

(2)

最后，根據(jù)曲率估計(jì)值dc與閾值T大小的比較，得到特征點(diǎn)集與非特征點(diǎn)集：曲率估計(jì)值大于閾值的點(diǎn)標(biāo)記為特征點(diǎn)；否則，標(biāo)記為非特征點(diǎn)，所有特征點(diǎn)組成特征點(diǎn)集,非特征點(diǎn)組成非特征點(diǎn)集。

1.3 建立快速點(diǎn)特征直方圖

快速點(diǎn)特征直方圖(FPFH)是一種能夠描述特征點(diǎn)鄰域內(nèi)幾何特征的描述子，利用多個(gè)不同維度的直方圖來(lái)描述特征點(diǎn)的法向量特征，從而描述特征點(diǎn)鄰域內(nèi)的幾何特征信息。在三維空間中，空間內(nèi)任意一點(diǎn)與其K鄰域內(nèi)的每一個(gè)點(diǎn)兩兩連接，將點(diǎn)與點(diǎn)之間的幾何特征定量化表示，基于點(diǎn)云的三維坐標(biāo)信息與法向量之間的相互作用關(guān)系，描述出點(diǎn)云的幾何特征。查詢點(diǎn)Pq與其K鄰域點(diǎn)的FPFH計(jì)算關(guān)系如圖2所示。

圖2 查詢點(diǎn)Pq與其鄰域點(diǎn)FPFH描述子計(jì)算原理圖

FPFH描述子僅在查詢點(diǎn)與鄰域點(diǎn)之間建立幾何關(guān)系，為了更好地描述任意兩點(diǎn)Ps與Pt對(duì)應(yīng)法向量之間的關(guān)系，選擇其中一點(diǎn)作為坐標(biāo)原點(diǎn)，在兩點(diǎn)間構(gòu)造局部坐標(biāo)系，如圖3所示。

圖3 兩點(diǎn)間局部坐標(biāo)系

首先分別查詢兩點(diǎn)的法向量，其次以法向量為其中一個(gè)坐標(biāo)軸，然后根據(jù)法向量求剩下兩個(gè)坐標(biāo)軸的單位向量，再根據(jù)向量求其夾角。三個(gè)單位向量u，v,w計(jì)算如下:

(3)

根據(jù)三個(gè)單位向量u，v,w可計(jì)算法向量與局部坐標(biāo)系各坐標(biāo)軸之間的夾角。

(4)

α,φ,θ這三元組也被稱為簡(jiǎn)化點(diǎn)特征直方圖，記作SPFH。對(duì)于某個(gè)查詢點(diǎn)Pq來(lái)說，其FPFH的建立過程如下：

1)求該點(diǎn)的簡(jiǎn)化點(diǎn)特征直方圖SPFH(Pq)。

2)利用已經(jīng)得到的SPFH(Pq)并根據(jù)下列公式計(jì)算FPFH特征

FPFH(Pq)=SPFH(Pq)+

(5)

式中：ωk----權(quán)重;

k----鄰域點(diǎn)的個(gè)數(shù)。

1.4 優(yōu)化篩選特征點(diǎn)對(duì)

基于點(diǎn)云的特征直方圖尋找源點(diǎn)云與目標(biāo)點(diǎn)云間的對(duì)應(yīng)點(diǎn)對(duì)，采用最鄰近搜索方式查找源點(diǎn)云與目標(biāo)點(diǎn)云中特征直方圖相似的點(diǎn)作為初始對(duì)應(yīng)點(diǎn)集，設(shè)H(P)表示源點(diǎn)云的特征直方圖，H(Q)表示目標(biāo)點(diǎn)云的特征直方圖。對(duì)于源點(diǎn)云中的一點(diǎn)p在H(Q) 中能找到與H(p) 極為相似的最鄰近點(diǎn), 同理，對(duì)于目標(biāo)點(diǎn)云中的一點(diǎn)q在H(P) 中能找到與H(q)極為相似的最鄰近點(diǎn)，組成初始的對(duì)應(yīng)點(diǎn)集A1。

由于此時(shí)對(duì)應(yīng)點(diǎn)集的誤差較大，所以要對(duì)其進(jìn)行兩次優(yōu)化篩選。

1.4.1 一一對(duì)應(yīng)原則

在獲取初始的對(duì)應(yīng)點(diǎn)集時(shí)，是基于點(diǎn)特征直方圖的相似程度來(lái)確立的，存在源點(diǎn)云中的一點(diǎn)(目標(biāo)點(diǎn)云中的一點(diǎn))對(duì)應(yīng)目標(biāo)點(diǎn)云中多個(gè)點(diǎn)(源點(diǎn)云中多個(gè)點(diǎn))的情況。此時(shí)需要優(yōu)化點(diǎn)云以實(shí)現(xiàn)點(diǎn)云一對(duì)一的目標(biāo)，即對(duì)于初始對(duì)應(yīng)點(diǎn)集中的一組點(diǎn)對(duì)(p,q)需滿足p的最鄰近點(diǎn)是q,q的最鄰近點(diǎn)是p, 優(yōu)化后得到新的點(diǎn)集為A2。

1.4.2 近似等距離原則

等距離原則是源點(diǎn)云中的一點(diǎn)p1到目標(biāo)點(diǎn)云的對(duì)應(yīng)點(diǎn)q1的距離等于源點(diǎn)云中另一點(diǎn)p2到其目標(biāo)點(diǎn)云中對(duì)應(yīng)點(diǎn)q2的距離，同時(shí)源點(diǎn)云中p1與p2的距離等于目標(biāo)點(diǎn)云中q1與q2的距離，兩點(diǎn)集中點(diǎn)間距離模仿圖如圖4所示。

圖4 兩點(diǎn)集中點(diǎn)間距離模仿圖

(p1,q1)、(p2,q2)為模擬點(diǎn)云中正確的對(duì)應(yīng)點(diǎn)對(duì)，(p3,q3)是一組錯(cuò)誤點(diǎn)對(duì)。

由于實(shí)際中存在誤差，不能達(dá)到距離值完全相等的目標(biāo)，所以設(shè)定一個(gè)容忍小范圍誤差的閾值ε1和ε2，以剔除點(diǎn)集A2中錯(cuò)誤的對(duì)應(yīng)點(diǎn)對(duì)。

其中，ε1的計(jì)算過程如下。首先，設(shè)L={l1,l2,l3,…,lF-1},表示兩組對(duì)應(yīng)點(diǎn)中源點(diǎn)云兩點(diǎn)歐式距離與目標(biāo)點(diǎn)云中兩點(diǎn)歐式距離比值的集合，其中F表示A2對(duì)應(yīng)點(diǎn)對(duì)的個(gè)數(shù)，L中集合參數(shù)可由下式計(jì)算

(6)

然后，根據(jù)式(6)可得ε1的表達(dá)式為

(7)

同理也可得到ε2，設(shè)M={m1,m2,m3,…,mF-1}表示兩組對(duì)應(yīng)點(diǎn)中源點(diǎn)云兩點(diǎn)歐式距離與目標(biāo)點(diǎn)云中兩點(diǎn)歐式距離比值的集合。M中集合參數(shù)可根據(jù)下式計(jì)算

(8)

然后，根據(jù)式(8)計(jì)算

(9)

對(duì)于A2中的任意兩組對(duì)應(yīng)點(diǎn)對(duì)需滿足

|lf-1| ≤ε1且 |mf-1| ≤ε2,

(10)

經(jīng)過上述兩次優(yōu)化篩選得到最終的對(duì)應(yīng)點(diǎn)集。

2 點(diǎn)云初始配準(zhǔn)

為計(jì)算點(diǎn)云配準(zhǔn)的旋轉(zhuǎn)和平移矩陣，需要經(jīng)過多次的迭代遍歷過程，輸出使配準(zhǔn)誤差值最小情況下的旋轉(zhuǎn)矩陣和平移矩陣。具體步驟如下：

1)隨機(jī)選取n(n≥3) 組對(duì)應(yīng)點(diǎn)；

2)通過羅德里格斯公式求旋轉(zhuǎn)矩陣和平移向量[14]；

3)根據(jù)式(11)計(jì)算此時(shí)的配準(zhǔn)誤差為

(11)

式中:Qb----目標(biāo)點(diǎn)云旋轉(zhuǎn)后的點(diǎn)集;

G----對(duì)應(yīng)點(diǎn)對(duì)的個(gè)數(shù);

4)重復(fù)上述3個(gè)步驟，直到滿足迭代次數(shù)為止，輸出最小誤差對(duì)應(yīng)的旋轉(zhuǎn)矩陣和平移向量。

通過羅德里格斯公式求旋轉(zhuǎn)矩陣和平移向量的過程。

(12)

式中：D=1-cosθ。

根據(jù)旋轉(zhuǎn)關(guān)系，將目標(biāo)點(diǎn)云中一點(diǎn)(X,Y,Z)轉(zhuǎn)換到源點(diǎn)云坐標(biāo)系下，得到點(diǎn)(X′,Y′,Z′),兩個(gè)坐標(biāo)點(diǎn)之間的關(guān)系為

(13)

式中：t----平移向量;

R----旋轉(zhuǎn)矩陣。

通過反對(duì)稱矩陣可以構(gòu)建羅德里格矩陣表達(dá)為

R=(I+S)(I-S)-1,

(14)

式中：S----旋轉(zhuǎn)向量對(duì)應(yīng)的反對(duì)稱矩陣;

I----單位矩陣。

設(shè)旋轉(zhuǎn)向量為

a=[ax,ay,az]T。

根據(jù)已有的任意三個(gè)公共點(diǎn)，求解轉(zhuǎn)換參數(shù)，(Xi,Yi,Zi)和(Xj,Yj,Zj)兩組對(duì)應(yīng)點(diǎn)對(duì)式 (11)相減得到：

(15)

對(duì)式(15)代入R,并整理轉(zhuǎn)換可得到：

1)令

A和B中Δ表示求得的差值。

ΔXij=Xi-Xj,

隨機(jī)選取3組及以上點(diǎn)云時(shí),計(jì)算得到的矩陣A和矩陣B分別垂直合并,則AS=B，根據(jù)最小二乘原理可求得a=(ATA)-1ATB，對(duì)其進(jìn)行單位化得到旋轉(zhuǎn)軸O。

2)旋轉(zhuǎn)角的取值為

式中：max(a)----a中列向量的最大值。

3)將求得的參數(shù)代入羅德里格斯旋轉(zhuǎn)矩陣的展開式,可得到旋轉(zhuǎn)矩陣，根據(jù)旋轉(zhuǎn)前后兩坐標(biāo)點(diǎn)的關(guān)系，可求得平移向量。

3 實(shí)驗(yàn)結(jié)果及分析

3.1 實(shí)驗(yàn)環(huán)境與數(shù)據(jù)來(lái)源

實(shí)驗(yàn)采用計(jì)算機(jī)硬件環(huán)境為Intel(R) Core(TM) i5-5200U CPU @ 2.20 GHz 2.19 GHz，4 GB內(nèi)存，軟件環(huán)境為Windows 10 操作系統(tǒng)，使用Matlab編程軟件。采用長(zhǎng)春光機(jī)所自主研發(fā)的SVision751B型三維激光掃描儀對(duì)機(jī)器貓石膏像進(jìn)行點(diǎn)云數(shù)據(jù)采集，原始點(diǎn)云數(shù)據(jù)及點(diǎn)云封裝圖分別如圖5和圖6所示。

圖5 原始點(diǎn)云

圖6 點(diǎn)云封裝圖

兩圖中左邊均為目標(biāo)點(diǎn)云，右邊為源點(diǎn)云。

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

為減少因數(shù)據(jù)冗余造成計(jì)算效率低下的問題，對(duì)圖5中的原始點(diǎn)云數(shù)據(jù)進(jìn)行了精簡(jiǎn)，以精簡(jiǎn)后的點(diǎn)云作為輸入的點(diǎn)云數(shù)據(jù)集，按照?qǐng)D1步驟完成點(diǎn)云配準(zhǔn)實(shí)驗(yàn)。源點(diǎn)云與目標(biāo)點(diǎn)云法向量的求取結(jié)果，法向量的方向統(tǒng)一指向點(diǎn)云外側(cè)，源點(diǎn)云與目標(biāo)點(diǎn)云的法向量如圖7所示。

圖7 源點(diǎn)云與目標(biāo)點(diǎn)云的法向量

源點(diǎn)云與目標(biāo)點(diǎn)云的特征點(diǎn)如圖8所示。

圖8 源點(diǎn)云與目標(biāo)點(diǎn)云的特征點(diǎn)

特征點(diǎn)個(gè)數(shù)與輸入點(diǎn)云個(gè)數(shù)對(duì)比見表1。

源點(diǎn)云中提取到的特征點(diǎn)有8 424個(gè)，目標(biāo)點(diǎn)云中提取到的特征點(diǎn)有8 082個(gè)。

利用特征直方圖，在目標(biāo)點(diǎn)云與源點(diǎn)云中確立初始對(duì)應(yīng)點(diǎn)集，共計(jì)8 424對(duì)，第1～1 000組對(duì)應(yīng)點(diǎn)集如圖9所示。

圖9 優(yōu)化前的對(duì)應(yīng)點(diǎn)集(第1～1 000組)

從圖中可以看出，此時(shí)的對(duì)應(yīng)點(diǎn)集中存在較多的錯(cuò)誤點(diǎn)對(duì)，運(yùn)用文中提出的特征點(diǎn)對(duì)優(yōu)化篩選算法進(jìn)行處理后，得到新的對(duì)應(yīng)點(diǎn)集，如圖10所示。

圖10 優(yōu)化后的對(duì)應(yīng)點(diǎn)集

相比圖9，新點(diǎn)集中對(duì)應(yīng)點(diǎn)對(duì)質(zhì)量較高。

記錄優(yōu)化篩選前后點(diǎn)對(duì)個(gè)數(shù)的變化見表2。

通過數(shù)據(jù)對(duì)比發(fā)現(xiàn)，初始對(duì)應(yīng)點(diǎn)集中錯(cuò)誤點(diǎn)對(duì)的數(shù)量較多，若不對(duì)其優(yōu)化篩選，將直接影響后續(xù)點(diǎn)云初始配準(zhǔn)的精度，甚至導(dǎo)致配準(zhǔn)失敗。

表2 對(duì)應(yīng)點(diǎn)對(duì)的數(shù)量

為進(jìn)一步驗(yàn)證文中算法在精度及穩(wěn)定性方面的性能，將所研究算法與基于RANSAC思想剔除錯(cuò)誤點(diǎn)對(duì)的初始配準(zhǔn)算法的實(shí)驗(yàn)結(jié)果對(duì)比分析見表3。

表3 不同迭代次數(shù)下配準(zhǔn)誤差對(duì)比

實(shí)驗(yàn)中不斷調(diào)整迭代次數(shù)，并分別記錄兩種算法在不同迭代次數(shù)下的配準(zhǔn)誤差，對(duì)比發(fā)現(xiàn)，文中算法的配準(zhǔn)誤差相對(duì)較低。迭代次數(shù)為1 000時(shí)，兩種算法的配準(zhǔn)結(jié)果如圖11所示。

(a) 文中算法

結(jié)合表3中該迭代次數(shù)下的配準(zhǔn)誤差對(duì)比分析(a)和(b)兩圖可發(fā)現(xiàn)，文中算法略優(yōu)于基于RANSAC思想剔除錯(cuò)誤點(diǎn)對(duì)的初始配準(zhǔn)算法。

4 結(jié) 語(yǔ)

針對(duì)源點(diǎn)云和目標(biāo)點(diǎn)云中特征點(diǎn)對(duì)容易產(chǎn)生錯(cuò)誤匹配，從而導(dǎo)致初始配準(zhǔn)精度較低的問題，文中對(duì)基于特征點(diǎn)對(duì)篩選優(yōu)化的點(diǎn)云初始配準(zhǔn)算法進(jìn)行了研究，實(shí)驗(yàn)結(jié)果表明，該算法的誤差較小，精確度較高,且穩(wěn)定性較強(qiáng)。