基于SHOT 特征提取的三維點云配準算法*

梁古南 潘 豐 王昌龍

（1.江南大學輕工過程先進控制教育部重點實驗室 無錫 214122）（2.無錫信捷電氣股份有限公司 無錫 214072）

1 引言

隨著三維信息獲取設備成本的下降，利用三維信息來處理常見的工程應用如物體的姿態識別和抓取定位等，已是機器視覺領域的研究熱門方向之一。在三維特征描述方面，Salti S 等［1］研究了一種基于三維幾何信息描述的SHOT 局部特征描述子，其特征維度高達352 維。Rusu 等［2］研究了一種33維的FPFH 局部特征描述子。兩種特征描述子都具有較好地實時性，但前者的描述性更加精準。在配準方面，經典的配準算法是Paul J.Besl 等［3］提出的迭代最近點，后續的許多配準算法［4～5］都是在其基礎上的改進。ICP 算法要求配準的兩塊點云有一個較好的初始位姿，否則容易陷入局部最優而得不到精確解。文獻［6］研究了一種基于高斯混合模型的連續點移配準方法，將配準的兩塊點云分類為兩個模型，迭代優化靠近，直到方差達到要求；該方法方向識別性差，耗時大。基于對偶四元素的方法要求配準的點對正確，否則難以求解出合理的位姿參數［7～8］。針對存在的問題，本文設計了一種基于SHOT特征提取的三維點云配準算法可以實現目標點云和模板點云之間位姿參數的精確求解。

2 算法原理

預 處 理 后［9］的 兩 幅 點 云 為P∈R3×n和Q∈R3×m，P為目標點云，Q為模板點云。配準的目的是求得兩者之間的位姿參數即旋轉矩陣和平移向量，使兩者重合在一起。點云配準算法流程如圖1所示。

圖1 點云配準流算法程圖

2.1 初始配準

2.1.1 關鍵點提取

考慮到配準效率，僅需對一些具有顯著特征的點即關鍵點進行計算。本文用ISS算法提取目標點云的關鍵點［10］，該算法有兩個表述特征值之間比例系數的閾值threshold1和threshold2，前者取0.1～0.4，后者取0.6～1。ISS 算法提取關鍵點的步驟如下：

Step1：對目標點云P構建KD樹［11］，置k=1；

Step2：取P第k個點Pk，搜索其一定半徑內的鄰域點；

Step3：用主元分析法獲得該鄰域的三個特征向量和特征值［12］，對特征值升序排序得到λ1,λ2,λ3；

Step4：計算λ1與λ2的比值并記為δ1，計算λ2與λ3的比值并記為δ2；對δ1和δ2進行判斷，若δ1≥threshold1和δ2≤threshold2都成立，則認為該點是關鍵點，保存該點。否則，不保存該點，轉Step5；

Step5：判斷k是否等于n。是，停止遍歷，結束循環；否則，k=k+1轉Step2。

提取的關鍵點集合記為KeypointP∈R3×l，l為關鍵點的數目。半徑的設定為在數據中任選一點，用KD 樹搜索最近的點，得到兩者的距離d，半徑取1d～1.5d。圖2 瓶子關鍵點閾值上下限取0.3 和0.98，半徑取1mm。兔子關鍵點閾值上下限取0.12和0.9，半徑取6mm，可得到和圖2一樣的效果。

圖2 半徑r=1mm瓶子關鍵點顯示

2.1.2 SHOT特征提取

本文用SHOT 特征描述子對點進行特征提取，具體的實現步驟如下。

1）對當前點構建穩健的局部參考坐標系

用主元分析法獲得當前點半徑為r的鄰域的三個特征向量和特征值，對特征值進行升序排序，對應的特征向量作x軸、y軸和z軸，構建局部坐標系。用鄰域點對坐標系的符號進行修正［13］。

2）對局部球型鄰域劃分成32個子塊

