二維點(diǎn)云配準(zhǔn)的交換截取迭代最近點(diǎn)

南開(kāi)大學(xué)統(tǒng)計(jì)與數(shù)據(jù)科學(xué)學(xué)院 裴曉淞

為解決傳統(tǒng)ICP（迭代最近點(diǎn)Iterative Closest Point）[1]算法的存在容易陷入局部最優(yōu)、誤差大、計(jì)算量大等問(wèn)題，本文分析了傳統(tǒng)ICP算法缺點(diǎn)的成因，針對(duì)問(wèn)題，提出了截取機(jī)制和交換機(jī)制，在兩者協(xié)同作用下，經(jīng)過(guò)大量模擬實(shí)驗(yàn)將平均誤差減小了83.6%，計(jì)算時(shí)間減小了42.4%，且由數(shù)據(jù)顯示，能有效地減少一部分局部最優(yōu)的情況。

點(diǎn)云是通過(guò)測(cè)量?jī)x器掃描物體而生成的點(diǎn)數(shù)據(jù)的集合。點(diǎn)云的配準(zhǔn)在生活生產(chǎn)中，可以產(chǎn)生許多有價(jià)值的應(yīng)用如：數(shù)字自動(dòng)化生產(chǎn)、自動(dòng)駕駛、醫(yī)學(xué)圖像配準(zhǔn)等。點(diǎn)云的配準(zhǔn)問(wèn)題分為粗配準(zhǔn)和精確配準(zhǔn)兩部分[2]，最常見(jiàn)的、工業(yè)中使用最廣泛的精確配準(zhǔn)算法是由Rusu等人提出的ICP[1]算法。二維ICP算法在激光領(lǐng)域的圖像處理也產(chǎn)生了很多應(yīng)用[3]，但是配準(zhǔn)的效果還有一定的改進(jìn)空間。

謝小鵬等人對(duì)ICP算法進(jìn)行了改進(jìn)，加入了亂序一對(duì)一匹配和動(dòng)態(tài)閾值的機(jī)制，一定程度上提高了ICP算法的性能[4]，但是也存在動(dòng)態(tài)閾值運(yùn)行較為不穩(wěn)定等問(wèn)題。所以本文針對(duì)上述算法的問(wèn)題，研發(fā)出截取機(jī)制和交換機(jī)制，給出了一個(gè)理論說(shuō)明更加清晰，匹配效果更好的改進(jìn)算法。

1 基本原理

1.1 傳統(tǒng)ICP算法

Input：目標(biāo)點(diǎn)云A= {Ai}、待配準(zhǔn)點(diǎn)云B= {Bi}、終止最小誤差δ、最大迭代次數(shù)N ；

Step1：對(duì)任意點(diǎn)bi=Bi，在點(diǎn)云A找距離最近的點(diǎn)進(jìn)行匹配，記為{ai}，得到匹配后的一對(duì)點(diǎn)云{ai}和{bi}；

Step2：由公式找到變換R和T，使得{ai}和R× {bi} +T的誤差err達(dá)到最??；

Step3：對(duì)B進(jìn)行變換R×B+T得到新的點(diǎn)云；

Step4：若err沒(méi)有達(dá)到最小誤差δ和最大迭代次數(shù)N ，將上述步驟中B換成Step3中得到的新的點(diǎn)云回到Step1，重復(fù)迭代；否則結(jié)束并輸出結(jié)果；

Output：完成配準(zhǔn)的點(diǎn)云，配準(zhǔn)的變換矩陣，最終誤差，迭代次數(shù)。

其中Step2中誤差err的定義如式（1）所示：

變換R和T的求取公式推導(dǎo)如下：

旋轉(zhuǎn)矩陣用旋轉(zhuǎn)角度φ表示如式（2）所示：

平移矩陣T表示如式（3）所示：

其中ΔxΔy分別為點(diǎn)云在x軸和y軸上的位移。

記Ca和Cb是點(diǎn)集{ai}和的中心點(diǎn)，如式（4）所示：

