基于改進(jìn)PSO-TrICP算法的點(diǎn)云配準(zhǔn)

梁正友，王 璐，李軒昂，楊 鋒

(1.廣西大學(xué)計(jì)算機(jī)與電子信息學(xué)院，廣西 南寧 530004； 2.廣西多媒體通信與網(wǎng)絡(luò)技術(shù)重點(diǎn)實(shí)驗(yàn)室，廣西 南寧 530004)

0 引 言

點(diǎn)云配準(zhǔn)是通過求取點(diǎn)云間的空間變換參數(shù)，將不同場景、不同時(shí)刻采集的點(diǎn)云數(shù)據(jù)對(duì)齊至同一空間坐標(biāo)系的過程[1]。其中，點(diǎn)云的粗配準(zhǔn)以快速地估計(jì)點(diǎn)云的剛性變換為目的[2]，廣泛運(yùn)用于虛擬現(xiàn)實(shí)、即時(shí)定位與地圖構(gòu)建、航天影像等領(lǐng)域[3-4]。1992年，Besl等人[5]提出的ICP算法通過估計(jì)點(diǎn)云間的旋轉(zhuǎn)參數(shù)R∈3×3及平移參數(shù)T∈3×1，最小化點(diǎn)云間的空間偏差，是一種常用的配準(zhǔn)算法，但I(xiàn)CP算法存在對(duì)點(diǎn)云的初始空間位置要求高，且點(diǎn)集規(guī)模較大時(shí)運(yùn)行速度慢的缺點(diǎn)。因此，許多研究者對(duì)該算法進(jìn)行了改進(jìn)[8-10]。Chetverikov 等人[8]針對(duì)僅存在部分重疊的點(diǎn)云配準(zhǔn)需求提出了一種TrICP算法，通過估計(jì)點(diǎn)云間的重疊率，最小化點(diǎn)云間的均方根距離，從而提升點(diǎn)云的配準(zhǔn)質(zhì)量；Li等人[9]利用K-D樹結(jié)構(gòu)存儲(chǔ)點(diǎn)云數(shù)據(jù)并將其用于搜索點(diǎn)云的鄰域，使數(shù)據(jù)在得到高效管理的同時(shí)提升了配準(zhǔn)精度；楊小青等人[10]將點(diǎn)云的法向量夾角和曲率用于查找對(duì)應(yīng)點(diǎn)集，并通過計(jì)算點(diǎn)對(duì)的距離和高斯曲率兩重約束獲取匹配點(diǎn)，能夠得到可靠且高效的配準(zhǔn)結(jié)果。

近年來，由于智能算法對(duì)最優(yōu)解問題具有優(yōu)勢(shì)，吸引了研究者將其引入點(diǎn)云配準(zhǔn)技術(shù)中[11-14]。Chen等人[11]提出一種將增強(qiáng)人工蜂群算法用于三維點(diǎn)云配準(zhǔn)的方法，提升了配準(zhǔn)效率；Lomonosov等人[12]提出了一種利用遺傳算法(Genetic Algorithm, GA)對(duì)任意取向的三維曲面配準(zhǔn)的方法，該方法能夠完成搜索空間內(nèi)任意方位點(diǎn)集的粗配準(zhǔn)；Zhu等人[13]提出了一種將GA算法結(jié)合TrICP算法的多視圖配準(zhǔn)技術(shù)，通過進(jìn)化算法中的遺傳和變異等操作估計(jì)點(diǎn)云初始位置，能夠得到準(zhǔn)確的剛性變換參數(shù)；Yan等人[14]為結(jié)合GA算法的配準(zhǔn)技術(shù)提出了一種新的適應(yīng)度函數(shù)，滿足了配準(zhǔn)需求。

