三維目標(biāo)位姿跟蹤與模型修正

尚 洋，孫曉亮，張躍強(qiáng)，李 由，于起峰

國(guó)防科技大學(xué)空天科學(xué)學(xué)院圖像測(cè)量與視覺(jué)導(dǎo)航湖南省重點(diǎn)實(shí)驗(yàn)室，湖南 長(zhǎng)沙 410072

高效、穩(wěn)定的目標(biāo)位姿跟蹤技術(shù)對(duì)于諸多應(yīng)用領(lǐng)域具有重要意義，如空間應(yīng)用(交會(huì)對(duì)接、在軌服務(wù)等)、無(wú)人機(jī)應(yīng)用(自主降落等)、機(jī)器人應(yīng)用(抓取、裝配等)等。基于視覺(jué)的位姿跟蹤方法具有精度高、自主性強(qiáng)等優(yōu)勢(shì)，已成為上述應(yīng)用中近距離階段的主要位姿測(cè)量手段。基于圖像的精密位姿跟蹤方法涵蓋目標(biāo)特征提取、參數(shù)優(yōu)化求解等，屬于攝影測(cè)量與計(jì)算機(jī)視覺(jué)、數(shù)字圖像處理交叉的攝像測(cè)量或圖像測(cè)量范疇[1]。

研究人員將目標(biāo)固有的紋理、幾何特征用于對(duì)位姿參數(shù)的精密測(cè)量，基本思路是通過(guò)建立目標(biāo)圖像2D特征與3D模型之間的對(duì)應(yīng)關(guān)系，解算目標(biāo)位姿參數(shù)。雙目視覺(jué)方法[2]利用雙目交會(huì)成像理論對(duì)目標(biāo)位姿參數(shù)進(jìn)行測(cè)量，相關(guān)技術(shù)已經(jīng)較為成熟，且已得到廣泛應(yīng)用。本文關(guān)注更為通用、更富挑戰(zhàn)的利用單目圖像基于目標(biāo)3D模型的目標(biāo)精密位姿跟蹤方法。

本文開(kāi)展基于目標(biāo)3D模型的精密位姿跟蹤相關(guān)研究，具體內(nèi)容如下：①已知目標(biāo)3D精確模型，對(duì)于包含豐富直線特征的特殊目標(biāo)，提出基于直線模型的目標(biāo)位姿跟蹤方法；為處理更為一般目標(biāo)，提出法向距離迭代加權(quán)最小二乘位姿估計(jì)方法及距離圖迭代最小二乘位姿跟蹤方法。②針對(duì)目標(biāo)3D直線模型參數(shù)不準(zhǔn)確的情況，結(jié)合光束法平差思想，提出一種聯(lián)合優(yōu)化求解目標(biāo)位姿及直線參數(shù)的方法。③對(duì)于包含豐富直線特征的目標(biāo)，在其3D直線模型完全未知的情況下，利用序列圖像信息，在SFM框架下同時(shí)實(shí)現(xiàn)目標(biāo)直線模型重建與位姿參數(shù)的求解。

1 基于目標(biāo)3D模型位姿跟蹤相關(guān)研究現(xiàn)狀

針對(duì)基于目標(biāo)3D模型的位姿跟蹤問(wèn)題，本文依據(jù)目標(biāo)3D模型已知與否對(duì)相關(guān)工作進(jìn)行梳理。

1.1 目標(biāo)3D精確模型已知

在已知精確目標(biāo)3D模型的情況下，基于圖像特征，建立2D-3D對(duì)應(yīng)關(guān)系即可求解目標(biāo)位姿參數(shù)。廣泛采用的方法包含點(diǎn)、直線、邊緣輪廓幾何結(jié)構(gòu)特征、2D特征描述及深度學(xué)習(xí)等。

基于點(diǎn)特征的位姿解算即為熟知的“PnP(perspective-n-point)”問(wèn)題，通過(guò)建立n個(gè)3D空間點(diǎn)與2D圖像點(diǎn)之間的對(duì)應(yīng)關(guān)系，求解目標(biāo)位姿參數(shù)。常用的點(diǎn)特征有SIFT[3]、ORB[4]等。對(duì)于后續(xù)求解，F(xiàn)ischler等[5]對(duì)P3P問(wèn)題進(jìn)行了研究，并給出了閉式解。對(duì)于n≥6的情況，PnP問(wèn)題存在線性最小二乘解，因此后續(xù)研究工作主要集中在n≤5的情況。為提高求解的穩(wěn)定性及精度，迭代優(yōu)化求解的策略被引入到PnP問(wèn)題中，如POSIT[6]算法及正交迭代算法[7]等。點(diǎn)特征較為依賴于目標(biāo)的紋理信息，易受紋理分布、光照、噪聲等因素的影響，算法穩(wěn)健性不高。

相比于點(diǎn)特征，直線特征包含了更多信息，更有利于實(shí)現(xiàn)穩(wěn)定、精確、可靠的匹配?；谥本€對(duì)應(yīng)的位姿測(cè)量方法即為被廣泛研究的“PnL(Perspective-n-Line)”問(wèn)題，已有算法可歸為以下兩類(lèi)：