令ai′ =ai-Ca，bi′=bi-Cb， 結(jié) 合err的 定 義，可以得到新的誤差函數(shù)如式（5）所示：

這里已經(jīng)消去了變量T，只需求解旋轉(zhuǎn)矩陣R[3]，對(duì)上式進(jìn)行分解得到如式（6）所示：

對(duì)f（φ）求導(dǎo)，如式（7）所示：

進(jìn)而得到如式（9）所示：

由R的定義即可得到旋轉(zhuǎn)矩陣R。

平移矩陣由公式（10）得到：

1.2 交換截取ICP算法

Alternate and truncated Iterative Closest Point Algorithm簡(jiǎn)稱ATICP。

傳統(tǒng)ICP算法有以下幾個(gè)問(wèn)題：

（1）迭代容易陷入局部最優(yōu)解。在面對(duì)點(diǎn)云較為對(duì)稱的情況下，不同旋轉(zhuǎn)方向的梯度可能會(huì)近似抵消，迭代有可能會(huì)陷入局部最優(yōu)。

（2）每次迭代的運(yùn)算量較大。在傳統(tǒng)ICP算法中，對(duì)于n個(gè)點(diǎn)的點(diǎn)云，每個(gè)點(diǎn)都需要計(jì)算到另一個(gè)點(diǎn)云所有點(diǎn)的距離，最近點(diǎn)的距離需要進(jìn)行6n2次運(yùn)算，邊際計(jì)算成本高，收益低，點(diǎn)數(shù)越多，模型的運(yùn)算越復(fù)雜。

（3）迭代方向錯(cuò)誤。點(diǎn)云可能在中心有一部分點(diǎn)數(shù)據(jù)，由于點(diǎn)云數(shù)據(jù)有一定的誤差，這樣的點(diǎn)更容易導(dǎo)致迭代方向的錯(cuò)誤。

針對(duì)以上問(wèn)題，這里給出兩點(diǎn)改進(jìn)：

（1）交換機(jī)制（Alternate在后文圖標(biāo)簡(jiǎn)寫(xiě)為A）。

在上文介紹的傳統(tǒng)ICP算法的Step1進(jìn)行修改：在奇數(shù)次迭代中，與傳統(tǒng)ICP相同，在偶數(shù)次迭代中交換點(diǎn)云A、B的操作，對(duì)任意點(diǎn)ai=Ai，在點(diǎn)云B找距離最近的點(diǎn)進(jìn)行匹配，記為{bi}，得到匹配后的點(diǎn)云{ai}和{bi}。

交換機(jī)制對(duì)ICP算法的影響：當(dāng)誤差較大時(shí)，如果匹配的點(diǎn)云圖像較為對(duì)稱，傳統(tǒng)ICP算法的旋轉(zhuǎn)梯度可能會(huì)相互抵消，從而陷入局部最優(yōu)；交換機(jī)制從另一個(gè)對(duì)稱匹配的角度，有可能讓點(diǎn)云進(jìn)行正確的變換，從而幫助算法跳出局部最優(yōu)。 而誤差較小時(shí)，無(wú)論是否交換，都傾向于一對(duì)一的正確匹配，交換機(jī)制對(duì)算法沒(méi)有影響。

如圖1所示，傳統(tǒng)ICP算法的旋轉(zhuǎn)梯度抵消，陷入局部最優(yōu)；而交換機(jī)制使得算法跳出局部最優(yōu)。

圖1 對(duì)ATICP左為奇數(shù)次匹配，右為偶數(shù)次匹配；對(duì)傳統(tǒng)ICP只進(jìn)行左圖的匹配Fig.1 For ATICP, the left is an odd number of matches, and the right is an even number of matches; for traditional ICP,only the left image

（2）截取機(jī)制（Truncated在后文圖標(biāo)簡(jiǎn)寫(xiě)為T(mén)）。

在上文介紹的傳統(tǒng)ICP算法Step1前加入一步預(yù)處理，將點(diǎn)云A、B中距離中心點(diǎn)較小的點(diǎn)按比例去除一部分，再進(jìn)入算法。