Kennedy和 Eberhart提出的PSO算法[15]是一種常見的群體智能算法，能夠?yàn)槿我馊∠虻狞c(diǎn)云提供一個(gè)良好的初始位置，從而獲得更準(zhǔn)確的配準(zhǔn)結(jié)果[16-18]。Zhan等人[19]提出了一種基于熵和PSO算法的配準(zhǔn)方法，能夠達(dá)到抑制噪聲，有效提高配準(zhǔn)精度的效果；Ge等人[20]同樣運(yùn)用了PSO算法，通過限制法向量方向及對(duì)應(yīng)點(diǎn)距離等方法提升了配準(zhǔn)精度；Zhang等人[21]提出將多層粒子群優(yōu)化算法應(yīng)用于配準(zhǔn)，從而取得良好的效果，但該方法的運(yùn)行時(shí)間較長；Yang等人[22]提出了一種將PSO算法結(jié)合ICP算法的配準(zhǔn)，但該方法耗時(shí)較長，且無法完全避免配準(zhǔn)結(jié)果陷入局部最優(yōu)。綜上所述，結(jié)合智能算法的點(diǎn)云配準(zhǔn)方法需要在保證配準(zhǔn)性能的同時(shí)提升運(yùn)行效率，成為有待解決的問題。

因此，針對(duì)點(diǎn)云處于任意初始空間位置的情況，本文提出一種基于改進(jìn)PSO-TrICP算法的點(diǎn)云配準(zhǔn)方法。首先對(duì)傳統(tǒng)PSO算法改進(jìn)，添加了相似度測量準(zhǔn)則和新的學(xué)習(xí)因子提升算法的尋優(yōu)能力，其次利用改進(jìn)的PSO算法的隨機(jī)搜索機(jī)制構(gòu)造初始粒子群，最后通過TrICP算法在迭代中求取剛性變換參數(shù)，得到最終的配準(zhǔn)結(jié)果。在Stanford點(diǎn)云數(shù)據(jù)集和Kinect v2所采集的三維人體模型數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)的結(jié)果表明，本文方法的配準(zhǔn)精度及運(yùn)行效率優(yōu)于其他配準(zhǔn)方法，且具有較好的魯棒性。

1 基礎(chǔ)算法介紹

1.1 傳統(tǒng)PSO算法

PSO算法[15]模擬鳥群生存過程中的捕食行為：鳥類個(gè)體依靠自身經(jīng)驗(yàn)與其他成員間的信息交流調(diào)整搜索路徑，最終抵達(dá)食物最多的場所，具有無需梯度信息且所需參數(shù)少的特點(diǎn)[24-25]。

(1)

(2)

其中，ω是取值范圍在0～1之間的慣性參數(shù)，c1和c2分別是粒子的認(rèn)識(shí)系數(shù)和社會(huì)系數(shù)，通常取值為2；rand()是取值范圍在0～1間的隨機(jī)數(shù)。

1.2 TrICP算法

TrICP算法[8]是一種改進(jìn)的ICP算法，通過裁剪點(diǎn)云之間重疊部分的數(shù)據(jù)點(diǎn)，避免構(gòu)成錯(cuò)誤的點(diǎn)對(duì)關(guān)系，提升了配準(zhǔn)的精度和效率。假設(shè)存在部分重疊的點(diǎn)云Q和點(diǎn)云O，初始化重疊率為ζ，且ζ∈[ζmin,ζmax]；點(diǎn)云Q中共包含N個(gè)點(diǎn)，其中，只有ζ×N個(gè)點(diǎn)與點(diǎn)云O存在正確的匹配點(diǎn)。因此，從點(diǎn)云Q中提取ζ×N個(gè)點(diǎn)構(gòu)成特征點(diǎn)集QF，并計(jì)算點(diǎn)云QF和點(diǎn)云O間的剛性變換參數(shù)(R,T)，得到最終的配準(zhǔn)結(jié)果。

在迭代過程中，TrICP算法提取對(duì)應(yīng)點(diǎn)集的描述如下:

假設(shè)qi是點(diǎn)云Q中的點(diǎn)，根據(jù)初始空間變換矩陣(R,T)，則qi與點(diǎn)云O的對(duì)應(yīng)關(guān)系如公式(3)：

(3)

計(jì)算個(gè)體距離ri：

ri=‖ocl(i,R,T)-(Rqi+T)‖

(4)

對(duì)ri進(jìn)行增序排列：

(5)

其中，N′=ζ×N。

根據(jù)重疊率ζ從點(diǎn)云Q中提取特征點(diǎn)集QF={q1,q2,…,qN′}，STS是點(diǎn)集QF到點(diǎn)云O的距離之和：

(6)

計(jì)算最小化平均距離SN′：

(7)

根據(jù)SN′結(jié)合黃金分割原理更新重疊率ζ，參數(shù)λ通常設(shè)置為2：

(8)

TrICP算法的詳細(xì)步驟見算法1。

算法1TrICP算法

