基于特征點(diǎn)法向量的點(diǎn)云配準(zhǔn)算法

孫培芪，卜俊洲，陶庭葉，房興博，賀 晗，馮佳琪

(合肥工業(yè)大學(xué)土木與水利工程學(xué)院，安徽 合肥 230009)

三維激光掃描隨著其技術(shù)的發(fā)展，正不斷應(yīng)用到各行各業(yè)中。利用三維激光掃描儀對(duì)被測(cè)物體進(jìn)行多站、多次掃描，得到不同視角下的三維點(diǎn)云數(shù)據(jù)，將不同站的多片點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)，即可得到完整的被測(cè)物體三維點(diǎn)云模型。目前點(diǎn)云配準(zhǔn)已經(jīng)在三維重建、科研醫(yī)學(xué)、文物保護(hù)、測(cè)繪工程等領(lǐng)域得到了廣泛的應(yīng)用。

應(yīng)用最廣的點(diǎn)云配準(zhǔn)算法是文獻(xiàn)[1]中提出的迭代最近點(diǎn)算法(lerative closest point,ICP),該方法是目前點(diǎn)云配準(zhǔn)的算法基礎(chǔ)，后人在該算法的基礎(chǔ)上進(jìn)行改進(jìn)、延伸。文獻(xiàn)[2]提出了基于八叉樹(shù)結(jié)構(gòu)對(duì)ICP算法點(diǎn)云配準(zhǔn)方法進(jìn)行改進(jìn)，加快了點(diǎn)云的搜索速度；文獻(xiàn)[3]提出了基于對(duì)偶四元數(shù)的三維空間坐標(biāo)轉(zhuǎn)換點(diǎn)云配準(zhǔn)方法，更加適用于大角度變換的點(diǎn)云配準(zhǔn)工作；文獻(xiàn)[4]提出了基于采樣一致性(SAC- IA)的點(diǎn)云配準(zhǔn)技術(shù)，加快了點(diǎn)云配準(zhǔn)的收斂性。但是，上述方法并沒(méi)有解決配準(zhǔn)時(shí)容易陷入局部最優(yōu)這一問(wèn)題。

綜上，本文提出一種基于特征點(diǎn)法向量之間的歐氏距離及夾角的點(diǎn)云配準(zhǔn)方法。該方法在粗配準(zhǔn)階段，首先利用SIFT算法提取特征點(diǎn)；其次根據(jù)特征點(diǎn)的法向量之間的歐氏距離對(duì)兩片點(diǎn)云的特征點(diǎn)進(jìn)行自動(dòng)匹配；然后根據(jù)拓?fù)潢P(guān)系，即特征點(diǎn)法向量之間的夾角對(duì)特征點(diǎn)對(duì)進(jìn)行提純；最后利用單位四元數(shù)法將兩片點(diǎn)云進(jìn)行旋轉(zhuǎn)、平移，為后面的精配準(zhǔn)工作提供良好的初始位置，可以有效解決配準(zhǔn)時(shí)陷入局部最優(yōu)的問(wèn)題，也縮短了精配準(zhǔn)的時(shí)間。

1 粗配準(zhǔn)

在進(jìn)行精配準(zhǔn)之前，需要先將待配準(zhǔn)的兩片點(diǎn)云進(jìn)行粗配準(zhǔn)，以使其獲得良好的初始位置。

1.1 SIFT算法提取特征點(diǎn)

SIFT算法是David Lowe于1999年提出的局部特征描述子，并于2004年進(jìn)行了更深入的發(fā)展和完善[5]。SIFT特征是基于尺度不變性的圖像局部特征，對(duì)平移、旋轉(zhuǎn)、尺度縮放、亮度變化、遮擋和噪聲等具有良好的不變性，對(duì)視覺(jué)變化、仿射變換也保持一定的穩(wěn)定性，特別適用于在海量數(shù)據(jù)庫(kù)進(jìn)行快速、準(zhǔn)確的匹配。

