點云的剛體運動參數(shù)估計方法的比較

袁志聰，魯鐵定，鄧小淵

(1.東華理工大學(xué) 測繪工程學(xué)院，江西 南昌 330013；2.流域生態(tài)與地理環(huán)境監(jiān)測國家測繪地理信息局重點實驗室，江西 南昌 330013)

近年來，隨著三維激光掃描技術(shù)的不斷發(fā)展，點云獲取的速度越來越快，如何高效地處理海量點云數(shù)據(jù)已成為科研領(lǐng)域的研究熱點。點云配準(zhǔn)是海量點云數(shù)據(jù)處理中的核心問題；點云配準(zhǔn)就是要使所有來自兩個點云集合中的同名點對滿足同一剛體變換[1-2]，如何快速準(zhǔn)確地求解剛體變換參數(shù)成了點云配準(zhǔn)中的核心問題。

傳統(tǒng)的坐標(biāo)轉(zhuǎn)換采用布爾沙七參數(shù)模型[3],由于布爾沙模型的旋轉(zhuǎn)矩陣是由坐標(biāo)軸之間的夾角表示的，其表示方式繁瑣，計算復(fù)雜，同時存在象限判斷難等問題，文獻(xiàn)[4]提出基于羅德里格矩陣的求解方法，羅德里格矩陣由3個獨立參數(shù)組成旋轉(zhuǎn)矩陣，結(jié)合最小二乘法具有精度高，速度快等優(yōu)點。常用的剛體運動估計方法還有奇異值分解法；正交分解法[5]；四元數(shù)法[6-7]等。宋林霞[8]通過奇異值分解法求解旋轉(zhuǎn)矩陣，并提出一條相關(guān)引理，將距離優(yōu)化問題轉(zhuǎn)為估計的最優(yōu)的旋轉(zhuǎn)矩陣R使得對于任意給定的方陣A都有tr(ATR)最大，對算法進(jìn)行優(yōu)化。邱志強[9]等提到基于三維特征點以及二維特征點的“8點算法”的剛體運動估計方法。官云蘭[10-11]應(yīng)用單位四元數(shù)進(jìn)行剛體運動估計，四元數(shù)法具有計算速度快、穩(wěn)定性好等優(yōu)點，代俁西[12]在基于對偶四元數(shù)的三維運動估計中介紹對偶四元數(shù)的估計方法。對偶四元數(shù)是對偶數(shù)和四元數(shù)的結(jié)合，具備其他方法所沒有的優(yōu)點，對偶四元數(shù)能同時求解旋轉(zhuǎn)矩陣和平移參數(shù)，計算過程中不會引入旋轉(zhuǎn)矩陣的誤差。

本文在現(xiàn)有基礎(chǔ)上，通過模擬數(shù)據(jù)和實例對奇異值分解法、正交分解法、單位四元數(shù)法和對偶四元數(shù)法4種估計方法作比較，分析4種方法的優(yōu)缺點，得出對偶四元數(shù)總體性能最優(yōu)。

1 剛體運動及參數(shù)求解方法

1.1 剛體運動

(1)

式中:R為3×3正交單位矩陣，稱為旋轉(zhuǎn)矩陣;T為3維列向量，稱為平移向量。

剛體運動估計就是在目標(biāo)函數(shù)F(R,T)=min的情況下求解最優(yōu)的旋轉(zhuǎn)矩陣R,平移向量T，使得兩點集盡可能的重合。目標(biāo)函數(shù)具體形式為

(2)

1.2 剛體運動參數(shù)求解方法

1.2.1 單位四元數(shù)法

四元數(shù)是數(shù)學(xué)家Hamilton創(chuàng)造的；可以認(rèn)為是一個包含四個元素的列向量，可表示為

q=w+xi+yj+zk.

(3)

式中:w為標(biāo)量，(x,y,z)為四元數(shù)對應(yīng)的向量。四元數(shù)亦可寫為q=(w,v)。

定義4階矩陣

(4)

設(shè)矩陣S最大特征值對應(yīng)的單位特征向量為q=(q0,q1,q2,q3)，則對應(yīng)的旋轉(zhuǎn)矩陣為