(1) 線性求解方法。文獻(xiàn)[8]給出了3條直線對(duì)應(yīng)情況下的封閉式解法。文獻(xiàn)[9]同樣對(duì)P3L問(wèn)題進(jìn)行了研究，并指出最多存在8個(gè)解。文獻(xiàn)[10]依據(jù)旋轉(zhuǎn)主軸構(gòu)建局部坐標(biāo)系，將直線分為3條一組，求解局部坐標(biāo)系與相機(jī)坐標(biāo)系之間的變換關(guān)系，以確定目標(biāo)位姿參數(shù)。文獻(xiàn)[11]提出一種直接最小二乘位姿求解方法，通過(guò)最小化一個(gè)關(guān)于姿態(tài)矩陣的二次目標(biāo)函數(shù)，得到旋轉(zhuǎn)矩陣的全局最優(yōu)解。

(2) 非線性優(yōu)化求解方法。為得到更為精確的位姿參數(shù)，一般采用迭代求解方法對(duì)線性求解結(jié)果進(jìn)行優(yōu)化，通常選擇像方誤差或物方誤差構(gòu)建目標(biāo)函數(shù)。文獻(xiàn)[12]提出先迭代求解旋轉(zhuǎn)參數(shù)，再線性求解平移參數(shù)的方法。文獻(xiàn)[13]針對(duì)仿射相機(jī)模型下的位姿迭代估計(jì)方法進(jìn)行了研究。文獻(xiàn)[14]中針對(duì)未知模型圖像直線對(duì)應(yīng)的情況進(jìn)行探討，綜合考慮匹配與位姿迭代解算，同時(shí)求解直線對(duì)應(yīng)關(guān)系和位姿參數(shù)。文獻(xiàn)[15]通過(guò)引入同名點(diǎn)及高程平面約束，實(shí)現(xiàn)可靠的航空影像中直線特征匹配。文獻(xiàn)[16]將直線特征約束引入到衛(wèi)星影像區(qū)域網(wǎng)平差優(yōu)化中。

在目標(biāo)位姿測(cè)量中，對(duì)目標(biāo)邊緣輪廓進(jìn)行離散采樣是廣泛采用的處理思路。文獻(xiàn)[17]提出的RAPiD(real-time attitude position determination)是最為研究人員熟知的基于邊緣輪廓特征的位姿跟蹤算法，其通過(guò)對(duì)輪廓進(jìn)行離散采樣，沿采樣點(diǎn)法向方向搜索匹配點(diǎn)，求解位姿參數(shù)。視覺(jué)伺服相關(guān)方法[18]在視覺(jué)伺服的框架下完成位姿跟蹤，其在本質(zhì)上等價(jià)于RAPiD算法[19]。為克服RAPiD類(lèi)方法欠穩(wěn)健性的缺陷，已有改進(jìn)可分為以下三類(lèi)[20]：

(1) 多特征融合：此類(lèi)方法將其他圖像特征同邊緣輪廓特征融合，以提高方法的穩(wěn)健性。關(guān)鍵點(diǎn)特征[21]、紋理特征[22]等被引入提升位姿跟蹤的穩(wěn)健性。

(2) 穩(wěn)健估計(jì)：此類(lèi)方法在匹配階段引入穩(wěn)健估計(jì)方法，以剔除誤匹配對(duì)位姿跟蹤算法性能的影響。Drummond等[23]將在最小二乘框架下引入M-估計(jì)，通過(guò)求解加權(quán)最小二乘問(wèn)題實(shí)現(xiàn)位姿跟蹤。RANSAC方法被Armstrong等[24]應(yīng)用于邊緣擬合中。

(3) 貝葉斯估計(jì)：此類(lèi)方法主要包含卡爾曼濾波相關(guān)方法[25]及粒子濾波相關(guān)方法[26]。卡爾曼濾波適用于噪聲滿足高斯分布假設(shè)的情況，粒子濾波更為適用于目標(biāo)作非線性運(yùn)動(dòng)且為非高斯隨機(jī)過(guò)程的情況。

基于邊緣輪廓特征的位姿測(cè)量方法通過(guò)搜索或迭代優(yōu)化，無(wú)須顯式提取點(diǎn)、直線，更為穩(wěn)健、通用。文獻(xiàn)[27]對(duì)相關(guān)工作進(jìn)行了系統(tǒng)總結(jié)，指出最主要的問(wèn)題集中在幾何結(jié)構(gòu)特征的提取與匹配上，易受復(fù)雜背景、光照變化、噪聲等因素干擾。

2D圖像中目標(biāo)檢測(cè)與識(shí)別領(lǐng)域取得了突出的研究成果，其中涉及的2D特征描述被推廣到目標(biāo)位姿參數(shù)的估計(jì)中，基本思路是基于虛擬離散位姿參數(shù)[28-29]、目標(biāo)不同側(cè)面[30]等，將針對(duì)3D目標(biāo)的位姿求解問(wèn)題轉(zhuǎn)換為若干2D圖像之間的匹配問(wèn)題，進(jìn)而采用2D特征描述建立對(duì)應(yīng)。

基于HOG(histogram of oriented gradient)特征的DPM(deformable parts model)被推廣用于目標(biāo)位姿估計(jì)中[30]。文獻(xiàn)[28]綜合HOG特征、目標(biāo)結(jié)構(gòu)顯著性特征、區(qū)域特征及紋理特征等信息，進(jìn)一步改善匹配結(jié)果?；陔x散采樣視點(diǎn)或目標(biāo)不同側(cè)面的方法，僅能得到粗略的目標(biāo)位姿參數(shù)。對(duì)此，文獻(xiàn)[29]將離散采樣視點(diǎn)條件下得到的位姿參數(shù)估計(jì)作為初值，采用MCMC(Markov chain monte carlo)采樣方法對(duì)位姿參數(shù)進(jìn)一步求精。

