基于矩陣指數的點云配準方法

楊正世，李文國

(昆明理工點大學機電工程學院，云南 昆明 650500)

0 引言

點云配準技術是研究不同視角，即不同參考坐標下的一組點云或多組點云。通過某種算法，將具有一定重疊度的點云數據，通過平移旋轉縮放等操作變換到同一參考坐標系下，進而得到實體完整的點云數據信息[1]。它是整個三維點云數據處理中的關鍵步驟，不但為后期的點云數據建模提供了較為精確的點云數據，而且為構建高精度的模型提供了保證。該技術廣泛應用于3D游戲[2]、物體識別[3]、大壩測量[4]、文物保護[5]、醫療矯形[6]、機器人點位導航及自動地圖構建[7-8]等領域。

點云配準的關鍵在于求取變換矩陣。目前常用的點云配準算法，是由Paual J和Besl[9]在1992年提出的最近點迭代(iterative closest point,ICP)算法以及基于ICP算法的改進算法。點云配準優化問題，可以轉化為求取一個有6個自由度的剛體變換M(R,T)，將兩片點云配準到同一坐標系下，使配準點云之間的距離誤差達到最小。ICP算法對初始點云對的對齊要求非常高。如果初始點云對未能對齊，將會使ICP算法陷入局部最優。初始點云對對齊，將減少迭代的次數，提高點云配準的速度和精度。

目前，求解位置變換矩陣的主要方法如下。Beltrami和Jordan提出的奇異值分值(singular value decomposition，SVD)[10]法，求解的變換矩陣精度高，但是需多次迭代，會影響點云配準的速度。文獻[11]對Horn[12]提出的四元素表示法進行了細致的描述。列文伯格-馬夸爾特法(Levenberg-Marquardt,LM)[13]算法能夠借助高斯-牛頓算法以及梯度下降法的優點，并對兩者的不足之處作了改善;但是其局部搜索耗時大，求得的變換矩陣精度不高。因此，本文提出了一種基于矩陣指數表示變換矩陣的點云配準方法，從而提高求取變換矩陣的速度和精度，以及ICP算法的收斂速度和計算精度。

1 算法描述

1.1 目標函數

如前文所述，點云配準優化問題，可以轉化為求取一個有6個自由度的剛體變換M(R,T)，將2片點云配準到同一坐標系下，使配準點云之間的距離誤差達到最小。假設待配準點云集P={p1,p2,…,pi}，1≤i≤n},Q={q1,q2,…,qi}，1≤i≤m。其中：n、m分別為P和Q中點云的總數。用R表示旋轉變換矩陣，T表示平移變換向量，則ε(R,T)表示源點集Q在變換矩陣M(R,T)的作用下與目標點集P之間的誤差。因此，求解點云配準優化的問題就可以轉化為求解滿足 min[ε(R,T)]的最優解，可以定義為：

(1)

式中:qi為點云集Q中的一個點云數據；pi為點云集P中的一個數據點；R∈SO(3)；T∈R3。

假如(pi，qi)為對齊的點云對，ni為它們之間單個點云的法向量，如需確定最佳的變換矩陣M(R,T)，以實現點云集P和點云集Q配準，則可將式(1)變換為：

(2)

式中：ni為點云qi點的法向量；(piR+T-qi)為點云pi到點云qi的一種線性轉換；[(piR+T-qi)ni]2為piR+T點到qi平面距離的平方。

1.2 求解變換矩陣

(3)

(4)

由指數ex展開可知，當|x|<1時，ex≈1+x。所以，當坐標旋量‖ω‖<1時，式(4)可以表示為：

(5)

T=v

(6)

將式(5)和式(6)代入式(2)，可得：

(7)

定義a=P×n，則式(2)可以重寫為：

(8)

要使式(8)有最小值，則要先求取函數ε(R,T)關于ω和v參數的偏導數。當偏導數為零，即可求出參數的值。求解偏導公式為：

(9)