SIFT算法的原理[6]可以簡(jiǎn)單描述為：一幅圖像在尺度不同的尺度空間L(x,y,σ)定義為一個(gè)變化尺度的高斯函數(shù)G(x,y,σ)與原圖像I(x,y)的卷積，即

L(x,y,σ)=G(x,y,σ)*I(x,y)

(1)

式中，(x,y)表示圖像的像素位置；*為卷積運(yùn)算；σ是尺度空間因子。使用高效的高斯差分算子D(x,y,σ)進(jìn)行極值檢測(cè)，以此來(lái)選擇出尺度空間中的穩(wěn)定關(guān)鍵點(diǎn),具體如下

D(x,y,σ)=(G(x,y,kσ)-G(x,y,σ))*I(x,y)=

L(x,y,kσ)-L(x，y,σ)

(2)

式中，k為一個(gè)常量。特征點(diǎn)是由DoG空間的局部極值點(diǎn)組成的，通過(guò)同一組內(nèi)各DoG相鄰兩層圖像之間比較完成特征點(diǎn)的探查。為了尋找DoG函數(shù)的極值點(diǎn)，每一個(gè)像素點(diǎn)要與同尺度的8個(gè)相鄰點(diǎn)和上下相鄰尺度對(duì)應(yīng)的18個(gè)點(diǎn)共26個(gè)點(diǎn)進(jìn)行比較，如果該像素點(diǎn)的DoG算子值在這26個(gè)鄰域中為極值，則將該點(diǎn)定義為特征點(diǎn)。目前得到的特征點(diǎn)是離散空間的極值點(diǎn)，還要通過(guò)擬合三維曲面來(lái)精確確定特征點(diǎn)的位置和尺度，同時(shí)去除低對(duì)比度的特征點(diǎn)和不穩(wěn)定的邊緣點(diǎn)，以增強(qiáng)匹配穩(wěn)定性，提高抗噪聲能力。

利用DoG函數(shù)在尺度空間的泰勒二次展開(kāi)式進(jìn)行最小二乘擬合，公式下

(3)

式中，X=(x,y,σ)T。求導(dǎo)并讓方程等于0，可以得到極值點(diǎn)的偏移量為

(4)

1.2 特征點(diǎn)匹配

兩片點(diǎn)云公共部分的特征點(diǎn)提取出之后，要將特征點(diǎn)進(jìn)行兩兩匹配，采用特征點(diǎn)的法向量之間的歐氏距離作為兩片點(diǎn)云中特征點(diǎn)相似性判定度量[7]。選取源點(diǎn)云中某一特征點(diǎn)，計(jì)算出該點(diǎn)的法向量ni與目標(biāo)點(diǎn)云的所有特征點(diǎn)的法向量nj之間的歐氏距離，并相互比較選取出歐氏距離最小的兩個(gè)點(diǎn)。如果最小距離與次最小距離相比小于預(yù)先設(shè)定的某個(gè)閾值τ(τ>0)，則目標(biāo)點(diǎn)云中與源點(diǎn)云中特征點(diǎn)法向量歐氏距離最近點(diǎn)為一對(duì)特征點(diǎn)對(duì)。若降低閾值，則匹配點(diǎn)對(duì)數(shù)量會(huì)減少，但是會(huì)增加匹配的穩(wěn)定性，故剔除一部分錯(cuò)誤匹配點(diǎn)對(duì)。

需要注意的是，每個(gè)特征點(diǎn)的法向量是具有正負(fù)之分的[7]。因此，為保證掃描對(duì)象表面法向量方向的一致性，對(duì)所有求得的法向量進(jìn)行方向調(diào)整，使之滿足ni·nj<0(i≠j)，使得特征點(diǎn)對(duì)的匹配更加準(zhǔn)確。

1.2.1 求解特征點(diǎn)法向量[8]