(5)

平移向量為

(6)

1.2.2 奇異值分解法(SVD)

首先求出運動前后兩點集的重心化坐標(biāo)

Pi=Pi-Pio,

(7)

(8)

R=VUT.

(9)

平移向量為

(10)

1.2.3 正交分解法(OD)

設(shè)矩陣HHT的特征值分別為λ1，λ2，λ3，對應(yīng)的特征向量分別為u1,u2,u3。旋轉(zhuǎn)矩陣可表示為

(11)

平移向量為

(12)

1.2.4 對偶四元數(shù)法(DQD)

對偶四元數(shù)是由Clifford提出來的,是對偶數(shù)和四元數(shù)的組合，可以理解為元素為四元數(shù)的對偶數(shù),同樣可理解為元素為對偶數(shù)的四元數(shù),對偶四元數(shù)的優(yōu)越性體現(xiàn)在它繼承了二者的共性，從而能夠統(tǒng)一表示旋轉(zhuǎn)和平移。對偶四元數(shù)可表示為

ε(q00,q01,q02,q03)T.

(13)

式中q，q0均為四元數(shù)，分別稱做對偶四元數(shù)的實部和對偶部，ε為對偶算符。

對偶四元數(shù)的空間變換如圖1所示。n是單位向量，是空間旋轉(zhuǎn)和平移的變換方向。傳統(tǒng)的坐標(biāo)轉(zhuǎn)換是先通過坐標(biāo)軸進(jìn)行旋轉(zhuǎn)變換，再通過平移向量t完成平移變換，形成新的坐標(biāo)系，對偶四元數(shù)空間變換首先將原始坐標(biāo)系沿著n方向平移距離d，然后圍繞過點p的直線n旋轉(zhuǎn)角度θ，即可得到轉(zhuǎn)換后的坐標(biāo)系。

圖1 對偶四元數(shù)空間變化

設(shè)點p=(x,y,z)T剛體運動后變?yōu)閜′=(x′,y′,z′)T，可用對偶四元數(shù)表示剛體運動，即

(14)

(15)

(16)

2 實例驗證

2.1 模擬數(shù)據(jù)與結(jié)果分析

利用Matlab隨機生成1 000個數(shù)據(jù)得到點云A，如圖2所示，給定初始旋轉(zhuǎn)矩陣和平移向量；對點云A進(jìn)行剛體運動變換，得到點云B，現(xiàn)通過4種方法分別求解剛體運動參數(shù)，把旋轉(zhuǎn)矩陣轉(zhuǎn)換為旋轉(zhuǎn)角，以求得的旋轉(zhuǎn)矩陣對應(yīng)的旋轉(zhuǎn)角與初始旋轉(zhuǎn)矩陣對應(yīng)的旋轉(zhuǎn)角之間的差作為旋轉(zhuǎn)角誤差；對應(yīng)的平移向量之間的差作為平移向量誤差。

已知兩點云的變換參數(shù)如表1所示。

表1 兩點云變換參數(shù)

圖2 兩點云相對位置

目標(biāo)點云不加噪聲情況下，4種方法求得的轉(zhuǎn)換參數(shù)如表2所示。

表2 不加噪聲的情況

續(xù)表2

下面將旋轉(zhuǎn)矩陣轉(zhuǎn)換為角度，計算不同噪聲水平下的參數(shù)誤差，算式為

(17)

(18)

加標(biāo)準(zhǔn)差為0.2的噪聲的參數(shù)誤差如表3所示。

表3 加標(biāo)準(zhǔn)差為0.2的噪聲計算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為0.4的噪聲的參數(shù)誤差如表4所示。

表4 加標(biāo)準(zhǔn)差為0.4的噪聲計算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為0.6的噪聲的參數(shù)誤差如表5所示。

表5 加標(biāo)準(zhǔn)差為0.6的噪聲計算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為0.8的噪聲的參數(shù)誤差如表6所示。

表6 加標(biāo)準(zhǔn)差為0.8的噪聲計算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為1.0的噪聲的參數(shù)誤差如表7所示。