2D圖像特征描述在定位精度上尚不及點(diǎn)、直線等幾何特征，因此，位姿求解精度不高。另外，位姿求解的精度同樣受位姿參數(shù)離散的粗細(xì)粒度的影響。

深度學(xué)習(xí)已經(jīng)成為機(jī)器視覺(jué)、圖像處理等領(lǐng)域的熱點(diǎn)。相關(guān)研究成果也被推廣到目標(biāo)位姿跟蹤中，已有應(yīng)用主要集中在兩個(gè)方面：①類(lèi)似于2D特征描述，將學(xué)習(xí)得到的2D深度特征描述應(yīng)用于離散位姿參數(shù)后的2D圖像之間的匹配中[31-32]；②構(gòu)建卷積神經(jīng)網(wǎng)絡(luò)，直接通過(guò)2D圖像信息預(yù)測(cè)控制點(diǎn)圖像坐標(biāo)，進(jìn)而建立2D-3D控制點(diǎn)對(duì)應(yīng)，實(shí)現(xiàn)位姿參數(shù)的求解[33]。

類(lèi)似于2D圖像特征，深度特征同樣存在定位精度不高的缺陷，且位姿參數(shù)估計(jì)的精度依賴于離散的粒度。另外，深度學(xué)習(xí)是一項(xiàng)數(shù)據(jù)驅(qū)動(dòng)的技術(shù)，樣本數(shù)據(jù)是決定算法性能的關(guān)鍵因素。

1.2 目標(biāo)3D模型未知

針對(duì)目標(biāo)3D模型未知的情況，需完全利用圖像信息實(shí)現(xiàn)目標(biāo)模型參數(shù)與位姿參數(shù)的求解。已有相關(guān)方法可歸為兩類(lèi)：

(1) 利用序列圖像(或立體視覺(jué))對(duì)目標(biāo)進(jìn)行重建，依據(jù)重建目標(biāo)模型，采用絕對(duì)定向方法求解目標(biāo)位姿參數(shù)，并利用光束平差方法對(duì)結(jié)果進(jìn)行優(yōu)化。

(2) 將目標(biāo)模型參數(shù)與位姿參數(shù)的估計(jì)轉(zhuǎn)化為機(jī)器人領(lǐng)域中經(jīng)典的視覺(jué)SLAM(simultaneous localization and mapping)問(wèn)題[34]進(jìn)行考慮。已有求解方法主要有基于貝葉斯濾波器的方法[35]及基于非線性優(yōu)化的方法[36]兩類(lèi)。文獻(xiàn)[37]基于特征點(diǎn)，在FastSLAM[36]框架下同時(shí)求解目標(biāo)模型參數(shù)及位姿參數(shù)。

針對(duì)模型未知情況下的目標(biāo)位姿估計(jì)問(wèn)題，當(dāng)前已公開(kāi)發(fā)表的研究工作相對(duì)較少，且已有工作大多基于特征點(diǎn)的思路，對(duì)紋理分布、光照變化及噪聲等干擾因素的魯棒性較差。

2 基于目標(biāo)3D模型的同時(shí)位姿跟蹤與模型修正方法

開(kāi)展基于目標(biāo)3D模型的位姿跟蹤研究，分別針對(duì)目標(biāo)3D模型精確已知、不準(zhǔn)確及未知的情況提出位姿跟蹤與模型修正方法。

2.1 基于目標(biāo)3D直線模型的位姿跟蹤算法

在目標(biāo)3D直線模型已知的情況下，提出基于目標(biāo)3D直線模型的位姿跟蹤方法，包含基于直線間積分距離度量的位姿跟蹤方法及基于多假設(shè)直線對(duì)應(yīng)的位姿跟蹤方法。

2.1.1 基于直線間積分距離度量的位姿跟蹤方法

端點(diǎn)距離導(dǎo)出的直線間相似性度量對(duì)直線間斷、誤差及觀測(cè)距離等干擾的穩(wěn)健性較差，影響位姿參數(shù)的估計(jì)精度。對(duì)此，本節(jié)提出基于積分距離的直線相似性度量[18]

(1)

(2)

(3)

仿真試驗(yàn)?zāi)M衛(wèi)星位姿跟蹤，虛擬相機(jī)參數(shù)(fx=fy=1 814.8，分辨率640×640，cx=320，cy=320，無(wú)像差)。衛(wèi)星目標(biāo)模型參數(shù)(主立方體尺寸為1000×1000×1000 mm3，太陽(yáng)能帆板2860×0×557 mm3)。設(shè)定目標(biāo)旋轉(zhuǎn)靠近，沿光軸方向的運(yùn)動(dòng)距離為50 m到20 m，采樣間隔0.1 s，共得到450幀圖像數(shù)據(jù)，本節(jié)方法實(shí)現(xiàn)全程穩(wěn)定跟蹤，垂直于光軸方向上位置誤差約0.4 mm，沿光軸方向誤差約為10 mm，x、y方向上的角度誤差約為0.04°，z方向上的角度誤差約為0.01°。采用實(shí)驗(yàn)室真實(shí)場(chǎng)景獲取的圖像數(shù)據(jù)對(duì)算法進(jìn)行測(cè)試，圖1給出部分實(shí)物位姿跟蹤試驗(yàn)輪廓投影疊加結(jié)果，可見(jiàn)本節(jié)算法實(shí)現(xiàn)了有效跟蹤。