某一點(diǎn)法向量的求解是基于該點(diǎn)所在的曲面而獲得的，而點(diǎn)云表征的是一個(gè)個(gè)離散點(diǎn)，點(diǎn)云數(shù)據(jù)所記錄的信息是每個(gè)獨(dú)立點(diǎn)的三維坐標(biāo)。因此，為了求得每個(gè)特征點(diǎn)所對(duì)應(yīng)的法向量，取該點(diǎn)附近一定半徑內(nèi)的點(diǎn)進(jìn)行曲面擬合，在擬合面的基礎(chǔ)上求取目標(biāo)點(diǎn)的法向量。求解過(guò)程如下：

(1) 待擬合的n個(gè)掃描點(diǎn)(xi,yi,zi),i=1,2,…，n，擬合的平面方程為

ax+by+cz=dd≥0

(5)

式中，a、b、c應(yīng)滿足a2+b2+c2=1。且任一點(diǎn)(xi,yi,zi)到該平面的距離為

di=|axi+byi+czi-d|

(6)

(7)

(3) 將f分別對(duì)4個(gè)未知參數(shù)d、a、b、c求偏導(dǎo)，從而求出未知參數(shù)d，即

(8)

(4) 此時(shí)任一點(diǎn)到平面的距離可以改寫(xiě)為

(9)

(10)

(5) 對(duì)式(10)繼續(xù)求關(guān)于a、b、c的偏導(dǎo)

(11)

(12)

同理

(13)

(14)

(6) 將式(12)—式(14)合并改寫(xiě)為

(15)

求解出(a,b,c)T即為該點(diǎn)在該平面的法向量。

1.2.2 求解特征點(diǎn)法向量之間的歐氏距離

從源點(diǎn)云中選取一個(gè)特征點(diǎn)，其法向量為(ai,bi,ci)T。從目標(biāo)點(diǎn)云中選取一個(gè)特征點(diǎn)，其法向量為(aj,bj,cj)T，則公式為兩個(gè)法向量之間的歐氏距離計(jì)算

(16)

1.3 特征點(diǎn)對(duì)提純

在特征點(diǎn)配對(duì)時(shí)設(shè)置的閾值，并不能將特征點(diǎn)完全正確地一一對(duì)應(yīng)，誤匹配的現(xiàn)象始終無(wú)法避免。在理論上，只要提取出3對(duì)正確的匹配點(diǎn)對(duì)，即可求解出準(zhǔn)確的兩片點(diǎn)云之間的旋轉(zhuǎn)、平移矩陣[9]。但若其中1對(duì)特征點(diǎn)對(duì)存在誤匹配，則計(jì)算出的變換矩陣與實(shí)際變換矩陣之間便會(huì)存在很大的差異。因此，為了保證求解出源點(diǎn)云與目標(biāo)點(diǎn)云之間的旋轉(zhuǎn)與平移矩陣的準(zhǔn)確性，需要將配對(duì)后的特征點(diǎn)對(duì)進(jìn)行更深一步的提純工作。

目前，匹配點(diǎn)提純算法常用的是隨機(jī)抽樣一致性算法(random sample consensus，RANSAC)[10]，采用迭代的方式從一組包含離群的被觀測(cè)數(shù)據(jù)中估算出數(shù)學(xué)模型的參數(shù)。數(shù)據(jù)分為有效數(shù)據(jù)與無(wú)效數(shù)據(jù)兩種。隨機(jī)抽樣一致性算法就是根據(jù)不同的數(shù)據(jù)建立不同的模型，滿足該模型的數(shù)據(jù)為有效數(shù)據(jù)，不滿足該模型的數(shù)據(jù)為無(wú)效數(shù)據(jù)，從而剔除無(wú)效數(shù)據(jù)，將誤匹配的特征點(diǎn)對(duì)進(jìn)行排除。從結(jié)果上來(lái)看，該算法獲得的模型數(shù)據(jù)穩(wěn)健性較強(qiáng)，估算出的參數(shù)精度相對(duì)較高。但是，該算法每次針對(duì)不同的數(shù)據(jù)，所擬合的模型均不相同，故不適用于復(fù)雜的點(diǎn)云場(chǎng)景。并且，計(jì)算模型參數(shù)的迭代次數(shù)沒(méi)有上限，如果設(shè)置迭代次數(shù)的上限，得到的結(jié)果可能不是最優(yōu)結(jié)果，甚至可能會(huì)得到錯(cuò)誤的結(jié)果。故本文采取一種根據(jù)特征點(diǎn)對(duì)之間法向量的夾角作為評(píng)判量的匹配點(diǎn)提純算法。

