非合作紋理目標(biāo)單目位姿計(jì)算

馮肖維，謝安安，肖健梅，王錫淮

(上海海事大學(xué) 電氣自動化系，上海 201306)

1 引 言

非合作目標(biāo)位姿測量廣泛應(yīng)用于機(jī)器人環(huán)境感知、空間交會對接和虛擬現(xiàn)實(shí)等領(lǐng)域[1-4]。由于無法事先獲得目標(biāo)物體外形信息，使非合作目標(biāo)位姿計(jì)算變得復(fù)雜與困難，必須首先恢復(fù)目標(biāo)表面三維信息(如紋理特征、外形輪廓等)，然后利用這些信息提供的空間約束關(guān)系解算位姿[5]。

單目視覺系統(tǒng)無法直接獲得景深信息，只能利用目標(biāo)與相機(jī)間的相對運(yùn)動來恢復(fù)場景三維結(jié)構(gòu)(Structure From Motion，SFM)[6]。而單目位姿計(jì)算不僅要恢復(fù)目標(biāo)三維結(jié)構(gòu)，還需要實(shí)時(shí)精確估計(jì)相機(jī)與目標(biāo)之間的相對位姿。Segal[7]首先恢復(fù)目標(biāo)三維信息，然后利用擴(kuò)展卡爾曼濾波算法迭代求解位姿，由于需要事先恢復(fù)目標(biāo)的三維結(jié)構(gòu)，無法實(shí)時(shí)在線應(yīng)用。在一些非結(jié)構(gòu)化應(yīng)用場景中，由于無法事先恢復(fù)目標(biāo)完整外形信息，只能根據(jù)逐漸恢復(fù)的目標(biāo)三維表面結(jié)構(gòu)信息來確定目標(biāo)與相機(jī)間的相對位姿，此過程類似于同時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)[8]。Augenstein[9]利用粒子濾波實(shí)現(xiàn)目標(biāo)同步三維重建與位姿估計(jì)，依靠粒子濾波的多假設(shè)估計(jì)提高算法抗干擾能力。為了實(shí)現(xiàn)目標(biāo)特征和相對位姿的聯(lián)合估計(jì)，前述方法需要濾波器對每幀圖像進(jìn)行處理，將時(shí)間消耗在處理具有極少新增信息的冗余圖像幀中，且忽略了累積線性化誤差。為此，Klein[4]和Mouragnon[10]提出基于關(guān)鍵幀的單目視覺方法，僅使用選定的圖像幀重構(gòu)三維信息，因此可以執(zhí)行更費(fèi)時(shí)但更準(zhǔn)確的非線性優(yōu)化。Strasdat等人證明在相同的計(jì)算成本下，基于關(guān)鍵幀的優(yōu)化技術(shù)比濾波法更為準(zhǔn)確[11]。

在單目視覺SLAM中，環(huán)境中的所有特征都可以用于解算位姿，當(dāng)鏡頭移動時(shí)，連續(xù)圖像幀之間存在豐富的共視特征，保證視覺SLAM具有較高穩(wěn)定性與抗干擾能力。但在指定目標(biāo)的位姿計(jì)算中，目標(biāo)只占圖像的局部區(qū)域，提取的特征數(shù)量有限，使得空間約束關(guān)系變少，算法的穩(wěn)定性變差，特別是當(dāng)背景特征豐富時(shí)，會進(jìn)一步給目標(biāo)特征辨識造成困難，導(dǎo)致計(jì)算失敗。此外，單目視覺具有尺度漂移問題，目標(biāo)模型在增量式恢復(fù)過程中會產(chǎn)生累積誤差，進(jìn)而影響位姿計(jì)算的精度。Mur-Artal等[8]利用關(guān)鍵幀思想增量式恢復(fù)環(huán)境三維共視特征地圖，并依靠回環(huán)檢測與位姿圖優(yōu)化保證地圖尺度一致性，實(shí)現(xiàn)相機(jī)相對非結(jié)構(gòu)環(huán)境的位姿計(jì)算，但是無法針對具體目標(biāo)計(jì)算相對位姿。在此基礎(chǔ)上，劉宗明等[12]實(shí)現(xiàn)非合作目標(biāo)基于關(guān)鍵幀的純旋轉(zhuǎn)位姿測量，由于沒有建立目標(biāo)模型，無法分離前景與背景，因此該方法只能在結(jié)構(gòu)化環(huán)境中使用。并且前述方法在優(yōu)化時(shí)都只考慮成像模型約束，而沒有考慮特征點(diǎn)與目標(biāo)輪廓之間的幾何關(guān)系。