截取機(jī)制對(duì)ICP算法的影響：當(dāng)點(diǎn)數(shù)較少時(shí)，越多的點(diǎn)意味著越多的信息，可以幫助更快更好的配準(zhǔn)，但是點(diǎn)數(shù)足夠時(shí)，在相同的點(diǎn)分布的情況下，更多的點(diǎn)數(shù)只能徒增計(jì)算量。而且由于點(diǎn)云本身的誤差，部分距離中心較近的點(diǎn)還會(huì)產(chǎn)生錯(cuò)誤的梯度信息，對(duì)配準(zhǔn)不利。

2 實(shí)驗(yàn)

編程環(huán)境如表1所示。

表1 編程環(huán)境Tab.1 Programming environment

隨機(jī)生成點(diǎn)云數(shù)據(jù)集的生成規(guī)則如式（11）、式（12）所示：

對(duì)任意i∈ { 1 ， 2 …50}

這樣生成的隨機(jī)點(diǎn)云較為對(duì)稱，配準(zhǔn)難度大，更容易體現(xiàn)模型的優(yōu)劣。

最大迭代次數(shù)N=10，終止誤差δ=3，在截取步驟中截取40%的點(diǎn)云進(jìn)行迭代。

一次實(shí)驗(yàn)具有偶然性，這里進(jìn)行1000次模擬實(shí)驗(yàn)對(duì)比，如圖3所示展示為誤差大于100的模擬。圖例中T指截取機(jī)制，A指交換機(jī)制，前綴表示在ICP算法上的使用對(duì)應(yīng)的技術(shù)。

如圖2所示，誤差大于100的模擬實(shí)驗(yàn)中，ICP算法出現(xiàn)的次數(shù)最多且誤差較大，加入兩種機(jī)制的ATICP算法出現(xiàn)次數(shù)最少且誤差較小。兩種機(jī)制加入后的單獨(dú)實(shí)驗(yàn)也顯示對(duì)ICP算法有不同程度的優(yōu)化。

圖2 4種算法1000次模擬實(shí)驗(yàn)的誤差大于100的散點(diǎn)圖Fig.2 Scatter plot with errors bigger than 100 for 1000 simulation experiments of four algorithms

err為前文定義的誤差均值，Time表示運(yùn)行的平均時(shí)間，T指截取機(jī)制，A指交換機(jī)制。

如表2所示，分別加入交換機(jī)制和截取都對(duì)算法的誤差和計(jì)算量有一定程度的優(yōu)化，且二者協(xié)同后，起到了疊加作用，使得平均誤差減小了83.6%，計(jì)算時(shí)間減小了42.4%。

表2 4種算法的模擬測(cè)試誤差和時(shí)間的均值Tab.2 The mean value of simulated test error and time for 4 algorithms

3 結(jié)論

本文在傳統(tǒng)ICP算法上進(jìn)行改進(jìn)，針對(duì)容易陷入局部最優(yōu)、誤差大、計(jì)算量大的成因進(jìn)行分析，提出了截取機(jī)制和交換機(jī)制。加入算法改進(jìn)后，平均誤差減小了83.6%，計(jì)算時(shí)間減小了42.4%，提高了算法的性能。

引用

[1]BESL P J,MCKAY H D.A Method for Registration of 3-D Shapes[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2):239-256.

[2]解則曉,徐尚.三維點(diǎn)云數(shù)據(jù)拼接中ICP及其改進(jìn)算法綜述[J].中國(guó)海洋大學(xué)學(xué)報(bào)(自然科學(xué)版),2010,40(1):99-103.

[3]渠瀛.基于激光測(cè)距儀的移動(dòng)機(jī)器人二維地圖創(chuàng)建問(wèn)題研究[D].湖南:中國(guó)人民解放軍國(guó)防科技大學(xué),2011.

[4]謝小鵬,古家威.一種改進(jìn)的二維ICP點(diǎn)云配準(zhǔn)算法[J].激光與紅外,2021,21(7):951-955.