考慮源點(diǎn)云與目標(biāo)點(diǎn)云雖然處于不同的空間坐標(biāo)系，但其空間拓?fù)潢P(guān)系應(yīng)該保持一致，相對(duì)應(yīng)的點(diǎn)云之間除了有平移變化，還應(yīng)有旋轉(zhuǎn)關(guān)系，特征點(diǎn)對(duì)的法向量之間的夾角應(yīng)滿足一定的數(shù)學(xué)關(guān)系[11]。因此，預(yù)先對(duì)特征點(diǎn)對(duì)法向量的夾角設(shè)置一個(gè)閾值G，用于對(duì)誤匹配點(diǎn)進(jìn)行剔除，從而達(dá)到提純的效果。

對(duì)于任意特征點(diǎn)對(duì)A和B，它們的法向量分別為nA和nB，它們之間的法向量差別越大，其夾角的余弦值越小，即nA·nB就越小。因此，根據(jù)式(17)對(duì)法向量之間的夾角余弦設(shè)置閾值G，將法向量的乘積小于G的點(diǎn)對(duì)視為誤匹配點(diǎn)，并將其剔除。

nA·nB≥G

(17)

1.4 點(diǎn)云坐標(biāo)轉(zhuǎn)換

當(dāng)源點(diǎn)云與目標(biāo)點(diǎn)云的特征點(diǎn)完成一一對(duì)應(yīng)后，便要利用特征點(diǎn)對(duì)求解源點(diǎn)云到目標(biāo)點(diǎn)云的平移和旋轉(zhuǎn)矩陣。常用的三維坐標(biāo)轉(zhuǎn)換方法是利用布爾沙模型求得七參數(shù)[12]，其中包括3個(gè)坐標(biāo)平移參數(shù)、3個(gè)角度旋轉(zhuǎn)參數(shù)及1個(gè)尺度參數(shù)。由于在點(diǎn)云配準(zhǔn)問(wèn)題中，不涉及點(diǎn)云尺度的大小變換，因此僅使用前6個(gè)參數(shù)對(duì)源點(diǎn)云進(jìn)行平移、旋轉(zhuǎn)。但是，該方法含有線性化后產(chǎn)生的不穩(wěn)定誤差，僅在小角度轉(zhuǎn)換情況下，可以將轉(zhuǎn)換參數(shù)的誤差忽略，而在坐標(biāo)轉(zhuǎn)換角度較大時(shí)，其精度則達(dá)不到轉(zhuǎn)換的要求。故本文選用單位四元數(shù)法，該方法更適用于大角度三維坐標(biāo)轉(zhuǎn)換。

四元數(shù)由哈密頓于1843年提出，其實(shí)質(zhì)是1種簡(jiǎn)單的超復(fù)數(shù)，由1個(gè)實(shí)部單位和3個(gè)虛部單位構(gòu)成。使用單位四元數(shù)法進(jìn)行點(diǎn)云三維坐標(biāo)轉(zhuǎn)換時(shí)，必須找到兩片點(diǎn)云中的公共部分，即求得源點(diǎn)云與目標(biāo)點(diǎn)云的重疊部分的旋轉(zhuǎn)、平移矩陣，從而應(yīng)用到源點(diǎn)云整體部分的轉(zhuǎn)換[13]。其使用的具體條件為：①分別選取源點(diǎn)云與目標(biāo)點(diǎn)云的公共部分A和B，且A和B兩片點(diǎn)云中的點(diǎn)云數(shù)量相等；②A和B兩片點(diǎn)云中的點(diǎn)云要滿足一一對(duì)應(yīng)的關(guān)系，即Ai與Bj代表著在不同坐標(biāo)系下的相同點(diǎn)位。在前文中，將源點(diǎn)云與目標(biāo)點(diǎn)云公共部分的特征點(diǎn)進(jìn)行一一匹配，獲得的匹配點(diǎn)對(duì)即可滿足單位四元數(shù)法的使用要求。