對球型鄰域沿徑向、高度和方位角方向劃分成2 份、2 份和8 份，對子塊按順序進行序號編碼，圖3是劃分成2份，2份和4份的球型鄰域示意圖。

圖3 球型鄰域示意圖

3）特征編碼和直方圖統計

點的法向量［14］與z軸正向夾角余弦值范圍在-1～1，將其映射到0～1，把區間[0 ,1] 分成11等份后擴展到長度為11 的區間內，每個子塊可得到一個長度為11 維的向量描述，32 個子塊依次連接形成352 維，統計區間內橫坐標出現的頻數并歸一化。

4）線性插值消除邊界效應

SHOT 描述子是局部特征描述子，相鄰塊之間存在邊界效應。為此需對每個點在當前塊、徑向相鄰塊、高度相鄰塊和方位相鄰塊上進行線性插值。圖4 是方位插值與直方圖統計圖。q點所在塊為1，相鄰塊為2；qy和qx是點q的縱坐標和橫坐標，wq是點q與x軸正向的夾角，wj和wj+1是對應塊的平分線與x軸正向的夾角；cosθi是點q與z軸正向夾角的余弦值，φ為45°。權值分配系數分別為dj和dj+1；其它方向的插值類似，提取的關鍵點特 征 記 為SHOTP∈Rl×352，模 板 特 征 記 為SHOTQ∈Rm×352。圖5 是任選兩個關鍵點，繪制出SHOT特征示意圖。

圖4 方位插值與直方圖統計

圖5 任意兩個關鍵點SHOT特征示意圖

2.1.3 配準點對提取

引入二次約束來提高配準點對的質量［15］。初步提取配準點對的過程為：在KeypointP中隨機采樣f≥3 個點，對每個點在模板特征中搜索與其特征距離最近的點作模板配準點。初步提取的配準點對記為(Di,DTi)，i=1,2,…,f，Di為目標配準點，DTi為模板配準點，模板Q和目標P的質心為Qcenter∈R3×1和Pcenter∈R3×1，距離比值δdis由式（1）計算：

半徑為r的鄰域內各自包含的點數目為NUMQ和NUMP，鄰域比值δNUM描述由式（2）計算：

其中，Di∈R3×1和DTi∈R3×1分別為Q和P中的點，f為初步提取的點對數目，m為Q的點數目，n為P的點數目，半徑r同2.1.1關鍵點鄰域半徑一樣。

理想的δdis和δNUM皆為1，由于干擾的存在，實際的取值時應留有裕量，下上限可分別取0.9 和1.1，即0.9 ≤δdis≤1.1 和0.9 ≤δNUM≤1.1，當約束條件均滿足時，則認定其為一組合適的點對。由于旋轉矩陣的自由度為3，因此至少需要3組點對，最終提取出的配準點對記為(Di,DTi)，i=1,2,…,s，s≥3。圖6是瓶子配準點對提取顯示。

圖6 瓶子配準點對提取顯示

2.1.4 對偶四元數求解旋轉矩陣和平移向量

其中，K()是式（5）生成的斜對稱矩陣，I是單位陣，

旋轉矩陣和平移向量的參數估計步驟如下：

1）根據2.1.3 節提取出配準點對(Di,DTi)，i=1,2,…s，設定初始配準誤差均方根閾值Th1；

2）由式（6）生成矩陣H，進行特征值分解求最大特征值對應的特征向量vˉ，由式（7）解得四元數：

其中，c1和c2是兩個重要的四元數系數矩陣c1=是點DTi和Di對應的四元數，是將和代入式（3）求得的矩陣，是將代入式（4）求得的矩陣的轉置。

3）由式（8）求出Rcoarse∈R3×3和Tcoarse∈R3×1：

其中，是將代入式（4）求得的矩陣，是將代入式（3）求得的矩陣的轉置。

4）由式（9）更新得到Pnew，由式（10）計算誤差均方根Error1RMSE，若Error1RMSE≤Th1，停止迭代輸出Pnew、Rcoarse和Tcoarse，否則，返回步驟1）。

