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

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

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

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

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

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

在回顧了經(jīng)典ICP算法的基礎(chǔ)上，采用羅德里格斯矩陣求解點(diǎn)云數(shù)據(jù)之間的運(yùn)動(dòng)參數(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 引 言

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

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

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

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

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

所謂點(diǎn)云數(shù)據(jù)間的運(yùn)動(dòng)參數(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)矩陣和平移向量。對(duì)于相互對(duì)應(yīng)的兩個(gè)點(diǎn)集，可利用羅德里格斯矩陣求得它們之間的變換關(guān)系。羅德里格斯矩陣為反對(duì)稱矩陣，形式如下:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ICP算法每運(yùn)算一次，都會(huì)在最小化對(duì)應(yīng)點(diǎn)均方距離的條件下，使得配準(zhǔn)點(diǎn)云M更接近基準(zhǔn)點(diǎn)云F。這樣，通過(guò)算法的迭代運(yùn)行，可求出點(diǎn)云數(shù)據(jù)間運(yùn)動(dòng)參數(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分別為:

為簡(jiǎn)化參與運(yùn)算的點(diǎn)云數(shù)據(jù)，分別對(duì)基準(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個(gè)原始數(shù)據(jù)點(diǎn)，而提取特征點(diǎn)后只剩下9 214個(gè)數(shù)據(jù)點(diǎn)，參與運(yùn)算的數(shù)據(jù)減少了74.5%；同理配準(zhǔn)點(diǎn)云進(jìn)行特征點(diǎn)提取后，27 714個(gè)原始數(shù)據(jù)點(diǎn)只余3 946個(gè)特征點(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ù)的自動(dòng)配準(zhǔn)，可得到旋轉(zhuǎn)矩陣R和平移向量T，分別為:

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

結(jié)合已知的R0和T0，計(jì)算配準(zhǔn)的相對(duì)誤差△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ù)來(lái)進(jìn)行驗(yàn)證，導(dǎo)致圖4無(wú)法體現(xiàn)出配準(zhǔn)應(yīng)達(dá)到的立體效果。不過(guò)，分析配準(zhǔn)的相對(duì)誤差△R、△T和中誤差mR、mT，可以看出，本文提出的ICP改進(jìn)算法精度很高，完全可以滿足工程要求。

4 結(jié) 論

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

[4]戴靜蘭，陳志楊，葉修梓.ICP算法在點(diǎn)云配準(zhǔn)中的應(yīng)用[J]，中國(guó)圖像圖形學(xué)報(bào)，2007，12(3)，517～521

[5]張鈞，柳健，劉小茂.利用羅德里格矩陣確定三維表面重建中的絕對(duì)定向模型[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)測(cè)理論研究工作。

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