圖1 基于直線間積分距離度量的位姿跟蹤實(shí)物試驗(yàn)Fig.1 Experiment results of integral distance between lines based pose tracking

相比于基于端點(diǎn)距離的方法，基于積分距離的直線相似性度量對(duì)直線段間斷、觀測(cè)距離等穩(wěn)健性更好，對(duì)位姿參數(shù)跟蹤的精度更高。

2.1.2 基于多假設(shè)直線對(duì)應(yīng)的位姿跟蹤方法

基于模型圖像直線對(duì)應(yīng)的位姿跟蹤方法需先在圖像中檢測(cè)直線，進(jìn)而建立模型圖像直線對(duì)應(yīng)。在雜亂背景干擾或特殊觀測(cè)條件等不易建立直線對(duì)應(yīng)的情況下，位姿估計(jì)的性能將受到影響。對(duì)此，本節(jié)提出基于多假設(shè)直線對(duì)應(yīng)的位姿跟蹤方法[20]，為每條模型直線保留多條圖像直線對(duì)應(yīng)，在加權(quán)最小二乘框架下，通過(guò)最小化法向距離實(shí)現(xiàn)位姿參數(shù)的優(yōu)化。

為適應(yīng)邊緣檢測(cè)中野值點(diǎn)、圖像畸變、噪聲等因素的影響，采用加權(quán)最小二乘方法優(yōu)化位姿參數(shù)，權(quán)值由兩部分組成：直線約束權(quán)值wl(包含擬合誤差、直線間距離與夾角)及采樣點(diǎn)約束權(quán)值wp(包含法向距離、梯度模值與法向)。基于此，采用迭代加權(quán)最小二乘方法，通過(guò)優(yōu)化模型直線采樣點(diǎn)到圖像點(diǎn)的距離，求解目標(biāo)位姿參數(shù)μ，如下式所示

(4)

(5)

W為權(quán)值矩陣。

仿真試驗(yàn)同基于模型圖像直線對(duì)應(yīng)的位姿跟蹤方法中設(shè)置。本節(jié)方法實(shí)現(xiàn)全程穩(wěn)定跟蹤，垂直于光軸方向上誤差約8 mm，沿光軸方向誤差約為70 mm，x、y、z方向上的角度誤差分別約為0.5°、0.7°及0.1°。相比于基于模型圖像直線對(duì)應(yīng)的位姿跟蹤方法，本節(jié)算法精度稍差，主要原因是沿法向搜索得到的候選點(diǎn)集為單像素精度。圖2給出部分實(shí)物位姿跟蹤試驗(yàn)輪廓重投影疊加結(jié)果，可見(jiàn)本節(jié)算法在復(fù)雜光照、雜亂背景等情況下都給出了可靠的位姿跟蹤結(jié)果。

圖2 基于多假設(shè)直線對(duì)應(yīng)的位姿跟蹤實(shí)物試驗(yàn)Fig.2 Experiment results of multi-assumption line correspondence based pose tracking

基于多假設(shè)直線對(duì)應(yīng)的位姿跟蹤方法能夠有效適應(yīng)復(fù)雜光照、雜亂背景、特殊視圖條件下直線對(duì)應(yīng)錯(cuò)誤等情況，表現(xiàn)出了較強(qiáng)的魯棒性。

2.2 基于目標(biāo)3D邊緣輪廓模型的位姿跟蹤方法

為處理更為一般的目標(biāo)，本節(jié)利用目標(biāo)的輪廓特征，提出法向距離迭代加權(quán)最小二乘位姿跟蹤方法及距離圖迭代最小二乘位姿跟蹤方法。

2.2.1 法向距離迭代加權(quán)最小二乘位姿跟蹤方法

法向距離迭代加權(quán)最小二乘位姿跟蹤方法[36]通過(guò)沿采樣點(diǎn)法向一維搜索確定匹配點(diǎn)，采用最小二乘方法迭代尋找一個(gè)位姿參數(shù)修正量，使得位姿修正后邊緣輪廓與圖像中輪廓重合。

(6)

式中，μ′為位姿參數(shù)修正量；Je為描述圖像運(yùn)動(dòng)與空間運(yùn)動(dòng)之間的雅克比矩陣。

為提升方法對(duì)雜亂背景、復(fù)雜紋理、噪聲等因素的穩(wěn)健性，引入穩(wěn)健估計(jì)ρ(·)及加權(quán)策略，針對(duì)每一采樣點(diǎn)綜合考慮法向距離加權(quán)w1、候選點(diǎn)數(shù)目加權(quán)w2、邊緣強(qiáng)度加權(quán)w3及圖像位姿加權(quán)w4，最終權(quán)值w=w1w2w3w4，則式(6)可改寫(xiě)為

(7)

通過(guò)迭代求解上述加權(quán)最小二乘問(wèn)題得到位姿參數(shù)的優(yōu)化結(jié)果。