輸入：點(diǎn)云Q和點(diǎn)云O、最大重疊率ζmax、最小重疊率ζmin、最大迭代次數(shù)M

輸出：剛性變換矩陣(R,T)

Begin:

m=0;

Repeat:

m=m+1;

根據(jù)公式(3)建立點(diǎn)云間的對(duì)應(yīng)關(guān)系;

根據(jù)公式(4)獲取個(gè)體距離ri;

根據(jù)公式(6)計(jì)算距離之和STS;

求取點(diǎn)云Q和點(diǎn)云O間的剛性變換矩陣(R,T);

根據(jù)公式(7)和公式(8)更新重疊率ζ;

小于設(shè)定閾值

End

2 提出的方法

2.1 改進(jìn)的PSO算法

(9)

(10)

在添加新的學(xué)習(xí)因子后，計(jì)算粒子速度的公式為：

(11)

2.2 基于改進(jìn)PSO-TrICP的點(diǎn)云配準(zhǔn)算法

針對(duì)空間內(nèi)任意初始位置的點(diǎn)云粗配準(zhǔn)問題，本文結(jié)合改進(jìn)PSO算法和TrICP算法提出一種新的點(diǎn)云配準(zhǔn)方法，利用改進(jìn)的PSO算法為點(diǎn)云之間提供良好的相對(duì)位置，并通過TrICP算法裁剪重疊部分的點(diǎn)云提升配準(zhǔn)質(zhì)量，克服傳統(tǒng)配準(zhǔn)算法對(duì)點(diǎn)云的初始相對(duì)位置要求高、容易陷入局部最優(yōu)等缺點(diǎn)。

(12)

接著，將粒子群Pi作為初始空間變換參數(shù)，通過TrICP算法計(jì)算得到新的(R,T)及適應(yīng)度，粒子群的適應(yīng)度函數(shù)計(jì)算公式為：

fitness(Ri,Ti,ζi)=EXP(-SN′)

(13)

基于改進(jìn)PSO-TrICP的點(diǎn)云配準(zhǔn)算法的詳細(xì)步驟見算法2。

算法2基于改進(jìn)PSO-TrICP的點(diǎn)云配準(zhǔn)算法

輸入：點(diǎn)云Q和點(diǎn)云O，最大迭代次數(shù)K

輸出：剛性變換矩陣

Begin:

k=0;

初始化粒子群P;

Repeat:

k=k+1;

根據(jù)算法1計(jì)算粒子適應(yīng)度并更新粒子;

If持續(xù)x輪迭代U(k)<ε

根據(jù)公式(2)和公式(11)更新粒子;

Else

根據(jù)公式(1)和公式(2)更新粒子;

End

Untilk>K

End

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

3.1 實(shí)驗(yàn)建立與評(píng)價(jià)指標(biāo)

為了驗(yàn)證本文提出算法的可靠性和高效性，本文采用Stanford三維點(diǎn)云數(shù)據(jù)集和Kinect v2采集的三維人體模型數(shù)據(jù)集為例。其中，Stanford三維點(diǎn)云數(shù)據(jù)集分別包括Bunny、Armadillo、Dragon及Buddha，模型圖像如圖1所示，點(diǎn)云詳情如表1所示；三維人體模型數(shù)據(jù)集包括身高為0.90 m的男童人體模型和身高為1.80 m的女性人體模型，如圖2所示。實(shí)驗(yàn)使用Windows 10(64位)的操作系統(tǒng)、Intel(R)Core(TM)i5-8250U CPU@1.60 GHz的計(jì)算機(jī)，代碼通過Matlab2018b實(shí)現(xiàn)。

圖1 Stanford三維點(diǎn)云數(shù)據(jù)集在不同角度的點(diǎn)云圖像

圖2 三維人體模型數(shù)據(jù)集在不同角度的點(diǎn)云圖像

表1 點(diǎn)云詳情

為消除實(shí)驗(yàn)過程中的隨機(jī)性，本文對(duì)各算法分別進(jìn)行50次蒙特卡洛實(shí)驗(yàn)，并取其旋轉(zhuǎn)誤差ER、平移誤差ET(·10-3·m)及運(yùn)行時(shí)長的均值。設(shè)為點(diǎn)云添加的隨機(jī)真實(shí)值為[Rg,Tg]，通過配準(zhǔn)算法得到的剛性變換矩陣為[Rw,Tw]，則旋轉(zhuǎn)誤差ER的度量公式表示為：

