張鴻燕，王婧妍，周璐莎，王炳輝

（中國民航大學(xué)中歐航空工程師學(xué)院，天津 300300）

視覺SLAM 中三維點云配準的Cayley 方法

張鴻燕，王婧妍，周璐莎，王炳輝

（中國民航大學(xué)中歐航空工程師學(xué)院，天津 300300）

利用三維點對應(yīng)關(guān)系估計攝像機的位姿矩陣是同步定位與地圖構(gòu)建（SLAM）系統(tǒng)的一個基本問題。由于系統(tǒng)的誤差累積會導(dǎo)致點云尺度的漂移，三維點對應(yīng)關(guān)系在誤差干擾下從歐式變換退化為相似變換，在計算過程中需考慮尺度因子。利用Cayley變換來估計三維對應(yīng)點之間相似變換的閉式算法，可方便地確定定位與構(gòu)圖過程中攝像機的位姿變化。仿真實驗和誤差分析驗證了基于Cayley變換的配準算法的準確性與魯棒性。該算法不僅可用于SLAM系統(tǒng)，還適用于計算機視覺中的三維重建與三維拼接問題。

同步定位與地圖構(gòu)建；點云配準；相似變換；Cayley變換

由三維對應(yīng)點估計剛體的三維運動[1-3]，即剛體運動估計問題，被廣泛應(yīng)用在姿態(tài)估計、圖像配準、目標運動識別、機器人學(xué)等領(lǐng)域。該問題的本質(zhì)就是估計運動過程中的姿態(tài)變換矩陣[4]

其中：R為旋轉(zhuǎn)矩陣；SO（3）為三維旋轉(zhuǎn)群；t為平移向量；s為尺度因子。一般地，s為正數(shù)，T矩陣是攝像機姿態(tài)變換（相似變換）矩陣；特殊地，s=1時，T矩陣是剛體運動變換（歐式變換）矩陣。在視覺同步定位與地圖構(gòu)建（SLAM，simultaneous localisation and mapping）系統(tǒng)中，攝像機運動姿態(tài)（R與t）實時估計是關(guān)鍵環(huán)節(jié)[5-6]。當(dāng)視覺系統(tǒng)獲得不同局部坐標系下的三維點云，即可估計攝像機的姿態(tài)變換矩陣，進而將點云配準到全局坐標系下。若系統(tǒng)所得點云無先驗對應(yīng)關(guān)系，通常選用迭代最近點（ICP）[1，7]算法進行配準，該算法計算復(fù)雜度高，用于實時處理時需硬件加速。對于無需硬件加速的SLAM系統(tǒng)，如ORB-SLAM[8]，一般根據(jù)稀疏點云之間的對應(yīng)關(guān)系估計三維點云配準矩陣。

針對三維對應(yīng)點的配準矩陣計算，經(jīng)典的估計算法包括Arun[9]提出的奇異值分解法、Horn[10]提出的正交分解法和單位四元數(shù)法[11]以及Walker[12]提出的對偶四元數(shù)法。Eggert等[13]分析了這4種算法的準確性、穩(wěn)定性及效率，同時指出，無論從魯棒性、效率還是收斂性考慮，閉式解（解析解）都優(yōu)于迭代解。運動狀態(tài)估計問題必然會涉及到旋轉(zhuǎn)矩陣的計算。這些經(jīng)典算法均采用Euler角、四元數(shù)或Rodrigues公式來表達三階旋轉(zhuǎn)矩陣。當(dāng)數(shù)據(jù)誤差較大時，得到的姿態(tài)可能是一個正交矩陣而非旋轉(zhuǎn)矩陣。吳福朝[14]提出將Cayley變換應(yīng)用于歐式變換估計，揭示了Cayley變換在攝像機姿態(tài)估計中的優(yōu)越性。Cayley變換及旋轉(zhuǎn)矩陣的Cayley表達是數(shù)學(xué)家Cayley[15]于1846年提出的，但在計算機視覺領(lǐng)域一直未引起足夠的注意。Cayley方法的優(yōu)點在于直接提供了一個旋轉(zhuǎn)矩陣，無須校正與近似，而且只需3個對應(yīng)點就能提供線性解[16]。