單位四元數(shù)法的具體算法如下[14]：

(1) 分別計(jì)算A和B點(diǎn)云的重心，重心坐標(biāo)為ZA與ZB

(18)

式中，NA與NB分別為A與B中點(diǎn)云的數(shù)量。本文要求兩片點(diǎn)云中的點(diǎn)云數(shù)量相等，即NA=NB。

(2) 求A與B的協(xié)方差矩陣，即

(19)

由式(19)所獲得的協(xié)方差矩陣，構(gòu)成一個(gè)4×4對(duì)稱(chēng)陣Q(R)

(20)

(21)

(4) 求得旋轉(zhuǎn)矩陣后，即可求解出平移矩陣qT

qT=ZA-R(qR)ZB

(22)

旋轉(zhuǎn)矩陣R(qR)與平移矩陣qT即為源點(diǎn)云與目標(biāo)點(diǎn)云之間的變換關(guān)系。利用該旋轉(zhuǎn)、平移矩陣可調(diào)整源點(diǎn)云的三維坐標(biāo)，為后面的精配準(zhǔn)工作提供了較好的初始位置。

1.5 粗配準(zhǔn)算法整體流程

(1) 找出源點(diǎn)云與目標(biāo)點(diǎn)云的公共重疊部分A與B。

(2) 使用SIFT算法提取出A與B的公共點(diǎn)。

(3) 計(jì)算每個(gè)特征點(diǎn)的法向量，并統(tǒng)一方向。

(4) 將A中某一特征點(diǎn)的法向量與B中所有特征點(diǎn)的法向量計(jì)算歐氏距離并選取出距離最近的兩個(gè)點(diǎn)，其距離比值若小于閾值，則距離最小的兩個(gè)點(diǎn)進(jìn)行匹配。

(5) 根據(jù)點(diǎn)云的空間拓?fù)潢P(guān)系，計(jì)算已配對(duì)的特征點(diǎn)之間的法向量夾角。根據(jù)數(shù)學(xué)關(guān)系，若大于設(shè)定的閾值，則視為誤匹配點(diǎn)，予以剔除。

(7) 將求得的旋轉(zhuǎn)、平移矩陣應(yīng)用于源點(diǎn)云。

2 精配準(zhǔn)

經(jīng)過(guò)粗配準(zhǔn)以后，源點(diǎn)云與目標(biāo)點(diǎn)云已經(jīng)獲得了較好的初始位置，在此基礎(chǔ)上采用ICP迭代算法進(jìn)行精配準(zhǔn)。ICP算法根據(jù)源點(diǎn)云與目標(biāo)點(diǎn)云之間的幾何特征求解其運(yùn)動(dòng)參數(shù)(即旋轉(zhuǎn)與平移)，再代入這些運(yùn)動(dòng)參數(shù)對(duì)數(shù)據(jù)進(jìn)行轉(zhuǎn)化，得到新的目標(biāo)點(diǎn)云與源點(diǎn)云，從而繼續(xù)確定配準(zhǔn)點(diǎn)云之間新的對(duì)應(yīng)關(guān)系，不斷重復(fù)上述過(guò)程。當(dāng)源點(diǎn)云與目標(biāo)點(diǎn)云經(jīng)過(guò)不斷旋轉(zhuǎn)與平移，從而達(dá)到目標(biāo)函數(shù)最小，即滿足最小二乘定理時(shí)，那么配準(zhǔn)效果達(dá)到最優(yōu)[15- 16]。目標(biāo)函數(shù)表示如下

(23)

式中，T、R分別表示平移與旋轉(zhuǎn)參數(shù)；Pi與Qi分別代表目標(biāo)點(diǎn)云與源點(diǎn)云。

3 試驗(yàn)分析