表7 加標(biāo)準(zhǔn)差為1.0的噪聲計算2 000次的參數(shù)誤差

4種方法計算2 000次所耗時間如表8所示。

表8 計算2 000次耗時

噪聲標(biāo)準(zhǔn)差與角度誤差之間的關(guān)系如圖3所示。

圖3 噪聲標(biāo)準(zhǔn)差與角度誤差之間的關(guān)系

噪聲標(biāo)準(zhǔn)差與平移參數(shù)誤差之間的關(guān)系如圖4所示。

圖4 噪聲標(biāo)準(zhǔn)差與平移參數(shù)誤差之間的關(guān)系

由表2數(shù)據(jù)可知，在目標(biāo)點云不加噪聲的情況下，4種剛體運動參數(shù)求解方法具有相同的精度，且與初始旋轉(zhuǎn)矩陣；平移向量結(jié)果一致。由表3—表7數(shù)據(jù)可知，當(dāng)目標(biāo)點云加入隨機噪聲時，對于旋轉(zhuǎn)矩陣的估計，奇異值分解法、正交分解法、對偶四元數(shù)法三者具有相同的精度，均優(yōu)于四元數(shù)的求解精度，以表7數(shù)據(jù)為例，前3種方法的求解誤差為0.011 5，而四元數(shù)的求解誤差為0.013 0，圖3所示的折線圖可以明顯看出4種方法噪聲水平對角度估計誤差的影響。

對于平移向量的求解，通過表3—表7數(shù)據(jù)可得出對偶四元數(shù)法精度最高，穩(wěn)定性最好；體現(xiàn)了對偶數(shù)和四元數(shù)的優(yōu)良性質(zhì)。以表7數(shù)據(jù)為例，當(dāng)噪聲標(biāo)準(zhǔn)差達(dá)到1.0時，對偶四元數(shù)的參數(shù)誤差為0.057 5，四元數(shù)參數(shù)誤差0.141 0，奇異值分解法和正交分解法具有相同的精度，參數(shù)誤差均為0.132 3。由圖4可得出平移參數(shù)求解誤差與噪聲標(biāo)準(zhǔn)差之間的關(guān)系。對偶四元數(shù)精度最高的原因在于求解變換參數(shù)時，旋轉(zhuǎn)矩陣和平移向量同時求解，平移向量不會引入旋轉(zhuǎn)矩陣所帶來的誤差。

通過實驗數(shù)據(jù)分析，可知：

1)在目標(biāo)點云不含任何誤差時，4種剛體運動參數(shù)求解方法具有相同的求解精度，均滿足實際工程的需求。

2)在誤差水平相同的情況下，奇異值分解法，正交分解法、對偶四元數(shù)法對于旋轉(zhuǎn)矩陣的估計具有相同的精度，優(yōu)于四元數(shù)的求解。對于平移參數(shù)的估計，對偶四元數(shù)求解精度最高，奇異值分解法與正交分解法具有相同的精度，優(yōu)于四元數(shù)求解精度。

3)在計算速度方面，由快到慢依次為奇異值分解法、四元數(shù)法、正交分解法、對偶四元數(shù)法。

4)在綜合性能方面，對偶四元數(shù)綜合性能最優(yōu)。

2.2 實測數(shù)據(jù)分析

以實測的點云數(shù)據(jù)為例，如圖5所示，圖中兩平面點云是不同視角下獲得的點云數(shù)據(jù)，已知兩者之間的變換參數(shù)，現(xiàn)分別通過4種方法求解剛體變換參數(shù)。

圖5 兩平面點云相對位置

兩平面點云的已知變換參數(shù)如表9所示。

表9 兩點云變換參數(shù)

4種方法求得的轉(zhuǎn)換參數(shù)如表10所示。

表10 4種方法求得的轉(zhuǎn)換參數(shù)

通過實測數(shù)據(jù)求解，由表10數(shù)據(jù)可知，對偶四元素總體性能優(yōu)于其他三種算法，由圖6可知，經(jīng)過對偶四元數(shù)求解的剛體變換參數(shù)；兩點云很好的重合在了一起。對偶四元數(shù)法滿足實際工程的需求。