非合作目標(biāo)位姿計(jì)算的關(guān)鍵是如何在線恢復(fù)目標(biāo)外形信息、抑制累積誤差以及避免環(huán)境干擾。本文選擇具有良好共視特性的圖像幀——模型幀——構(gòu)建共視特征模型，實(shí)現(xiàn)非合作紋理目標(biāo)三維信息的合作化。利用網(wǎng)格模型對目標(biāo)表面輪廓及其拓補(bǔ)關(guān)系進(jìn)行估計(jì)，增量式恢復(fù)目標(biāo)表面未知區(qū)域特征點(diǎn)與非共視特征點(diǎn)三維信息，增加模型包含特征的數(shù)量。并利用網(wǎng)格法向場引導(dǎo)特征模型的優(yōu)化調(diào)整，減少由于網(wǎng)格近似造成的特征恢復(fù)誤差，從而提高目標(biāo)位姿的計(jì)算精度。在此基礎(chǔ)上引入模型幀閉環(huán)優(yōu)化，抑制尺度漂移形成的累積誤差。依靠運(yùn)動預(yù)測模型跟蹤共視特征實(shí)現(xiàn)目標(biāo)相對位姿的實(shí)時(shí)計(jì)算。

2 目標(biāo)特征模型恢復(fù)

2.1 共視特征模型

(1)

考慮實(shí)時(shí)性與盡可能多的引入視差，模型幀在選擇時(shí)需要滿足如下條件：

(1)模型幀的間隔大于15個(gè)連續(xù)圖像幀；

(2)模型幀必須包含αm=40個(gè)以上目標(biāo)表面特征點(diǎn)，從而提供盡可能多的約束條件，便于三維信息的恢復(fù)與優(yōu)化；

(3)新增加的模型幀與某個(gè)已有模型幀之間共視特征的比率要小于βm=85%，使鄰近模型幀之間具有較大的視覺變化，提高匹配特征之間的視差。

由于使用了共視特征模型，位姿計(jì)算與模型優(yōu)化可以在局部區(qū)域進(jìn)行，而與圖像幀率無關(guān)。同時(shí)，為了控制共視特征模型M中模型幀的總體數(shù)量，每增加新的模型幀都會伴隨冗余幀檢測，剔除模型中特征匹配率高于γm=90%的模型幀，使得在增量式建模過程中可以靈活地?cái)U(kuò)展目標(biāo)模型，當(dāng)連續(xù)觀測到目標(biāo)表面相同區(qū)域時(shí)，模型無需被重復(fù)構(gòu)建。

2.2 模型初始化

2.3 三維信息恢復(fù)

圖1 特征點(diǎn)三維重構(gòu)Fig.1 3D reconstruction of feature points

(2)

(3)

(4)

2.4 特征點(diǎn)的選擇

為了只恢復(fù)目標(biāo)表面的特征，本文利用網(wǎng)格模型F的拓?fù)潢P(guān)系對位于網(wǎng)格外的特征點(diǎn)進(jìn)行篩選，避免背景特征加入共視特征模型。

(5)

2.5 模型法向約束優(yōu)化

由于三角網(wǎng)格模型是實(shí)際物體表面的一個(gè)近似逼近，因此根據(jù)網(wǎng)格模型重投影恢復(fù)的特征三維信息含有誤差，影響位姿計(jì)算的精度。當(dāng)共視特征模型被更新，即有新的模型幀被加入時(shí)，本文所述方法會對共視模型中與新加入模型幀具有共視關(guān)系的區(qū)域進(jìn)行優(yōu)化調(diào)整。

假設(shè)鏡頭無畸變，相機(jī)的內(nèi)參矩陣為K，則成像模型π可以表示為：

(6)

其中si,k為尺度系數(shù)，取默認(rèn)值si,k=1，則第i個(gè)特征點(diǎn)對第k個(gè)模型幀的重投影誤差為：

(7)

(8)

圖2 特征模型局部優(yōu)化Fig.2 Local optimization of feature model

如圖2所示，目標(biāo)表面三角網(wǎng)格模型F中局部區(qū)域f的某個(gè)三角面片f∈f的法向量為nf。則根據(jù)f中每個(gè)三角面片的法向量與其3條邊之間的正交關(guān)系，定義如下優(yōu)化正則項(xiàng)：

(9)

其中?f表示面f對應(yīng)的邊。則最終的基于面法向場約束的模型局部優(yōu)化表示為：

(10)

其中參數(shù)θ用于控制投影約束優(yōu)化與模型法向約束優(yōu)化之間的平衡。本文取θ=1，使重投影誤差與法向調(diào)整誤差具有相同重要性。本文使用Levenberg-Marquardt法進(jìn)行非線性優(yōu)化求解。