對于多數(shù)單目SLAM系統(tǒng)，由于缺乏絕對尺度信息，系統(tǒng)運行的累積誤差會導(dǎo)致點云尺度發(fā)生漂移[8]，從而使得對應(yīng)點無法正常匹配。此時，三維點對應(yīng)關(guān)系在誤差干擾下已不是歐式變換而是相似變換。針對尺度漂移問題，ORB-SLAM系統(tǒng)采用Horn[11]提出的基于單位四元數(shù)的閉式算法，用四元數(shù)表達旋轉(zhuǎn)矩陣，轉(zhuǎn)化過程復(fù)雜，且在數(shù)據(jù)誤差較大的情況下給出的可能不是旋轉(zhuǎn)矩陣。針對三維對應(yīng)點的相似變換問題，提出基于Cayley變換的閉式配準算法，避免了復(fù)雜的迭代過程，并直接給出旋轉(zhuǎn)矩陣，使得不同局部坐標系下的點云配準在統(tǒng)一的坐標系與尺度之下。

1 點云配準與旋轉(zhuǎn)矩陣的Cayley表達

已知SLAM系統(tǒng)在相鄰k-1和k時刻構(gòu)建的一組三維對應(yīng)點為，點云配準算法根據(jù)點對應(yīng)關(guān)系來計算兩幀圖像Ik-1和Ik所確定的攝像機姿態(tài)變換矩陣[4，17]

然而實際系統(tǒng)運行時存在累積誤差，這會導(dǎo)致點云的相對尺度發(fā)生漂移，此時尺度因子，點對應(yīng)關(guān)系轉(zhuǎn)化為[18]

圖1 三維點云坐標配準Fig.1 3D points registration

求解攝像機的位姿變換矩陣時，采用Cayley表達來表示旋轉(zhuǎn)矩陣。Cayley變換是反對稱矩陣與旋轉(zhuǎn)矩陣之間的一對一映射。給定反對稱矩陣W，則存在一個唯一的三維向量 w=[w1，w2，w3]T滿足

旋轉(zhuǎn)矩陣可由三維向量w表示成[15-16]

其中：向量w稱為旋轉(zhuǎn)矩陣R的Cayley表達。

由于本文只考慮相鄰兩幀圖像確定的攝像機姿態(tài)變換矩陣，也就是計算一組三維對應(yīng)點的坐標配準矩陣，為簡化符號描述，省去上下標k-1和k，并記

2 點云配準的Cayley方法

其中：s為尺度因子；t為平移向量；R為旋轉(zhuǎn)矩陣。由于實際系統(tǒng)存在噪聲，式（8）并不成立，而變成一個超定方程組。假設(shè)誤差為

位姿變換參數(shù)（R，t，s）的估計問題轉(zhuǎn)化為誤差函數(shù)最小化問題

為得到較好的三維點云配準估計算法，先對原始三維點云數(shù)據(jù)進行0均值化，以簡化誤差函數(shù)的描述。

2.1 數(shù)據(jù)的0均值化

然后將點集中的各個元素減去質(zhì)心坐標，得到各個點與質(zhì)心之間的偏移距離，形成新的點云集合和，其中

新生成的對應(yīng)點之間的幾何約束關(guān)系為

2.2 誤差函數(shù)的等價描述

對于新的對應(yīng)點集，其誤差表示為

因此，可得平移向量的估計公式

誤差函數(shù)可簡化如下

2.3 姿態(tài)估計的Cayley方法

由于旋轉(zhuǎn)矩陣給出等距變換，可得‖RXi‖=‖Xi‖。因此當(dāng)范數(shù)‖Xi‖和‖Yi‖已知時，可按下式來估計尺度因子

然后將式（7）代入式（13），可得

最后根據(jù)式（7）估計旋轉(zhuǎn)矩陣R。于是可得姿態(tài)變換參數(shù)的最優(yōu)解為

已知尺度因子的估計值，式（19）便可寫成

由式（20）與式（21）可得方程組

解得w的最小二乘解為

基于Cayley變換的三維點云配準算法總結(jié)如下：

算法1三維點云配準矩陣估計的閉式算法。

Ensure：攝像機運動的位姿變換參數(shù)（R，t，s）

4）利用式（23）計算R的Cayley向量wLS

3 實驗結(jié)果及分析

