基于特征點法向量的點云配準算法

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

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

三維激光掃描隨著其技術的發展，正不斷應用到各行各業中。利用三維激光掃描儀對被測物體進行多站、多次掃描，得到不同視角下的三維點云數據，將不同站的多片點云數據進行配準，即可得到完整的被測物體三維點云模型。目前點云配準已經在三維重建、科研醫學、文物保護、測繪工程等領域得到了廣泛的應用。

應用最廣的點云配準算法是文獻[1]中提出的迭代最近點算法(lerative closest point,ICP),該方法是目前點云配準的算法基礎，后人在該算法的基礎上進行改進、延伸。文獻[2]提出了基于八叉樹結構對ICP算法點云配準方法進行改進，加快了點云的搜索速度；文獻[3]提出了基于對偶四元數的三維空間坐標轉換點云配準方法，更加適用于大角度變換的點云配準工作；文獻[4]提出了基于采樣一致性(SAC- IA)的點云配準技術，加快了點云配準的收斂性。但是，上述方法并沒有解決配準時容易陷入局部最優這一問題。

綜上，本文提出一種基于特征點法向量之間的歐氏距離及夾角的點云配準方法。該方法在粗配準階段，首先利用SIFT算法提取特征點；其次根據特征點的法向量之間的歐氏距離對兩片點云的特征點進行自動匹配；然后根據拓撲關系，即特征點法向量之間的夾角對特征點對進行提純；最后利用單位四元數法將兩片點云進行旋轉、平移，為后面的精配準工作提供良好的初始位置，可以有效解決配準時陷入局部最優的問題，也縮短了精配準的時間。

1 粗配準

在進行精配準之前，需要先將待配準的兩片點云進行粗配準，以使其獲得良好的初始位置。

1.1 SIFT算法提取特征點

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

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

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

(1)

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

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

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

(2)

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

利用DoG函數在尺度空間的泰勒二次展開式進行最小二乘擬合，公式下

(3)

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

(4)

1.2 特征點匹配

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

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

1.2.1 求解特征點法向量[8]

某一點法向量的求解是基于該點所在的曲面而獲得的，而點云表征的是一個個離散點，點云數據所記錄的信息是每個獨立點的三維坐標。因此，為了求得每個特征點所對應的法向量，取該點附近一定半徑內的點進行曲面擬合，在擬合面的基礎上求取目標點的法向量。求解過程如下：

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

ax+by+cz=dd≥0

(5)

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

di=|axi+byi+czi-d|

(6)

(7)

(3) 將f分別對4個未知參數d、a、b、c求偏導，從而求出未知參數d，即

(8)

(4) 此時任一點到平面的距離可以改寫為

(9)

(10)

(5) 對式(10)繼續求關于a、b、c的偏導

(11)

(12)

同理

(13)

(14)

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

(15)

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

1.2.2 求解特征點法向量之間的歐氏距離

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

(16)

1.3 特征點對提純

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

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

考慮源點云與目標點云雖然處于不同的空間坐標系，但其空間拓撲關系應該保持一致，相對應的點云之間除了有平移變化，還應有旋轉關系，特征點對的法向量之間的夾角應滿足一定的數學關系[11]。因此，預先對特征點對法向量的夾角設置一個閾值G，用于對誤匹配點進行剔除，從而達到提純的效果。

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

nA·nB≥G

(17)

1.4 點云坐標轉換

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

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

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

(1) 分別計算A和B點云的重心，重心坐標為ZA與ZB

(18)

式中，NA與NB分別為A與B中點云的數量。本文要求兩片點云中的點云數量相等，即NA=NB。

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

(19)

由式(19)所獲得的協方差矩陣，構成一個4×4對稱陣Q(R)

(20)

(21)

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

qT=ZA-R(qR)ZB

(22)

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

1.5 粗配準算法整體流程

(1) 找出源點云與目標點云的公共重疊部分A與B。

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

(3) 計算每個特征點的法向量，并統一方向。

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

(5) 根據點云的空間拓撲關系，計算已配對的特征點之間的法向量夾角。根據數學關系，若大于設定的閾值，則視為誤匹配點，予以剔除。

(7) 將求得的旋轉、平移矩陣應用于源點云。

2 精配準

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

(23)

式中，T、R分別表示平移與旋轉參數；Pi與Qi分別代表目標點云與源點云。

3 試驗分析

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

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

粗配準后精配準結果如圖3所示，直接ICP配準結果如圖4所示。將精配準所用時間、迭代次數、精度與直接ICP所用時間、迭代次數、精度進行對比，見表1。

表1 精配準與直接ICP精度對比

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

4 結 語

本文首先使用SIFT算法提取源點云與目標點云的特征點，并根據特征點的法向量之間的空間拓撲關系對特征點進行配對、提純；接著利用單位四元數法，根據匹配成功的特征點對，計算出源點云與目標點云的旋轉、平移矩陣，為兩片點云進行粗配準。粗配準的結果為精配準提供了良好的初始位置，改善了ICP算法對于大角度轉換情況下的配準工作容易陷入局部最優的問題。從過程與結果來看，本文算法不僅縮短了ICP配準時間，還降低了誤差。同時，需要注意的是，雖然ICP配準時間得到了改善，但是粗配準時使用SIFT算法進行提取特征點仍需要耗費一定的時間，這也是日后該算法需要優化的地方。