仿真試驗(yàn)?zāi)M衛(wèi)星位姿跟蹤，虛擬相機(jī)參數(shù)(fx=fy=4 434.33，分辨率764×574，cx=382，cy=287，無(wú)像差)。衛(wèi)星目標(biāo)模型參數(shù)(主立方體尺寸為1000×1000×1000 mm3，太陽(yáng)能帆板2000×0.5×400 mm3)。設(shè)定目標(biāo)運(yùn)動(dòng)從(0，0，30 000，10，30，40)(Tx(mm)，Ty，Tz，Ax(°)，Ay，Az)以速率(0，0，-300,0,0，-1)勻速運(yùn)動(dòng)到(0，0，12 900，10，30，-17)，共采集58幀圖像，本節(jié)方法實(shí)現(xiàn)全程穩(wěn)定跟蹤，垂直于光軸方向上誤差約6 mm，沿光軸方向誤差約為340 mm，x、y、z方向上的角度誤差約為1°。圖3給出部分實(shí)物位姿跟蹤試驗(yàn)輪廓重投影疊加結(jié)果，可見(jiàn)本節(jié)算法給出了正確的位姿跟蹤結(jié)果。

圖3 法向距離迭代加權(quán)最小二乘位姿跟蹤實(shí)物試驗(yàn)Fig.3 Experiment results of normal distance iterative reweighted least squares based pose tracking

2.2.2 距離圖迭代最小二乘位姿跟蹤方法

基于Chamfer匹配思想，本節(jié)提出距離圖迭代最小二乘位姿跟蹤方法[38]，通過(guò)最小二乘方法迭代修正位姿參數(shù)，使得模型投影輪廓在距離圖中的取值達(dá)到最小。

采用Sobel算法對(duì)圖像進(jìn)行邊緣檢測(cè)得到邊緣圖I，距離圖DI定義如(8)，每個(gè)像素點(diǎn)x的灰度值為其到最近邊緣點(diǎn)的歐氏距離，

(8)

DI(pi)+(DIx(pi),DIy(pi))Je(pi)μ′

(9)

DIx(pi)及DIy(pi)分別為DI在pi處x及y方向上的差分，綜合式(8)及式(9)，并引入穩(wěn)健估計(jì)，可得如下最小二乘問(wèn)題

(DIx(pi),DIy(pi))Je(pi)μ′)2)

(10)

迭代求解上述最小二乘問(wèn)題得到位姿參數(shù)的優(yōu)化結(jié)果。

仿真試驗(yàn)中以月球著陸器為對(duì)象，模擬著陸過(guò)程。著陸器主體結(jié)構(gòu)尺寸約為2×3×3 m3，虛擬相機(jī)參數(shù)(fx=fy=10 000，分辨率1024×1024，cx=512，cy=512，無(wú)像差)，著陸器下落的過(guò)程，目標(biāo)由位姿初始位姿(0，0，53.9，0，0，1)(Tx(m)，Ty，Tz，Ax(°)，Ay，Az)，以(0，0，-0.1，0，0，1)的速度運(yùn)動(dòng)到(0，0，48.9，0，0，51)，采集到51幀圖像，本節(jié)方法實(shí)現(xiàn)全程穩(wěn)定跟蹤，垂直于光軸方向上誤差約為0.8 m，沿光軸方向上誤差約為1 m，x、y方向上的角度誤差約為0.7°，z方向上的角度誤差約為0.3°。采用蘭利發(fā)布的多段實(shí)物試驗(yàn)視頻對(duì)算法進(jìn)行測(cè)試，圖4給出兩段視頻中的部分輪廓投影疊加結(jié)果，本節(jié)方法對(duì)著陸器實(shí)現(xiàn)了穩(wěn)定跟蹤。

圖4 距離圖迭代最小二乘位姿跟蹤實(shí)物試驗(yàn)Fig.4 Experiment results of distance image iterative least squares based pose tracking

本節(jié)描述的兩種位姿估計(jì)方法基于目標(biāo)邊緣輪廓特征，使得對(duì)于一般目標(biāo)的位姿估計(jì)成為可能，并在處理過(guò)程中引入加權(quán)、穩(wěn)健估計(jì)等策略，使得算法對(duì)光照變化、復(fù)雜背景、紋理等干擾具有較高穩(wěn)健性。與2.1節(jié)中描述的基于直線特征的方法相比，本節(jié)方法僅基于單像素精度的匹配點(diǎn)對(duì)實(shí)現(xiàn)位姿解算，因此算法精度稍差。

2.3 基于目標(biāo)3D直線模型的同時(shí)位姿跟蹤與模型修正方法

本節(jié)針對(duì)目標(biāo)3D直線模型參數(shù)不準(zhǔn)確的情況，結(jié)合光束法平差的思想，同時(shí)優(yōu)化求解位姿參數(shù)及直線模型參數(shù)，提出針對(duì)序列圖像的基于目標(biāo)3D直線模型的位姿跟蹤與模型修正方法。

(11)

(12)

式(11)中待優(yōu)化參數(shù)共有4M+6N個(gè)，每條圖像直線提供2個(gè)約束，N幀圖像中共可提供2MN個(gè)約束，為完成優(yōu)化求解，M及N需滿足2MN≥4M+6N。

采用衛(wèi)星仿真圖像序列對(duì)算法進(jìn)行測(cè)試，仿真參數(shù)設(shè)置同2.1節(jié)，衛(wèi)星模型包含24條直線，試驗(yàn)中，在模型直線參數(shù)中加入隨機(jī)誤差，其中位置誤差水平為30 mm，方向誤差水平為1°，圖像序列長(zhǎng)度為36幀，重復(fù)100次試驗(yàn)。統(tǒng)計(jì)試驗(yàn)結(jié)果，模型直線朝向誤差約為0.3°，位置誤差約為3.5 mm；目標(biāo)位姿平均角度誤差[20]為0.12°，平均位置誤差[18]為20.1 mm。另外，模型直線參數(shù)修正精度及位姿參數(shù)求解精度隨著參與優(yōu)化視圖數(shù)的增加而提升。