仿真實驗采用的數(shù)據(jù)來自于Vtk庫[19]中的三維恐龍狀點云數(shù)據(jù)集，點數(shù)目N=10 755。隨機生成姿態(tài)變換參數(shù)（R，t，s），對原始數(shù)據(jù)集進行三維相似變換，變換后的點云集記為。為驗證算法1，姿態(tài)變換參數(shù)設(shè)定如下

原始點云如圖2（a）所示。仿真中，為兩組點云添加均值為0，標準差分別為3.721 1m和3.490 8m的高斯噪聲，得到新點云，如圖 2（b）所示。

圖2 加噪前后的三維對應(yīng)點表示Fig.2 Description of pointpairs

1估計點云配準矩陣S^，得到姿態(tài)變換參數(shù)

從旋轉(zhuǎn)矩陣的誤差和平移距離誤差來進一步分析算法1的位姿估計精度。旋轉(zhuǎn)矩陣角度的誤差計算定義為

圖3 一組含噪三維對應(yīng)點的匹配結(jié)果Fig.3 Registration of 3D correspondence with noise

其中：τk是旋轉(zhuǎn)矩陣R的第k列。平移向量估計值與真實平移向量的距離被定義為平移距離誤差，即

考慮Cayley方法的魯棒性和準確性，進行多組獨立的實驗。設(shè)定不同尺度因子s并隨機生成變換參數(shù)（R，t），然后對初始點云進行相似變換并生成多組三維對應(yīng)點，添加與實驗示例相同強度的高斯噪聲。接著用Cayley方法估計姿態(tài)變換參數(shù)，最后計算估計誤差，所得結(jié)果如圖4所示。

圖4 Cayley方法的姿態(tài)估計誤差Fig.4 Error of estimated posevia Cayley approach

由圖4可知，在不同變換參數(shù)條件下，利用Cayley方法計算出的旋轉(zhuǎn)角度誤差不超過2°，平移估計相對誤差小于0.05%，這說明Cayley方法的位姿變換參數(shù)估計誤差小、精度高。

考慮點云配準的準確性，計算配準后的兩組點云的平均距離為

同樣，隨機設(shè)定變換參數(shù)（R，t，s）生成 50組三維對應(yīng)點，添加的噪聲強度保持不變。然后采用算法1對三維點云進行配準，最后計算配準后的點云集之間的平均絕對誤差edistance=5.734m。已知實驗采用的點云集規(guī)模是89.64m×197.62m×166.94m，且每組對應(yīng)點分別加了標準差為3.721 1m和3.490 8m的高斯噪聲。在此實驗條件下，配準后的點云集之間的絕對誤差在可接受范圍內(nèi)。

4 結(jié)語

針對SLAM中的地圖點云尺度漂移問題，本文提出了利用Cayley變換來估計三維對應(yīng)點之間的相似變換的閉式配準算法，從而確定相鄰兩幀圖像的攝像機姿態(tài)變換矩陣。該算法是一個直接提供位姿變換參數(shù)最優(yōu)解的閉式算法，無須迭代與校正，計算效率高，對SLAM的實時性至關(guān)重要。另外，該方法采用三維向量來表達姿態(tài)矩陣中的旋轉(zhuǎn)矩陣，在僅有3個對應(yīng)點的情況下便可提供線性解，而且其算法結(jié)構(gòu)比較簡單，容易實現(xiàn)。

同時，本文進行了多組隨機并獨立的仿真實驗，通過誤差分析來說明基于Cayley變換的閉式算法在三維點云配準問題中的優(yōu)越性。實驗結(jié)果表明，無論是旋轉(zhuǎn)矩陣誤差、相對平移誤差還是配準后的點云集之間的距離誤差都比較小，進而證實了該算法具有較高精度和較強的魯棒性。

最后要強調(diào)的是，基于Cayley變換的閉式配準算法的適用性較廣，不僅可以解決SLAM系統(tǒng)的點云配準問題，而且適用于計算機視覺和機器人學(xué)中的三維重建與三維拼接問題[17-18]。

[1]HARALICKRM,JOOH,LEECN,et al.Poseestimation from corresponding point data[J].Machine Vision for Inspection&Measurement,1989,19(6):1-84.

[2]BESLPJ,MCKAYND.Method for registration of 3D shapes[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2):239-256.

[3]HENRY P,KRAININM,HERBSTE,etal.RGB-D mapping:using Kinect-style depth cameras for dense 3D modeling of indoor environments[J].International Journal of Robotics Research,2012,31(5):647-663.