2.2 精確配準

對模板點云Q和2.1.4得到的更新后的目標點云Pnew∈R3×n，ICP算法實現的步驟如下：

1）置迭代次數k=1，設定最大迭代次數kmax，精確配準的誤差均方根閾值Th2，Th2取0.01 mm，初始化Rfinal和Tfinal為2.1.4解得的Rcoarse和Tcoarse；

2）對Pnew中的每個點Pnew(i)在Q中搜索與其距離最近的點Qnew(i)，最近點的集合記為Qnew∈R3×n；

3）由式（11）計算Qnew和Pnew的質心uQ和uP，由式（12）對進行去質心操作，得到和；

5）對ConvQP進行奇異值分解［16］得到左奇異向量U∈R3×3和右奇異向量V∈R3×3，求得本次迭代的旋轉矩陣和平移矩陣Ticp=uQ-

7）由式（13）計算均方根誤差Error2RMSE；

若Error2RMSE＞Th2或k≤kmax，更新Pnew的每個點賦值給Pnew，k=k+1 轉步驟2）繼續迭代；否則停止迭代，輸出Rfinal和Tfinal。

3 仿真與分析

本算法試驗的軟件平臺為Matlab R2014b，試驗數據采用盛相科技公司的3D 結構光相機Sizector R掃描獲取的瓶子三維點云數據和斯坦福3D庫里的兔子模型，分別對算法進行驗證和對比分析。

3.1 驗證環節

1）瓶子的長、寬和高為6cm、2.5cm 和5cm，點數為1500 點左右，關鍵點提取半徑取1mm，閾值上下限分別取0.98和0.3，特征提取半徑為4mm，鄰域約束半徑取4mm。以模板和目標之間的方位差為180°為例給出仿真結果，其它角度經試驗均可得到驗證。配準效果如圖7 所示，可以看出兩者較好地貼合在一起，配準成功，驗證了算法的可行性。

圖7 瓶子配準效果圖

2）兔子的長、寬和高為18cm、10cm 和15cm，點數為1300 點左右，關鍵點提取半徑取4mm，閾值上下限取0.9 和0.12，特征提取半徑取10mm，鄰域約束半徑取10mm。這給出模板與目標之間的方位差為45°的配準效果圖，如圖8所示。

圖8 兔子配準效果圖

3.2 對比環節

表1記錄了點云配準試驗數據，模型一1200點左右，數據缺失大，模型二1500 點左右，數據較完整。對比模型一和二的試驗數據發現當目標數據較完整時由于SHOT 特征描述性更加準確，點對提取耗時相對更短。但當目標數據點缺失大時，特征的描述性會下降，相應的也會增加點對提取耗時，從而加大整個初始配準的時間開銷，從本文方法在模型一和模型二下的數據縱向對比中得到。總的來說，基于本文的方法比基于FPFH+ICP 方法耗時更短。基于傳統ICP 的配準方對數據的方位要求苛刻，適應性差。從表1 可以看出當模板與目標之間的方位超出范圍后，對于模型一和二都不能實現配準。對比三種方法的實驗結果可以得出本文的方法能適應數據的不同位姿。

表1 點云配準試驗結果

本文試驗了其它常見的模型如Bunny 兔等，在參數設置合理的情況下可在較短時間內得到較好的配準效果。

4 結語

本文設計一種基于SHOT 特征提取的三維點云配準算法，分為初始配準和精確配準。首先對目標點云進行關鍵點提取，在關鍵點中隨機采樣并計算其特征，在模板特征中結合二次約束搜索出合適的配準點對，用對偶四元數求解位姿參數完成初始配準。用迭代最近點算法實現最終位姿參數的精確求解完成精確配準。試驗表明本文方法能縮小配準耗時，適應目標與模板之間的不同位姿，在數據缺失的情況下也能夠得到較好的配準結果。