2.4 基于序列圖像直線對(duì)應(yīng)的目標(biāo)結(jié)構(gòu)重建與位姿跟蹤方法

針對(duì)目標(biāo)3D直線模型完全未知的情況，本節(jié)利用序列圖像信息，在SFM框架下實(shí)現(xiàn)目標(biāo)直線模型的重建與位姿參數(shù)的求解[20]。

式(11)描述的優(yōu)化問(wèn)題具有較多參數(shù)，直接進(jìn)行優(yōu)化求解，若初值設(shè)置不當(dāng)會(huì)造成算法不收斂。對(duì)此，本節(jié)對(duì)式(11)求解前先對(duì)部分變量進(jìn)行優(yōu)化求解：

(13)

(14)

(15)

采用仿真圖像序列測(cè)試算法性能，同2.1節(jié)中仿真試驗(yàn)設(shè)置，模型直線參數(shù)完全未知，本節(jié)算法僅基于圖像序列完成目標(biāo)直線模型重建與位姿跟蹤。采用LSD[39]算法完成直線特征的提取，相關(guān)參數(shù)采用文獻(xiàn)[37]推薦的設(shè)置(scale=0.8，sigma_scale=0.6，quant=2，ang_th=22.5，n_bins=1024，density_th=0.7)。完成在序列長(zhǎng)度為36幀情況下，對(duì)多次試驗(yàn)結(jié)果統(tǒng)計(jì)得模型直線朝向及位置誤差約為0.4°及7.5 mm；目標(biāo)位姿平均角度及平均位置誤差約為0.16°及23.5 mm。另外，選取一段真實(shí)場(chǎng)景拍攝的圖像序列，對(duì)其中未知模型參數(shù)的紙盒目標(biāo)進(jìn)行結(jié)構(gòu)重建與位姿跟蹤，相機(jī)內(nèi)參數(shù)已事先標(biāo)定。圖5(a)給出同名直線匹配結(jié)果示例，圖5(b)給出目標(biāo)結(jié)構(gòu)重建與位姿跟蹤結(jié)果的可視化顯示。本節(jié)方法準(zhǔn)確重建了目標(biāo)結(jié)構(gòu)，并給出了準(zhǔn)確、穩(wěn)定的位姿跟蹤結(jié)果。

3 結(jié) 論

本文引入機(jī)器視覺(jué)領(lǐng)域的研究成果，對(duì)三維數(shù)字?jǐn)z影測(cè)量智能構(gòu)象基礎(chǔ)工作之一——基于目標(biāo)3D模型的位姿跟蹤及模型修正問(wèn)題進(jìn)行研究。文中對(duì)已有相關(guān)工作進(jìn)行了較為系統(tǒng)地總結(jié)，分別針對(duì)目標(biāo)3D模型精確已知、不準(zhǔn)確及完全未知的情況下目標(biāo)位姿跟蹤及模型修正問(wèn)題，依據(jù)目標(biāo)直線、邊緣輪廓特征，提出系列處理方法，形成了較為系統(tǒng)、完整的解決方案。

圖5 目標(biāo)結(jié)構(gòu)重建與位姿跟蹤試驗(yàn)結(jié)果Fig.5 Experiment results of model reconstruction and pose tracking

在目標(biāo)3D模型精確已知的情況下，基于直線特征的方法在精度方面優(yōu)于基于一般輪廓特征的方法，其中基于直線間積分距離度量的位姿估計(jì)方法在仿真試驗(yàn)條件下，在垂直與沿光軸方向上位置誤差分別約為0.4 mm及10 mm，x、y及z方向上的角度誤差分別約為0.4°、0.4°及0.1°；但是，基于多假設(shè)直線對(duì)應(yīng)的方法對(duì)復(fù)雜光照、雜亂背景等干擾表現(xiàn)出更強(qiáng)的穩(wěn)健性，基于一般輪廓特征的方法能夠處理結(jié)構(gòu)更為一般的目標(biāo)；大量實(shí)際場(chǎng)景數(shù)據(jù)試驗(yàn)驗(yàn)證了相關(guān)方法能夠?qū)崿F(xiàn)穩(wěn)定、可靠的位姿跟蹤。針對(duì)目標(biāo)3D直線模型不準(zhǔn)確的情況，提出同時(shí)優(yōu)化求解模型參數(shù)及位姿參數(shù)方法，統(tǒng)計(jì)多次仿真試驗(yàn)結(jié)果，模型直線朝向及位置誤差分別約為0.3°及3.5 mm，目標(biāo)位姿參數(shù)平均角度誤差及平均位置誤差分別約為0.12°及20.1 mm，本文方法實(shí)現(xiàn)了可靠跟蹤并有效修正了模型參數(shù)。對(duì)于目標(biāo)3D模型完全未知的情況，本文提出基于序列圖像直線對(duì)應(yīng)的目標(biāo)重建與位姿跟蹤方法，仿真及實(shí)際場(chǎng)景數(shù)據(jù)試驗(yàn)驗(yàn)證了方法能夠準(zhǔn)確重建目標(biāo)直線模型并實(shí)現(xiàn)可靠位姿跟蹤，在多次仿真試驗(yàn)中，重建模型直線朝向及位置誤差分別約為0.4°及7.5 mm，位姿參數(shù)平均角度及平均位置誤差約為0.16°及23.5 mm。

