一種基于羅德里格斯矩陣的ICP改進算法及應用

田豐瑞，花向紅，劉金標，周波

(1.武漢大學災害監測與防治研究中心，湖北 武漢 430079； 2.武漢大學測繪學院，湖北 武漢 430079；3.武漢市勘測設計研究院，湖北武漢 430022)

一種基于羅德里格斯矩陣的ICP改進算法及應用

田豐瑞1，2，花向紅1，2，劉金標3，周波2

(1.武漢大學災害監測與防治研究中心，湖北 武漢 430079； 2.武漢大學測繪學院，湖北 武漢 430079；3.武漢市勘測設計研究院，湖北武漢 430022)

在回顧了經典ICP算法的基礎上，采用羅德里格斯矩陣求解點云數據之間的運動參數，實現了一種基于特征點的ICP改進算法，并在實際應用中檢驗了該算法的可行性。

羅德里格斯矩陣；點云數據配準；ICP改進算法；特征點

1 引 言

在逆向過程中，三維激光掃描技術得到了越來越多的應用。進行實際數據采集時，由于激光傳播的直線性以及被感應物體存在自遮擋問題，需要在不同站點上對目標進行多次掃描，才能獲得足夠的信息來完成三維重建工作。這些掃描得到的點云數據都是基于掃描儀所在站點的獨立坐標系的，進行三維重建就必須將這些不同站點得到的掃描數據整合到同一個坐標系中，即點云數據配準。

1992年Besl和 McKay[1]提出了最鄰近點迭代(Iterative Closest Point，ICP)算法，也稱對應點迭代(Iterative Corresponding Point)算法，算法通過“確定對應點——計算運動參數”的迭代運算，求得限定條件下的最小二乘解。但ICP算法不能處理初始位置相差太大的點云數據，并且要求兩個匹配點集中的一個點集是另外一個點集的子集。由于ICP算法的時間效率有限，不適合用于海量數據的處理，因此，國內外很多學者都為改進ICP算法做了大量工作。其中，Chen和Medioni[2]選用點集中的部分點進行運算，并采用點到對應點切平面的距離代替對應點之間的距離來進行迭代，改進了ICP算法，達到了較快的收斂速度。羅先波[3]和戴靜蘭[4]等都在ICP算法中引入了特征點的選取，減少了參與運算的數據量，大大提高了ICP算法的時間效率。本文在采用羅德里格斯旋轉矩陣完成待配準點云之間運動參數求解的基礎上，實現了一種基于特征點距離最近的ICP改進算法，提高了ICP算法的時間效率，同時也更利于程序實現。

2 基于羅德里格斯矩陣的ICP改進算法

ICP算法實質上可分為運動參數的求解和最鄰近點的搜索兩步。為便于對ICP改進算法進行說明，這里將待配準的兩個點云數據分別稱為基準點云(Fixed Point Cloud)和配準點云(Moving Point Cloud)。

2.1 運動參數的求解

所謂點云數據間的運動參數，即將待配準點云數據統一到基準點云數據所在坐標系中所需要的變換參數。由于三維激光掃描數據為剛體數據，不存在尺度變化，所以這里的變換參數只包括旋轉矩陣和平移向量。對于相互對應的兩個點集，可利用羅德里格斯矩陣求得它們之間的變換關系。羅德里格斯矩陣為反對稱矩陣，形式如下:

可見，通過羅德里格斯矩陣求解點云數據配準的運動參數，其關鍵在于a，b和c三個參數的計算，具體算法流程為:

(1)計算基準點云F和配準點云M的重心坐標:

(2)計算基準點云數據和配準點云數據的重心化坐標，從而將旋轉矩陣和平移向量的求解分開進行，簡化運算:

(3)計算a，b和c，構造羅德里格斯矩陣:

根據最小二乘原理，可求得:

其中P為單位陣。a，b，c三參數一旦求出，便可按照式(1)構造羅德里格斯矩陣S。

(4)構造旋轉矩陣R(I為單位陣):

(6)計算結束。

2.2 改進的ICP算法

在采用羅德里格斯矩陣求解運動參數的基礎上，本文實現了一種基于特征點的ICP改進算法，提高了算法的時間效率。特征點主要是指點云數據中具有關鍵影響的點，一般來說分布在曲面之間的相交線或曲面的邊界上，以曲線的拐點和交點為主。本文采用符號突變為判別條件進行特征點的選取。首先將平面上的點云按某一個方向排列(X或Y)，然后順序連接各個點并計算相應的斜率。作為曲線的拐點，斜率在特征點處符號會發生變化，即符號發生突變，此時就可判定該點為所需的特征點。

在提取出配準點云的特征點集之后，就需要在基準點云的特征點集中搜索與之對應的最鄰近點，構成對應點對，進一步進行點云配準。具體實現如下:

(1)準備好待配準的點云數據，并對它們進行特征點提取，得到簡化的基準點云F(含NF個點)和配準點云M(含NM個點)；

(2)參數初始化:

(3)根據得到的運動參數，對特征點集進行坐標變換:

式中旋轉矩陣R和平移向量T可根據參數由式(5)、式(6)求出。

(4)搜索對應點對，使得:

(5)計算旋轉矩陣Ri和平移向量Ti，方法如2.1所述；

ICP算法每運算一次，都會在最小化對應點均方距離的條件下，使得配準點云M更接近基準點云F。這樣，通過算法的迭代運行，可求出點云數據間運動參數的最小二乘解。

3 算法實現與分析

3.1 數據準備

本文采用云南金剛塔的三維激光掃描數據作為基準點云，然后將基準點云分別繞X軸、Y軸、Z軸旋轉15°，并平移20 mm得到配準點云。這樣，兩塊點云數據之間的旋轉矩陣R0和平移向量T0為已知值，便于分析算法的精度，并在此基礎上進行算法的檢驗。如圖1所示，左邊為基準點云，右邊為配準點云。

圖1 原始點云數據

點云數據之間的旋轉矩陣R0和平移向量T0分別為:

為簡化參與運算的點云數據，分別對基準點云數據和配準點云數據提取特征點，效果如圖2、圖3所示。

圖2 基準點云特征點

圖3 配準點云特征點

與圖1所示相比，明顯可以看出，進行特征點提取后的點云數據，其數據量大大減少。其中，基準點云有36 184個原始數據點，而提取特征點后只剩下9 214個數據點，參與運算的數據減少了74.5%；同理配準點云進行特征點提取后，27 714個原始數據點只余3 946個特征點，數據減少85.8%。

3.2 配準實現與結果分析

提取了點云數據的特征點后，采用2.1、2.2所述的ICP改進算法進行點云數據的自動配準，可得到旋轉矩陣R和平移向量T，分別為:

配準效果如圖4所示。

結合已知的R0和T0，計算配準的相對誤差△R、△T和中誤差mR、mT，分別為:

圖4 使用特征點最近距離配準效果

與圖2、圖3相比，圖4中顯示的金剛塔更為細膩，包含的信息也更加豐富，體現了點云數據配準的重要性。但由于為了便于分析算法的精度，算例采用基于同一站點的數據來進行驗證，導致圖4無法體現出配準應達到的立體效果。不過，分析配準的相對誤差△R、△T和中誤差mR、mT，可以看出，本文提出的ICP改進算法精度很高，完全可以滿足工程要求。

4 結 論

隨著逆向工程和數字城市的發展，三維激光掃描技術越來越受到社會的重視。作為三維重建的關鍵技術，點云數據配準算法成為測繪工作者研究的熱點問題。ICP算法是目前點云數據配準的主流算法，但其時間效率較低，且對初始位置要求高，否則，算法的收斂方向不確定，會嚴重影響配準效果。本文在回顧ICP算法的基礎上實現了一種以點云數據特征點為配準對象的ICP改進算法，提高了算法的運行效率。同時為了方便程序實現，采用羅德里格斯矩陣計算待配準點云數據間的運動參數。經試驗證明，無論運動參數相對誤差、中誤差，還是配準效果，都說明了本文所提出的ICP改進算法可行、有效，達到了預想效果。

[1]Besl.PJ，MeKay.N D.A method for registration of 3－D shapes[J].IEEE Transactions on Pattern Analysis and Machine Intelligence，1992，14(2):239～256

[2]Chen Y，Medioni G.Object modeling by registration of multiple range images.Image Vision Comput，1992，10:145～155[3]羅先波，三維掃描系統中的數據配準技術[J]，清華大學學報，2004，4(8)，1104～1106

[4]戴靜蘭，陳志楊，葉修梓.ICP算法在點云配準中的應用[J]，中國圖像圖形學報，2007，12(3)，517～521

[5]張鈞，柳健，劉小茂.利用羅德里格矩陣確定三維表面重建中的絕對定向模型[J]，紅外與激光工程，1998，27 (4)，30～32

An Improved ICP Algorithm Based on Rodrigues Matrix

Tian FengRui1，2，Hua XiangHong1，2，Liu JinBiao3，Zhou Bo2
(1.Hazard Monitoring and Prevention Research Center，Wuhan University，Wuhan 430079，China；2.School of Geodesy and Geomatics，Wuhan University，Wuhan 430079，China；3.Wuhan Geotechnical Engineering and Surveying Institute，Wuhan 430022，China)

ICP(Iterative Closet Point)and improved ICP algorithms are widely used in point clouds registration.This paper describes the original ICP algorithm，and then presents an improved ICP algorithm based on Rodrigues Matrix.Experiment shows the feasibility of this algorithm.

registration；ICP algorithm；rodrigues matrix；feature points

1672－8262(2010)05－90－03

P235

A

2010—04—28

田豐瑞(1986—)，男，碩士研究生，現從事變形監測理論研究工作。

國家自然科學基金資助項目(40901214)；精密工程與工業測量國家測繪局重點實驗室開放基金項目資助(PF2009－2)。