田豐瑞，花向紅，劉金標(biāo)，周波

(1.武漢大學(xué)災(zāi)害監(jiān)測與防治研究中心，湖北 武漢 430079； 2.武漢大學(xué)測繪學(xué)院，湖北 武漢 430079；3.武漢市勘測設(shè)計研究院，湖北武漢 430022)

一種基于羅德里格斯矩陣的ICP改進(jìn)算法及應(yīng)用

田豐瑞1，2，花向紅1，2，劉金標(biāo)3，周波2

(1.武漢大學(xué)災(zāi)害監(jiān)測與防治研究中心，湖北 武漢 430079； 2.武漢大學(xué)測繪學(xué)院，湖北 武漢 430079；3.武漢市勘測設(shè)計研究院，湖北武漢 430022)

在回顧了經(jīng)典ICP算法的基礎(chǔ)上，采用羅德里格斯矩陣求解點(diǎn)云數(shù)據(jù)之間的運(yùn)動參數(shù)，實(shí)現(xiàn)了一種基于特征點(diǎn)的ICP改進(jìn)算法，并在實(shí)際應(yīng)用中檢驗(yàn)了該算法的可行性。

羅德里格斯矩陣；點(diǎn)云數(shù)據(jù)配準(zhǔn)；ICP改進(jìn)算法；特征點(diǎn)

1 引 言

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

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

2 基于羅德里格斯矩陣的ICP改進(jìn)算法

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

2.1 運(yùn)動參數(shù)的求解

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

可見，通過羅德里格斯矩陣求解點(diǎn)云數(shù)據(jù)配準(zhǔn)的運(yùn)動參數(shù)，其關(guān)鍵在于a，b和c三個參數(shù)的計算，具體算法流程為:

(1)計算基準(zhǔn)點(diǎn)云F和配準(zhǔn)點(diǎn)云M的重心坐標(biāo):

(2)計算基準(zhǔn)點(diǎn)云數(shù)據(jù)和配準(zhǔn)點(diǎn)云數(shù)據(jù)的重心化坐標(biāo)，從而將旋轉(zhuǎn)矩陣和平移向量的求解分開進(jìn)行，簡化運(yùn)算:

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

根據(jù)最小二乘原理，可求得:

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

(4)構(gòu)造旋轉(zhuǎn)矩陣R(I為單位陣):

(6)計算結(jié)束。

2.2 改進(jìn)的ICP算法

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

在提取出配準(zhǔn)點(diǎn)云的特征點(diǎn)集之后，就需要在基準(zhǔn)點(diǎn)云的特征點(diǎn)集中搜索與之對應(yīng)的最鄰近點(diǎn)，構(gòu)成對應(yīng)點(diǎn)對，進(jìn)一步進(jìn)行點(diǎn)云配準(zhǔn)。具體實(shí)現(xiàn)如下:

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

(2)參數(shù)初始化:

(3)根據(jù)得到的運(yùn)動參數(shù)，對特征點(diǎn)集進(jìn)行坐標(biāo)變換:

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

(4)搜索對應(yīng)點(diǎn)對，使得:

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

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

3 算法實(shí)現(xiàn)與分析

3.1 數(shù)據(jù)準(zhǔn)備

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

圖1 原始點(diǎn)云數(shù)據(jù)

點(diǎn)云數(shù)據(jù)之間的旋轉(zhuǎn)矩陣R0和平移向量T0分別為:

為簡化參與運(yùn)算的點(diǎn)云數(shù)據(jù)，分別對基準(zhǔn)點(diǎn)云數(shù)據(jù)和配準(zhǔn)點(diǎn)云數(shù)據(jù)提取特征點(diǎn)，效果如圖2、圖3所示。

圖2 基準(zhǔn)點(diǎn)云特征點(diǎn)

圖3 配準(zhǔn)點(diǎn)云特征點(diǎn)

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

3.2 配準(zhǔn)實(shí)現(xiàn)與結(jié)果分析

提取了點(diǎn)云數(shù)據(jù)的特征點(diǎn)后，采用2.1、2.2所述的ICP改進(jìn)算法進(jìn)行點(diǎn)云數(shù)據(jù)的自動配準(zhǔn)，可得到旋轉(zhuǎn)矩陣R和平移向量T，分別為:

配準(zhǔn)效果如圖4所示。

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

圖4 使用特征點(diǎn)最近距離配準(zhǔn)效果

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

4 結(jié) 論

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

[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]羅先波，三維掃描系統(tǒng)中的數(shù)據(jù)配準(zhǔn)技術(shù)[J]，清華大學(xué)學(xué)報，2004，4(8)，1104～1106

[4]戴靜蘭，陳志楊，葉修梓.ICP算法在點(diǎn)云配準(zhǔn)中的應(yīng)用[J]，中國圖像圖形學(xué)報，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—)，男，碩士研究生，現(xiàn)從事變形監(jiān)測理論研究工作。

國家自然科學(xué)基金資助項(xiàng)目(40901214)；精密工程與工業(yè)測量國家測繪局重點(diǎn)實(shí)驗(yàn)室開放基金項(xiàng)目資助(PF2009－2)。