針對(duì)進(jìn)一步的研究工作，可在以下方面進(jìn)行著重考慮：多特征融合，在同一的框架下合理、綜合利用可獲取的多種特征，形成互補(bǔ)，提升算法性能。深度學(xué)習(xí)，可充分挖掘深度學(xué)習(xí)特征的強(qiáng)大表征能力，用于2D-3D對(duì)應(yīng)的建立；或直接建立端到端的機(jī)制，直接由2D圖像得到3D位姿參數(shù)估計(jì)結(jié)果；深度學(xué)習(xí)是數(shù)據(jù)驅(qū)動(dòng)的技術(shù)，為克服訓(xùn)練數(shù)據(jù)制備的難題，可探索利用目標(biāo)3D模型仿真構(gòu)建訓(xùn)練樣本。

參考文獻(xiàn)：

[1] 于起峰, 尚洋. 攝像測(cè)量學(xué)原理與應(yīng)用研究[M]. 北京: 科學(xué)出版社, 2009.

YU Qifeng, SHANG Yang. Videometrics: Principles and Researches[M]. Beijing: Science Press, 2009.

[2] 尚洋. 基于視覺(jué)的空間目標(biāo)位置姿態(tài)測(cè)量方法研究[D]. 長(zhǎng)沙: 國(guó)防科技大學(xué), 2006.

SHANG Yang. Researches on Vision-based Pose Measurements for Space Target[D]. Changsha: National University of Defense Technology, 2006.

[3] LOWE D G. Distinctive Image Features from Scale-invariant Keypoints[J]. International Journal of Computer Vision, 2004, 60(2): 91-110.

[4] RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: An Efficient Alternative to SIFT or SURF[C]∥IEEE International Conference on Computer Vision. Barcelona, Spain: IEEE, 2011: 2564-2571.

[5] FISCHLER M A, BOLLES R C. Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and Automated Cartography[J]. Communications of the ACM, 1981, 24(6): 381-395.

[6] DEMENTHON D F, DAVIS L S. Model-based Object Pose in 25 Lines of Code[J]. International Journal of Computer Vision, 1995, 15(1-2): 123-141.