2.6 模型閉環(huán)尺度優(yōu)化

如果相機(jī)再次采集到目標(biāo)表面某個(gè)已建模區(qū)域，則可以通過模型中已建立的模型幀調(diào)整當(dāng)前模型幀的尺度。

(11)

(12)

(13)

(14)

(15)

其中Λi,j=I7×7為誤差項(xiàng)的協(xié)方差矩陣。

(16)

3 位姿跟蹤計(jì)算

圖3 特征共視關(guān)系的建立Fig.3 Establishing covisibility between features

(17)

(18)

(19)

4 實(shí)驗(yàn)結(jié)果分析

實(shí)驗(yàn)中相機(jī)內(nèi)參K已知，圖像分辨率為640×480。程序用C++語言編寫，運(yùn)行于一臺Intel core i5@2.40 GHz雙核筆記本電腦，內(nèi)存為8 GB，安裝的操作系統(tǒng)為Ubuntu18.04 LTS。

圖4 圓筒增量式建模結(jié)果Fig.4 Incremental modelling results of cylinder

圖4顯示圓筒目標(biāo)共視特征模型恢復(fù)過程(彩圖見期刊電子版)。圖左側(cè)顯示恢復(fù)的所有三維特征點(diǎn)，以及經(jīng)過辨識后根據(jù)有效特征點(diǎn)構(gòu)建的三維網(wǎng)格模型，綠色方框表示相機(jī)當(dāng)前位姿。同時(shí)，將左側(cè)三維網(wǎng)格模型根據(jù)計(jì)算出的目標(biāo)位姿重投影到右側(cè)對應(yīng)二維圖像幀上，右圖中綠色小方框?yàn)樘崛〉腛RB特征。隨著計(jì)算的推進(jìn)，重投影網(wǎng)格與目標(biāo)輪廓保持重合，說明本文所述方法可以對非合作目標(biāo)進(jìn)行在線同步建模與位姿計(jì)算。此外，圖4(b)中有2個(gè)背景特征點(diǎn)被誤加入模型，使模型產(chǎn)生了畸變，隨著視角的變化，新信息的到來，干擾特征點(diǎn)被辨識，并從模型中剔除，如圖4(c)所示，說明本文所述方法的特征選擇機(jī)制可以有效區(qū)分目標(biāo)與背景特征，從而保證只有目標(biāo)表面的特征用于位置計(jì)算。

圖5顯示了圖4所示建模過程中，模型幀的數(shù)量隨時(shí)間變化的情況。本文所述方法的冗余幀回收機(jī)制使模型幀的數(shù)量保持在一個(gè)穩(wěn)定的數(shù)量上，而不會隨時(shí)間持續(xù)增長。同時(shí)由于目標(biāo)識別機(jī)制，背景的變化并不會使新模型幀被加入，保證本文方法可以長時(shí)間高效運(yùn)行。圖中柱狀圖形表示不同時(shí)刻共視特征模型中每個(gè)模型幀的共視模型幀的平均數(shù)量，可以保持在5～10 frame之間，說明共視特征模型可以提供足夠的特征進(jìn)行位姿計(jì)算。

圖5 模型幀數(shù)量及其共視關(guān)系的變化Fig.5 Evolution of the number of model frames and covisiblity

為了直觀顯示模型優(yōu)化的效果，圖6為目標(biāo)三維網(wǎng)格模型在優(yōu)化過程中的變化情況。可以發(fā)現(xiàn)，隨著特征點(diǎn)不斷優(yōu)化，網(wǎng)格模型逐漸逼近真實(shí)目標(biāo)輪廓，說明在建模過程中由于網(wǎng)格近似造成的特征恢復(fù)誤差可以得到有效抑制。

圖6 目標(biāo)網(wǎng)格模型優(yōu)化結(jié)果Fig.6 Optimization results of target mesh model

圖7顯示了插入第40個(gè)模型幀后，模型優(yōu)化前后特征模型恢復(fù)與特征重投影的結(jié)果(彩圖見期刊電子版)。每幅圖從左到右分別為特征模型與對應(yīng)的真實(shí)目標(biāo)外形線框模型、當(dāng)前幀提取的特征點(diǎn)(綠色圓圈)和根據(jù)目標(biāo)當(dāng)前位姿得到的反投影特征點(diǎn)(紅色交叉)及目標(biāo)線框模型、特征重投影放大視圖。圖7(a)是未進(jìn)行模型優(yōu)化時(shí)的結(jié)果，由于初始估計(jì)的位姿與恢復(fù)的目標(biāo)共視特征模型存在誤差，重投影的特征與線框模型都與實(shí)際值具有較大偏差。圖7(b)是模型經(jīng)過無法向約束優(yōu)化后的結(jié)果，通過共視特征點(diǎn)與位姿的優(yōu)化，重投影誤差得到明顯改善，但是特征模型并沒有收斂到目標(biāo)真實(shí)輪廓。圖7(c)是經(jīng)過本文所述法向約束優(yōu)化后的結(jié)果，重投影誤差得到改善的同時(shí)，共視特征模型也得到優(yōu)化，特征點(diǎn)朝著目標(biāo)真實(shí)表面輪廓調(diào)整。