為驗(yàn)證上述方法的點(diǎn)云配準(zhǔn)效果，本文采用經(jīng)典的斯坦福兔子作為試驗(yàn)素材。將兩個(gè)不同視角的斯坦福兔子點(diǎn)云數(shù)據(jù)分別作為源點(diǎn)云與目標(biāo)點(diǎn)云，其中亮色的為源點(diǎn)云，暗色的為目標(biāo)點(diǎn)云。本文算法使用C++編程實(shí)現(xiàn)，程序運(yùn)行環(huán)境為Intel(R) Core i5- 7500處理器，內(nèi)存16 GB。為了驗(yàn)證在交換角度較大時(shí)本文算法的可靠性，設(shè)置其初始位置如圖1所示。

在本次試驗(yàn)中，根據(jù)試驗(yàn)數(shù)據(jù)將特征點(diǎn)匹配的閾值τ1設(shè)定為0.05，將匹配點(diǎn)對(duì)提純的閾值τ2設(shè)定為0.8。試驗(yàn)中，每個(gè)兔子點(diǎn)云有35 947個(gè)點(diǎn)，提取出的特征點(diǎn)為1280個(gè)點(diǎn)。在此基礎(chǔ)上，滿足閾值要求，最終匹配成功的特征點(diǎn)對(duì)為209對(duì)。將特征點(diǎn)對(duì)代入四元數(shù)法后，求得旋轉(zhuǎn)、平移矩陣，并應(yīng)用于源點(diǎn)云，完成粗配準(zhǔn)，如圖2所示。

粗配準(zhǔn)后精配準(zhǔn)結(jié)果如圖3所示，直接ICP配準(zhǔn)結(jié)果如圖4所示。將精配準(zhǔn)所用時(shí)間、迭代次數(shù)、精度與直接ICP所用時(shí)間、迭代次數(shù)、精度進(jìn)行對(duì)比，見(jiàn)表1。

表1 精配準(zhǔn)與直接ICP精度對(duì)比

從圖4中可以看出，在大角度轉(zhuǎn)換的情況下進(jìn)行直接ICP配準(zhǔn)，使得兩片點(diǎn)云陷入了局部最優(yōu)。而先經(jīng)過(guò)粗配準(zhǔn)后，再使用ICP算法，配準(zhǔn)結(jié)果得到了很大的改善，兩片點(diǎn)云幾乎完全重疊。另外，由于為ICP算法提供了較好的初始位置，因此相應(yīng)的迭代次數(shù)與迭代時(shí)間都進(jìn)一步縮短，配準(zhǔn)誤差進(jìn)一步降低。

4 結(jié) 語(yǔ)

本文首先使用SIFT算法提取源點(diǎn)云與目標(biāo)點(diǎn)云的特征點(diǎn)，并根據(jù)特征點(diǎn)的法向量之間的空間拓?fù)潢P(guān)系對(duì)特征點(diǎn)進(jìn)行配對(duì)、提純；接著利用單位四元數(shù)法，根據(jù)匹配成功的特征點(diǎn)對(duì)，計(jì)算出源點(diǎn)云與目標(biāo)點(diǎn)云的旋轉(zhuǎn)、平移矩陣，為兩片點(diǎn)云進(jìn)行粗配準(zhǔn)。粗配準(zhǔn)的結(jié)果為精配準(zhǔn)提供了良好的初始位置，改善了ICP算法對(duì)于大角度轉(zhuǎn)換情況下的配準(zhǔn)工作容易陷入局部最優(yōu)的問(wèn)題。從過(guò)程與結(jié)果來(lái)看，本文算法不僅縮短了ICP配準(zhǔn)時(shí)間，還降低了誤差。同時(shí)，需要注意的是，雖然ICP配準(zhǔn)時(shí)間得到了改善，但是粗配準(zhǔn)時(shí)使用SIFT算法進(jìn)行提取特征點(diǎn)仍需要耗費(fèi)一定的時(shí)間，這也是日后該算法需要優(yōu)化的地方。