將式(9)表示為Ax=b的線性矩陣方程。其中：A為6×6的協方差矩陣，可以用已知的ai和ni表示；x為6×1的未知向量；b為6×1的向量，可以通過對應的點云對(pi，qi)和ai和ni表示。求解Ax=b的線性矩陣方程，就可以得出旋轉矩陣R和平移向量T，即完成變換矩陣求解。從式(9)可知，求解變換矩陣只需要6個方程，算法易于實現，且不需要構造其他的模型函數。在LM算法中，需要構造模型函數f，并在其領域內對估計參數向量p作線性近似變換。如未能較好地構造模型函數構，將直接影響變換矩陣的求解。在SVD算法中，需要計算點云的質心，然后構造協方差矩陣。該方法計算量大，迭代次數多。從式(9)可以看出，本文方法很好地避免了上述兩種算法的不足，能夠快速、精確地求出變換矩陣。

1.3 點云配準

為了加快對點云對(pi，qi)最近點的搜索，采用K-D tree搜索法來尋求最近點，提高整體的點云配準速度。如前文所述，ICP算法對初始位置對齊的點云對依耐性較高。采用隨機采樣一致性算法(random sample consensus，RANSAC)[15]去除錯誤的點云，以減少計算迭代的次數，并結合本文算法來求出變換矩陣，完成點云配準。 具體的點云配準流程如圖1所示。

圖1 點云配準流程圖

2 試驗結果與分析

2.1 試驗評估及試驗環境說明

試驗從以下2個方面進行評估。

②在相同的條件下，分別與基于SVD算法的ICP算法和基于LM算法的ICP算法，比較配準的精度和配準的運行時間。

本文所有試驗的計算機配置為：AMD A8 2.1 GHz CPU、4 GB內存，WIN7 64位操作系統。點云配準方法采用C++語言在Visual 2010上編程實現，并調用PCL 1.7.2版本[16]的點云庫顯示和保存點云。

為了驗證本文方法的有效性，試驗中的點云數據采用斯坦福大學提供的Bunny、Happy和Dragon數據樣本。數據大小分別為：35 947個Bunny數據，32 328個Happy數據，22 998個Dragon數據。

2.2 配準的結果展示

2.2.1 Bunny配準結果

將Bunny的點云數據分別代入基于SVD算法、LM算法以及本文方法。3種算法各自迭代30次，Bunny點云配準的結果如圖2所示。

圖2 Bunny點云配準結果

從圖2可知，本文方法和SVD算法優于LM算法。在Bunny的耳朵處，LM算法配準得不是很好，出現大量的不配準點。為了客觀地表示Bunny點云配準的結果，分別對上面3種算法迭代不同的次數，比較其配準時間和配準精度，結果如表1所示。

表1 Bunny點云配準精度和時間對比

從表1可以得出,本文方法配準精度和速度明顯優于LM算法和SVD算法。LM算法在迭代15次時配準精度出現最高，精度數量級達到10-8；在迭代20次時精度降低，然后隨著迭代次數的增加精度提高。SVD算法在迭代30次時達到局部最優，得到最佳的匹配，精度數量級達到10-14。本文方法在迭代15次時，精度數量級達就到10-16，明顯超過其他2種算法的最佳配準精度數量級。在3種算法分別迭代40次時，LM算法在速度上是最慢的，本文方法最快，所用時間是SVD算法所要時間的6倍、LM算法的11倍。很明顯，本文方法在速度上明顯優于其他2種算法。

2.2.2 Happy配準結果

將Happy的點云數據分別代入3種算法。3種算法各自迭代30次的配準結果如圖3所示。

由圖3可知，本文方法和LM算法優于SVD算法。SVD算法在Happy的上部分右邊處配準得不是很好。

同樣地，為了客觀表示Happy點云配準的結果，分別對3種算法迭代不同的次數，比較其配準時間和配準精度，結果如表2所示。

圖3 Happy點云配準結果

迭代次數LM算法精度/mt/msSVD算法精度/mt/ms本文方法精度/mt/ms103.56e-0646 9076.49e-0610 3132.10e-0710 098157.27e-0770 4423.13e-0617 4314.15e-1614 262202.48e-0797 3641.52e-0622 7884.84e-1614 689256.48e-09118 3567.15e-0728 1135.89e-1615 621301.27e-12141 5893.92e-0733 6566.87e-1615 789356.48e-16161 3703.37e-1439 3626.87e-1616 216408.90e-16167 4094.02e-1441 4416.87e-1616 542