圖7 特征模型與重投影誤差比較Fig.7 Comparison of feature model and reprojection error

(20)

(21)

圖8 特征重投影誤差與偏離誤差隨優(yōu)化的變化Fig.8 Evolution of reprojection error and normalized deviation error w.r.t. optimization

為了比較不同方法位姿計(jì)算精度，將多面體目標(biāo)固定于高精度6自由度云臺上做空間運(yùn)動，利用固定于支架上的相機(jī)對目標(biāo)進(jìn)行連續(xù)拍攝，如圖11所示。由于相機(jī)和目標(biāo)之間的相對位姿真值未知，而云臺提供的平移距離和旋轉(zhuǎn)角度只能反映目標(biāo)姿態(tài)的變化量，所以根據(jù)測量位姿的變化量來衡量位姿計(jì)算精度。

圖9 精度分析實(shí)驗(yàn)平臺Fig.9 Experiment platform of precision analysis

圖10顯示了，目標(biāo)做20次位姿變換，平移距離變化量和旋轉(zhuǎn)變化量的絕對和相對測量誤差。將目標(biāo)平移量測量值與對應(yīng)云臺平移量相比較，利用該比值對測量值進(jìn)行尺度恢復(fù)后與云臺實(shí)際平移量進(jìn)行比較，得到平移誤差。而旋轉(zhuǎn)誤差根據(jù)式(22)計(jì)算[16]：

(22)

圖10 回環(huán)優(yōu)化對于共視特征模型的影響Fig.10 Influence of loop detection on covisibility

為了分析算法的時(shí)效性，對程序中各個(gè)功能模塊的運(yùn)行時(shí)間進(jìn)行了統(tǒng)計(jì)，如表1所示。由于建模過程中網(wǎng)格數(shù)量是逐漸增長的，實(shí)驗(yàn)中等待模型幀數(shù)量穩(wěn)定后再進(jìn)行統(tǒng)計(jì)。從表中看出，位姿計(jì)算率維持在8 Hz，4個(gè)模塊中最耗時(shí)的是網(wǎng)格構(gòu)建，由于各模塊采用多線程機(jī)制，因此其他3個(gè)模塊對位姿計(jì)算的實(shí)時(shí)性影響較小。網(wǎng)格構(gòu)建與模型優(yōu)化過程中，當(dāng)有新的模型幀插入時(shí)，局部優(yōu)化會被終止，因此耗時(shí)變化較大。

表1 位姿計(jì)算時(shí)效分析

5 結(jié) 論

本文根據(jù)機(jī)器視覺和計(jì)算機(jī)圖形學(xué)相關(guān)理論，實(shí)現(xiàn)了非合作紋理目標(biāo)在線同步建模與位姿計(jì)算。從連續(xù)圖像幀序列中選擇具有良好特征共視關(guān)系的圖像幀構(gòu)建基于ORB特征的共視特征模型，實(shí)現(xiàn)目標(biāo)的合作化。利用網(wǎng)格模型指導(dǎo)目標(biāo)未知區(qū)域特征點(diǎn)的增量式恢復(fù)，避免背景特征干擾和目標(biāo)整體建模。在共視模型的局部重投影優(yōu)化中引入法向約束，提高特征模型的恢復(fù)質(zhì)量，并對模型進(jìn)行閉環(huán)尺度優(yōu)化，減少累積誤差的同時(shí)提高位姿計(jì)算精度。實(shí)驗(yàn)結(jié)果表明，本文所述方法能夠?qū)Ψ呛献骷y理目標(biāo)進(jìn)行實(shí)時(shí)的特征建模與位姿計(jì)算，為基于單目視覺的三維感知與測量建模提供一種有效方法。

在今后的研究中，可以考慮引入基于像素深度匹配的光流法進(jìn)行非紋理目標(biāo)的位姿計(jì)算；并利用如直線等特征施加更加穩(wěn)定的空間約束關(guān)系，進(jìn)一步提高算法的精度與穩(wěn)定性。