ER=‖Rw-Rg‖F(xiàn)

(14)

平移誤差ET的度量公式為：

ET=‖Tw-Tg‖2

(15)

3.2 對(duì)比試驗(yàn)與結(jié)果分析

為描述本文方法的性能，實(shí)驗(yàn)采用4種配準(zhǔn)算法與其對(duì)比，分別包括ICP算法[5]、TrICP算法[8]、PSO-ICP算法[22]和GA-TrICP算法[13]。

3.2.1 點(diǎn)云配準(zhǔn)精度

圖3、圖4和表2分別描述了不同算法對(duì)各模型的配準(zhǔn)可視化圖及精度?？梢钥闯觯篒CP算法和TrICP算法受初始空間位置的影響容易陷入局部最優(yōu)，從而導(dǎo)致配準(zhǔn)失??；PSO-ICP算法的配準(zhǔn)精度較差；GA-TrICP算法與本文方法相比，配準(zhǔn)的精度較低，但能夠滿足空間內(nèi)任意取向點(diǎn)云的配準(zhǔn)需求，利用智能算法在搜索空間內(nèi)獲得良好的初始位置，通過評(píng)估剛性變換參數(shù)的優(yōu)劣，從而使配準(zhǔn)結(jié)果趨于最優(yōu)解，提升了點(diǎn)云配準(zhǔn)的精度。

圖3 Stanford三維點(diǎn)云模型在各算法下的配準(zhǔn)結(jié)果

圖4 三維人體點(diǎn)云模型在各算法下的配準(zhǔn)結(jié)果

表2 不同算法對(duì)各模型配準(zhǔn)精度的比較

3.2.2 點(diǎn)云配準(zhǔn)效率

表3為不同算法對(duì)各模型配準(zhǔn)的運(yùn)行時(shí)長，可以看出ICP算法和TrICP算法是高效的，但這是以配準(zhǔn)失敗或損失配準(zhǔn)精度為前提的；PSO-ICP算法受到迭代最近點(diǎn)算法效率的影響，與本文方法相比，無法獲得較高的效率；GA-TrICP算法和本文方法都利用TrICP算法計(jì)算剛性變換參數(shù)，通過對(duì)點(diǎn)云重疊部分進(jìn)行裁剪，減少了算法運(yùn)算量，從而提升了配準(zhǔn)效率。

3.2.3 點(diǎn)云配準(zhǔn)魯棒性

為了更好地描述本文方法的魯棒性，本文為各個(gè)模型分別添加[-0.02f, 0.02f]的高斯噪聲，配準(zhǔn)的結(jié)果如表4所示?？梢钥闯霰疚姆椒軌蛲瓿删哂性肼暤狞c(diǎn)云配準(zhǔn)，且精度優(yōu)于對(duì)比的配準(zhǔn)算法。

表4 不同算法在添加[-0.02f, 0.02f]高斯噪聲后，點(diǎn)集的配準(zhǔn)精度的比較

綜上所述，本文提出的改進(jìn)PSO-TrICP算法在配準(zhǔn)實(shí)驗(yàn)中能夠在Stansford點(diǎn)云數(shù)據(jù)集中取得較好的結(jié)果，同時(shí)適用于本文用Kinect v2采集的人體數(shù)據(jù)集。比較其他方法，本文提升了配準(zhǔn)的精度及效率，且具有較強(qiáng)的魯棒性。

4 結(jié)束語

針對(duì)空間內(nèi)任意初始位置的點(diǎn)云粗配準(zhǔn)問題，本文提出了一種基于改進(jìn)PSO-TrICP的點(diǎn)云配準(zhǔn)算法。首先為傳統(tǒng)PSO算法增加了相似度測量準(zhǔn)則和新的學(xué)習(xí)因子，其次通過改進(jìn)的PSO算法將點(diǎn)云間的剛性變換參數(shù)及重疊率作為粒子，最后利用TrICP算法求取點(diǎn)云間的剛性變換矩陣，從而得到最終的配準(zhǔn)結(jié)果。實(shí)驗(yàn)結(jié)果表明了改進(jìn)PSO-TrICP算法的可靠性和高效性，且該方法對(duì)包含噪聲的點(diǎn)云配準(zhǔn)依然有效。