[4]HARTLEY R,ZISSERMAN A.Multiple View Geometry in Computer Vision[M].2nd ed.Cambridge:Cambridge University Press,2003.

[5]王忠立,趙 杰,蔡鶴皋.大規(guī)模環(huán)境下基于圖優(yōu)化SLAM的圖構(gòu)建方法[J].哈爾濱工業(yè)大學(xué)學(xué)報,2015,47(1):75-85.

[6]顧照鵬,劉 宏.單目視覺同步定位與地圖創(chuàng)建方法綜述[J].智能系統(tǒng)學(xué)報,2015(4):499-507.

[7]IZADIS,NEWCOMBE R A,KIM D,et al.Kinect Fusion:Real-time Dynamic 3D Surface Reconstruction and Interaction Using a Moving depth Camera[C]//International Conference on Computer Graphics and Interactive Techniques,Santa Barbara,CA,Oct 16-19,2011:1175-1184.

[8]RAULM A,MONTIEL JMM,JUAND T.ORB-SLAM:a versatile and accurate monocular SLAM system[J].IEEE Transaction on Robotics,2015,31(5):1147-1163.

[9]ARUN K S,HUANG TS,BLOSTEIN SD.Least-squares fitting of two 3-D point sets[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1987,9(5):698-700.

[10]HORNBKP,HILDENHM,NEGAHDARIPOURS.Closed-form solution of absoluteorientation using orthonormal matrices[J].Journal of the Optical Society of America A,1988,5(7):1127-1135.

[11]HORN B K P.Closed-form solution of absolute orientation using unit quaternions[J].Journal of the Optical Society of America A,1988,4(4):629-642.

[12]WALKERMW,SHAOL,VOLZRA.Estimating3D location parameters using dualnumber quaternions[J].CVGIP:Image Understanding,1991,54(3):358-367.

[13]EGGERTDW,LORUSSO A,FISHER R B.Estimating 3D rigid body transformations:a comparison of four major algorithms[J].Machine Vision and Applications,1997,9:272-290.

[14]WU Fuchao,WANG Zhiheng,HU Zhanyi.Cayley transformation and numericalstability of calibration equation[J].International Journal of Computer Vision,2009,82(4):156-184.

[15]CAYLEYA.Sur quelques proprietes des determinant gauches[J].Journal Fur Die Reine Und Angewandte Mathematik,1846(32):119-123.

[16]吳福朝.計算機視覺:Cayley變換與度量重構(gòu)[M].北京:科學(xué)出版社,2011.

[17]張鴻燕.基于視頻圖像的三維建模方法與系統(tǒng)——消化道三維建模方法與應(yīng)用研究[D].北京:中國科學(xué)院研究生院,2011.

[18]周璐莎.大尺度場景的同步定位與地圖構(gòu)建 [D].天津:中國民航大學(xué),2016.

[19]A Small Program for Rendering a Point Cloud UsingVTK[EB/OL].[2017-06-14].http://www.umiacs.umd.edu/～huytho/codes.htm.

Cayley approach for 3D points registration in visual SLAM

ZHANG Hongyan,WANG Jingyan,ZHOU Lusha,WANG Binghui
（Sino-European Institute of Aviation Engineering,CAUC,Tianjin 300300,China）

An essential problem in SLAM system is to compute the 3D rigid body transformation of cameras,which aligns two sets of points when correspondences are known.The errors accumulated during the locating and mapping process result in the drift of scale for the pairs of points.The Euclidean transformation will be degraded into the similarity transformation due to the drift.A closed-form solution using Cayley transform is presented to estimate the similarity transformation between 3D corresponding points.The accuracy and robustness of the proposed Cayley approach are verified by simulation and error analysis.Apart from SLAM system,this approach is also suitable for3D reconstruction and registration in computervision.

SLAM;3D registration;similarity transformation;Cayley transform

張鴻燕（1978—），男，湖南岳陽人，講師，博士，研究方向為計算機視覺.

TP391；V219

A

1674－5590（2017）05－0047－05

2017-01-10；

2017-02-23

國家自然科學(xué)基金項目（U1633101）；中央高?；究蒲袠I(yè)務(wù)費專項（ZXH2012H005）；國家級大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計劃項目（201510059038）

?

劉佩佩）