由表2可知，LM算法從迭代初始次數時配準精度就高于SVD算法，在迭代40次時配準精度高于本文方法。但是，本文方法收斂速度仍然是3種算法中最快的。在迭代15次時，本文方法精度數量級達就到10-16、LM算法精度數量級為10-7、SVD算法精度數量級為10-6，證明本文方法配準精度遠高于其他2種算法。在配準時間上，LM算法總大于SVD算法和本文方法。LM算法迭代10次所花時間大于SVD算法迭代40次的時間，本文方法的配準時間仍然是最小的，明顯小于其他2種算法。

2.2.3 Dragon配準結果

將Dragon的點云數據分別代入3種SVD算法。3種算法各自迭代30次，Dragon點云配準結果如圖4所示。

從圖4可知，3種算法的配準效果都較好。同樣地，為了客觀表示Dragon點云配準的結果，分別采用上面3種算法迭代不同的次數，比較其配準時間和配準精度，結果如表3所示。

圖4 Dragon點云配準結果

迭代次數LM算法精度/mt/msSVD算法精度/mt/ms本文方法精度/mt/ms103.39e-0636 7763.17e-067 5935.10e-087 174152.81e-0654 5614.42e-0711 5563.36e-127 531209.13e-1169 8184.14e-1414 5353.76e-177 900257.96e-1183 0254.66e-1414 8533.76e-178 156307.61e-1195 7604.66e-1415 2303.76e-178 289357.33e-11111 3114.66e-1415 5493.76e-178 366407.06e-11124 1174.66e-1415 7423.76e-178 741

由表3可知，從精度上看，LM算法是3種算法中最差的一種，在迭代次數超過25次時收斂幾乎保持不變，在整過迭代過程中沒有出現局部最優值； SVD算法在迭代20次時超過LM算法，在迭代25次時出現局部最優值；本文方法在迭代20次時就出現局部最優值，得到最佳的配準結果，配準精度結果為10-17，在精度數量級上超過LM算法和SVD算法。從時間上看，本文方法在配準時間仍然最少的，SVD算法次之，LM算法最多。本文方法在配準精度和速度都好于其他兩種算法。

2.2.4 單片點云配準結果

在以上試驗中，分別比較了3種算法對完整點云數據的配準結果。為了驗證本文方法的普遍有效性，對單片非閉合的點云進行配準。數據采用了Bunny的頭部,點云數據量為23 317; Happy的底座,點云數據量為6 823; Dragon的右部分,點云數據量為12 414。分別采用3種算法對以上點云數據進行處理，各自迭代30次，單片點云配準結果如圖5所示。

圖5 單片點云配準結果

由圖5可知，第1行中Bunny的頭部，3種算法配準精度無明顯的差別；第2行中Happy的底座，LM算法和本文方法明顯優于SVD算法；第3行中Dragon的右部分腿，LM算法比本文方法和SVD算法差。為了更加精確、直觀地描述3種算法的配準精度和速度，將3種算法各自迭代30次，結果如表4所示。

表4 單片點云配準精度和時間對比

由表4可知，在第1行Bunny中，LM算法在配準精度上要高于SVD算法，但時間上要遠大于SVD算法，本文方法在配準精度和時間上都要優于其他兩種算法；在第2行Happy中，SVD算法在配準精度和時間上都好于LM算法，本文方法是三者算法中最好的；在第3行Dragon中，LM算法在配準精度上高于SVD算法，本文方法遠超LM算法和SVD算法，還是三者中最好的。從總體上來看，本文方法對單片非閉合的點云配準精度最高，精度數量級達到10-17～10-16，與閉合的點云數據幾乎一致，其他2種算法在單片非閉合點云的配準上誤差較大，與閉合的點云數據相差較大。

3 結束語

點云配準是點云數據處理中最重要的一步，其關鍵在于求解位置變換矩陣。針對目前求解變換矩陣精度不高、耗時大的問題，提出一種新的求解變換矩陣的方法；結合K-D tree來加快最近點的搜索速度，并采用RANSAC剔除錯誤的點云，以減少迭代的次數，最終完成點云的配準。試驗結果表明，本文方法在配準精度和時間上優于LM算法和SVD算法。本文方法的精度數量級達到10-17～10-16,是其他兩種算法不可達到的，由此證明了本文方法的有效性和時效性。下一步將對點云數據有噪聲和點云密集程度大的情況開展研究工作。