對偶四元數(shù)配準(zhǔn)后兩點云的相對位置如圖6所示。

圖6 對偶四元數(shù)配準(zhǔn)后

3 結(jié) 論

本文通過模擬仿真實驗與實際算例對4種基于點云的剛體運動參數(shù)求解方法做了比較，得出以下結(jié)論：1)在目標(biāo)點云不含任何誤差時，4種剛體運動參數(shù)求解方法具有相同的求解精度，均滿足實際工程的需求。2)在誤差水平相同的情況下，奇異值分解法、正交分解法、對偶四元數(shù)法對于旋轉(zhuǎn)矩陣的估計具有相同的精度，優(yōu)于四元數(shù)的求解。對于平移參數(shù)的估計，對偶四元數(shù)求解精度最高，奇異值分解法與正交分解法具有相同的精度，優(yōu)于四元數(shù)求解精度。3)在計算速度方面，由快到慢依次為奇異值分解法、四元數(shù)法、正交分解法、對偶四元數(shù)法。4)在綜合性能方面，對偶四元數(shù)綜合性能最優(yōu)。相比其他3種方法，對偶四元數(shù)能夠同時求解旋轉(zhuǎn)參數(shù)和平移參數(shù)，平移參數(shù)不存在旋轉(zhuǎn)矩陣誤差累積的問題。在實際問題中，應(yīng)優(yōu)先使用對偶四元數(shù)求解。

本文探討當(dāng)樣本數(shù)據(jù)存在偶然誤差時，點云剛體運動參數(shù)估計的方法，發(fā)現(xiàn)基于對偶四元數(shù)的估計方法具有一定的優(yōu)勢；但是當(dāng)樣本數(shù)據(jù)中存在粗差時，本文并沒有做研究，研究具有良好抗差性的點云剛體運動參數(shù)估計方法是下一步的重點。

[1] 吳嘉嘉.貝葉斯剛性點云配準(zhǔn)的研究[D].蘭州：蘭州大學(xué),2013.

[2] 林文惠.剛體運動學(xué)的幾何討論[J].力學(xué)與實踐,2005,27(5):71-72.

[3] 陳宇,白征東,羅騰.基于改進(jìn)的布爾沙模型的坐標(biāo)轉(zhuǎn)換方法[J].大地測量與地球動力學(xué),2010,30(3):71-73.

[4] 田茂,花向紅,丁鴿,等.基于羅德里格矩陣的混合最小二乘方法在三維激光中的應(yīng)用[J].測繪地理信息,2014,39(2): 18-21.

[5] HORN B K P,HILDEN H M,NEGAHDARIPOUR S.Closed-form solution of absolute orientation using orthonormal matrices[J].JOSA A,1988,5(7): 1127-1135.

[6] WALKER M W,SHAO L,VOLZ R A.Estimating 3-D location parameters using dual numberquaternions[J].CVGIP: image understanding,1991,54(3): 358-367.

[7] 李志偉,李克昭,趙磊杰,等.基于單位四元數(shù)的任意旋轉(zhuǎn)角度的三維坐標(biāo)轉(zhuǎn)換[J].大地測量與地球動力學(xué),2017,37(1): 81-85.

[8] 宋林霞.三維點云配準(zhǔn)方法的研究[D].濟南：濟南大學(xué)，2013.

[9] 邱志強,陸宏偉,于起峰.基于圖像的三維剛體運動估計算法比較[J].光學(xué)技術(shù),2004,30(1):109-112.

[10] 官云蘭,程效軍,周世健,等.基于單位四元數(shù)的空間后方交會解算[J].測繪學(xué)報,2008,37(1): 30-35.

[11] 官云蘭.地面三維激光掃描數(shù)據(jù)處理中的若干問題研究 [D].上海: 同濟大學(xué),2008.

[12] 代俁西.基于對偶四元數(shù)的三維運動估計研究 [D].南京：南京航空航天大學(xué),2016.