[7] LU C P, HAGER G D, MJOLSNESS E. Fast and Globally Convergent Pose Estimation from Video Images[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2000, 22(6): 610-622.

[8] DHOME M, RICHETIN M, LAPRESTé J T, et al. Determination of the Attitude of 3D Objects from a Single Perspective View[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1989, 11(12): 1265-1278.

[9] CHEN H H. Pose Determination from Line-to-plane Correspondences: Existence Condition and Closed-form Solutions[C]∥IEEE Third International Conference on Computer Vision. Osaka, Japan, Japan: IEEE, 1990: 374-378.

[10] ZHANG Lilian, XU Chi, LEE K M, et al. Robust and Efficient Pose Estimation from Line Correspondences[M]∥LEE K M, MATSUSHITA Y, REHG J M, et al. Computer Vision-ACCV 2012. Berlin, Heidelberg: Springer, 2012: 217-230.

[11] 李鑫, 張躍強(qiáng), 劉進(jìn)博, 等. 基于直線段對(duì)應(yīng)的相機(jī)位姿估計(jì)直接最小二乘法[J]. 光學(xué)學(xué)報(bào), 2015, 35(6): 615003.

LI Xin, ZHANG Yueqiang, LIU Jinbo, et al. A Direct Least Squares Method for Camera Pose Estimation Based on Straight Line Segment Correspondences[J]. Acta Optica Sinica, 2015, 35(6): 615003.

[12] LIU Y, HUANG T S, FAUGERAS O D. Determination of Camera Location from 2D to 3D Line and Point Correspondences[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1990, 12(1): 28-37.

[13] CHRISTY S, HORAUD R. Iterative Pose Computation from Line Correspondences[J]. Computer Vision and Image Understanding, 1999, 73(1): 137-144.

[14] DAVID P, DEMENTHON D, DURAISWAMI R, et al. Simultaneous Pose and Correspondence Determination Using Line Features[C]∥IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Madison, WI: IEEE, 2003: 424-431.

[15] 王競(jìng)雪, 宋偉東, 王偉璽. 同名點(diǎn)及高程平面約束的航空影像直線匹配算法[J]. 測(cè)繪學(xué)報(bào), 2016, 45(1): 87-95. DOI: 10.11947/j.AGCS.2016.20140527.

WANG Jingxue, SONG Weidong, WANG Weixi. Line Matching Algorithm for Aerial Image Based on Corresponding Points and Z-plane Constraints[J]. Acta Geodaetica et Cartographica Sinica, 2016, 45(1): 87-95. DOI: 10.11947/j.AGCS.2016.20140527.

[16] 曹金山, 龔健雅, 袁修孝. 直線特征約束的高分辨率衛(wèi)星影像區(qū)域網(wǎng)平差方法[J]. 測(cè)繪學(xué)報(bào), 2015, 44(10): 1100-1107. DOI: 10.11947/j.AGCS.2015.20150023.

CAO Jinshan, GONG Jianya, YUAN Xiuxiao. A Block Adjustment Method of High-resolution Satellite Imagery with Straight Line Constraints[J]. Acta Geodaetica et Cartographica Sinica, 2015, 44(10): 1100-1107. DOI: 10.11947/j.AGCS.2015.20150023.

[17] HARRIS C, STENNETT C. RAPID-A Video Rate Object Tracker[C]∥Proceedings of the British Machine Vision Conference. [s.l.]: BMVA Press, 1990: 73-78.

[18] COMPORT A I, MARCHAND E, PRESSIGOUT M, et al. Real-time Markerless Tracking for Augmented Reality: the Virtual Visual Servoing Framework[J]. IEEE Transactions on Visualization and Computer Graphics, 2006, 12(4): 615-628.

[19] COMPORT A I, KRAGIC D, MARCHAND E, et al. Robust Real-time Visual Tracking: Comparison, Theoretical Analysis and Performance Evaluation[C]∥IEEE International Conference on Robotics and Automation. Barcelona, Spain: IEEE, 2005: 2841-2846.

[20] 張躍強(qiáng). 基于直線特征的空間非合作目標(biāo)位姿視覺(jué)測(cè)量方法研究[D]. 長(zhǎng)沙: 國(guó)防科技大學(xué), 2016.

ZHANG Yueqiang. Research on Vision Based Pose Measurement Methods for Space Uncooperative Objects Using Line Features[D]. Changsha: National University of Defense Technology, 2006.

[21] CHOI C, Christensen H I. Real-Time 3D Model-based Tracking Using Edge and Keypoint Features for Robotic Manipulation[C]∥IEEE International Conference on Robotics and Automation. Anchorage, AK: IEEE, 2010: 4048-4055.

[22] BROX T, ROSENHAHN B, GALL J, et al. Combined Region and Motion-Based 3D Tracking of Rigid and Articulated Objects[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2010, 32(3): 402-415.

[23] DRUMMOND T, CIPOLLA R. Real-time Tracking of Complex Structures with On-line Camera Calibration[C]∥Proceedings of British Machine Vision Conference. Nottingham: BMVC, 1999: 574-583.

[24] ARMSTRONG M, ZISSERMAN A. Robust Object Tracking[C]∥Proceedings of Second Asian Conference on Computer Vision. Singapore: [s.n.], 1995: 58-62.

[25] YOON Y, KOSAKA A, KAK A C. A New Kalman-filter-based Framework for Fast and Accurate Visual Tracking of Rigid Objects[J]. IEEE Transactions on Robotics, 2008, 24(5): 1238-1251.

[26] CHOI C, CHRISTENSEN H I. Robust 3D Visual Tracking Using Particle Filtering on the SE(3) Group[C]∥IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011: 4384-4390.

[27] MUNDY J L. Object Recognition in the Geometric Era: A Retrospective[M]∥PONCE J, HEBERT M, SCHMID C, et al. Toward Category-level Object Recognition. Berlin, Heidelberg: Springer, 2010: 3-28.

[28] LIM J J, PIRSIAVASH H, TORRALBA A. Parsing IKEA Objects: Fine Pose Estimation[C]∥IEEE International Conference on Computer Vision. Sydney, NSW, Australia: IEEE, 2014: 2992-2999.

[29] CHOY C B, STARK M, CORBETT-DAVIES S, et al. Enriching Object Detection with 2D-3D Registration and Continuous Viewpoint Estimation[C]∥IEEE Conference on Computer Vision and Pattern Recognition. Boston, MA: IEEE, 2015: 2512-2520.

[30] FIDLER S, DICKINSON S J, URTASUN R. 3D Object Detection and Viewpoint Estimation with a Deformable 3D Cuboid Model[C]∥International Conference on Neural Information Processing Systems. Lake Tahoe, Nevada: ACM, 2012: 611-619.

[31] MOTTAGHI R, XIANG Yu, SAVARESE S. A Coarse-to-fine Model for 3D Pose Estimation and Sub-Category Recognition[C]∥IEEE Conference on Computer Vision and Pattern Recognition. Boston, MA: IEEE, 2015: 418-426.

[32] WOHLHART P, LEPETIT V. Learning Descriptors for Object Recognition and 3D Pose Estimation[C]∥IEEE Conference on Computer Vision and Pattern Recognition. Boston, MA: IEEE, 2015: 3109-3118.

[33] CRIVELLARO A, RAD M, VERDIE Y, et al. A Novel Representation of Parts for Accurate 3D Object Detection and Tracking in Monocular Images[C]∥IEEE International Conference on Computer Vision. Santiago, Chile: IEEE, 2015: 4391-4399.

[34] DISSANAYAKE M W M G, NEWMAN P, CLARK S, et al. A Solution to the Simultaneous Localization and Map Building (Slam) Problem[J]. IEEE Transactions on Robotics and Automation, 2001, 17(3): 229-241.

[35] DAVISON A J, REID I D, MOLTON N D, et al. MonoSLAM: Real-time Single Camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052-1067.

[36] KONOLIGE K, AGRAWAL M. FrameSLAM: From Bundle Adjustment to Real-time Visual Mapping[J]. IEEE Transactions on Robotics, 2008, 24(5): 1066-1077.

[37] AUGENSTEIN S. Monocular Pose and Shape Estimation of Moving Targets, for Autonomous Rendezvous and Docking[D]. Stanford, CA: Stanford University, 2011.

[38] 李由. 基于輪廓和邊緣的空間非合作目標(biāo)視覺(jué)跟蹤[D]. 長(zhǎng)沙: 國(guó)防科技大學(xué), 2013.

LI You. Contour and Edge-based Visual Tracking of Non-cooperative Space Targets[D]. Changsha: National University of Defense Technology, 2013.

[39] VON GIOI R G, JAKUBOWICZ J, MOREL J M, et al. LSD: A Fast Line Segment Detector with a False Detection Control[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2010, 32(4): 722-732.