基于IMU輔助的快速相機(jī)重定位方法研究

楊偉力，羅達(dá)燦，陳朝猛，于陽(yáng)陽(yáng)

(貴州民族大學(xué)，貴州 貴陽(yáng) 550025)

1 引言

相機(jī)重定位是在三維環(huán)境中利用二維圖像快速、準(zhǔn)確地估計(jì)六自由度相機(jī)位姿，該任務(wù)往往作為同步定位與地圖重建[1]，增強(qiáng)現(xiàn)實(shí)[2]，自主導(dǎo)航[3]等任務(wù)的子模塊。在傳統(tǒng)相機(jī)重定位中，三維場(chǎng)景通過(guò)SfM(Structure from Motion)、視覺里程計(jì)或者地圖重建算法獲得，查詢幀圖像需與整個(gè)三維場(chǎng)景模型進(jìn)行匹配，建立2D-3D候選匹配關(guān)系，這些候選2D-3D匹配列表在RANSAC循環(huán)中利用PnP (Perspective-n-point)算法進(jìn)行幾何驗(yàn)證剔除異常點(diǎn)，對(duì)通過(guò)幾何驗(yàn)證的2D-3D匹配再利用PnP估計(jì)相機(jī)位姿[4，5]。但在大場(chǎng)景中，計(jì)算量則會(huì)指數(shù)級(jí)增長(zhǎng)，導(dǎo)致無(wú)法滿足實(shí)時(shí)應(yīng)用，特別對(duì)于敏捷型機(jī)器人或無(wú)人機(jī)需要快速實(shí)現(xiàn)定位，提升機(jī)動(dòng)性。

將視覺傳感器和慣性傳感器結(jié)合是近幾年里程計(jì)和同步定位與地圖重建的研究前沿，現(xiàn)有視覺慣性里程計(jì)中，常采用基于手工特征的濾波器[6]和非線性優(yōu)化[7]實(shí)現(xiàn)二維圖像和慣性姿態(tài)信息的融合。IMU中的加速度和陀螺姿態(tài)數(shù)據(jù)的采樣率可高達(dá)100Hz，使其更加適合于快速移動(dòng)的敏捷型機(jī)器人定位。然而消費(fèi)級(jí)IMU傳感器存在漂移、噪聲、軸偏差等，導(dǎo)致測(cè)量姿態(tài)存在誤差，簡(jiǎn)單特征融合會(huì)得到可信度較低的位姿狀態(tài)[8]，為此現(xiàn)有方法將視覺信息和慣性信息進(jìn)行融合得到更精確地機(jī)器人姿態(tài)[9，10]。

最近幾年，深度神經(jīng)網(wǎng)絡(luò)(Deep Neural Network， DNN)在相機(jī)重定位[11]和視覺慣性里程計(jì)[12]上表現(xiàn)出極具競(jìng)爭(zhēng)力的精度和魯棒性。深度神經(jīng)網(wǎng)絡(luò)能夠提取高層次特征表示，而無(wú)需對(duì)傳感器顯式地建立數(shù)學(xué)模型，這種數(shù)據(jù)驅(qū)動(dòng)型模型更加魯棒，且在高端圖像處理器中前向推理速度比傳統(tǒng)方法更快速，更適合于實(shí)時(shí)應(yīng)用。

本文設(shè)計(jì)一種基于IMU輔助的快速相機(jī)重定位算法，結(jié)合IMU信息快速地從單幀RGB場(chǎng)景圖像預(yù)測(cè)六自由度相機(jī)姿態(tài)。

2 基于IMU輔助的快速相機(jī)重定位方法

現(xiàn)有高精度相機(jī)重定位方法主要基于DSAC++[13]，其設(shè)計(jì)了可微RANSAC策略(Differentiable SAmple Consensus)，實(shí)現(xiàn)端到端訓(xùn)練相機(jī)位姿估計(jì)網(wǎng)絡(luò)。但如果網(wǎng)絡(luò)初始化存在偏差則導(dǎo)致參數(shù)搜索空間范圍擴(kuò)大，不僅需要較大網(wǎng)絡(luò)模型回歸位姿，導(dǎo)致其前向推理速度較慢，而且往往只獲得局部最優(yōu)解無(wú)法獲得高精度的相機(jī)姿態(tài)。為此，本文在DSAC++基礎(chǔ)之上，結(jié)合IMU姿態(tài)數(shù)據(jù)縮小了參數(shù)搜索空間，因此只需輕量級(jí)網(wǎng)絡(luò)估計(jì)相機(jī)位姿，從而加速相機(jī)姿態(tài)估計(jì)速度。

2.1 DSAC++

DSAC++[13]及其早期版本DSAC[14]采用兩個(gè)CNNs的端到端場(chǎng)景坐標(biāo)回歸器，其關(guān)鍵在于將傳統(tǒng)RANSAC可微分化以實(shí)現(xiàn)網(wǎng)絡(luò)端到端訓(xùn)練。DSAC采用兩個(gè)CNN分別去預(yù)測(cè)場(chǎng)景坐標(biāo)和評(píng)估每個(gè)像素對(duì)應(yīng)姿態(tài)的可靠度分?jǐn)?shù)，場(chǎng)景坐標(biāo)預(yù)測(cè)網(wǎng)絡(luò)輸入從原圖像截取的小塊，輸出為該小塊中心像素對(duì)應(yīng)的三維坐標(biāo)，從而形成稠密估計(jì)。但截取圖像塊的方式使得場(chǎng)景坐標(biāo)預(yù)測(cè)速度較慢，因此在DSAC++中利用FCN[15](Fully Convolutional Network)作為場(chǎng)景坐標(biāo)預(yù)測(cè)模型，直接回歸像素對(duì)應(yīng)的三維坐標(biāo)。特別是DSAC++采用無(wú)監(jiān)督式的重投影誤差損失訓(xùn)練網(wǎng)絡(luò)而無(wú)需場(chǎng)景坐標(biāo)真實(shí)值作為監(jiān)督。DSAC++的訓(xùn)練采用三階段訓(xùn)練策略，首先在使用PnP求解器獲得的場(chǎng)景坐標(biāo)下已給出的2D-3D匹配對(duì)上采樣一組位姿對(duì)場(chǎng)景坐標(biāo)預(yù)測(cè)模型進(jìn)行初始化；其次優(yōu)化由三維點(diǎn)至二維圖像平面的殘差之和構(gòu)成的重投影損失；最后利用一種概率式內(nèi)點(diǎn)計(jì)數(shù)對(duì)位姿假設(shè)進(jìn)行評(píng)估以此實(shí)現(xiàn)可微RANSAC。然而，如果初始化網(wǎng)絡(luò)的相近場(chǎng)景坐標(biāo)與真實(shí)值偏差較大則會(huì)降低定位精度[16]，為此，將IMU姿態(tài)信息融合至場(chǎng)景坐標(biāo)預(yù)測(cè)模型，而無(wú)需初始化步驟，提升定位精度，同時(shí)輔助姿態(tài)信息的使用縮小了網(wǎng)絡(luò)參數(shù)搜索空間，因此只需較小的網(wǎng)絡(luò)模型即可達(dá)到好的位姿估計(jì)結(jié)果。

2.2 IMU慣性編碼器

IMU可測(cè)量加速度和陀螺姿態(tài)信息，輸出的慣性數(shù)據(jù)流具有較強(qiáng)的時(shí)域特征，頻率相比圖像數(shù)據(jù)更高，一般約100Hz，更適合于快速移動(dòng)情況下的機(jī)器人位姿估計(jì)，但I(xiàn)MU測(cè)量存在偏差且易受噪聲影響，因此常使用統(tǒng)計(jì)濾波進(jìn)行狀態(tài)估計(jì)，如擴(kuò)展卡爾馬濾波(EKF)，無(wú)軌卡爾馬濾波(UKF)或者粒子濾波等。本文提出利用長(zhǎng)短期記憶模型(long short-term memory， LSTM)直接從慣性序列數(shù)據(jù)提取高層次特征表達(dá)，將高層次特征嵌入至場(chǎng)景坐標(biāo)預(yù)測(cè)網(wǎng)絡(luò)。直接使用LSTM估計(jì)姿態(tài)精度并不太高，但不會(huì)突然產(chǎn)生高度錯(cuò)誤的輸出，而濾波器方法會(huì)在某些數(shù)據(jù)幀產(chǎn)生較大誤差[17]。因此，IMU姿態(tài)的高層次特征更適合作為一組輔助特征與DSAC++中的場(chǎng)景坐標(biāo)預(yù)測(cè)網(wǎng)絡(luò)進(jìn)行特征融合，縮小模型參數(shù)搜索空間，促進(jìn)豐富的圖像特征獲得高精度位姿。

2.3 相機(jī)重定位網(wǎng)絡(luò)

所提出一種基于IMU輔助的快速相機(jī)重定位方法網(wǎng)絡(luò)框架如圖1所示。對(duì)圖像傳感器和IMU慣性傳感器進(jìn)行標(biāo)定，同時(shí)對(duì)輸出數(shù)據(jù)序列時(shí)間對(duì)齊。在訓(xùn)練和測(cè)試過(guò)程中，每輸入t時(shí)刻采集的圖像，則將t-1至t時(shí)刻之間的IMU慣性姿態(tài)數(shù)據(jù)輸入由LSTM組成的慣性編碼器中。慣性編碼器由具有128位隱藏狀態(tài)的三層雙向長(zhǎng)短期記憶模塊(Bi-directional LSTM)構(gòu)成，慣性編碼器提取高層次位姿特征aI

aI=fintertial(xI)

(1)

其中fintertial(·)是具有高度非線性特征表達(dá)能力的LSTM編碼器，xI為IMU數(shù)據(jù)序列。

輸入二維圖像的分辨率為640×480，進(jìn)入DSAC++中的場(chǎng)景坐標(biāo)預(yù)測(cè)FCN網(wǎng)絡(luò)，最終產(chǎn)生80×60個(gè)點(diǎn)的稠密場(chǎng)景坐標(biāo)預(yù)測(cè)。FCN是一種經(jīng)典網(wǎng)絡(luò)結(jié)構(gòu)，在像素級(jí)分類任務(wù)(如語(yǔ)義分割、光流估計(jì)等) 具有良好表現(xiàn)，因此在像素級(jí)三維坐標(biāo)估計(jì)任務(wù)上也能達(dá)到較好的結(jié)果。

圖1 基于IMU輔助的快速相機(jī)重定位方法網(wǎng)絡(luò)框架

記FCN前端編碼器提取的高層次特征為aV

aV=fcamera(I)

(2)

對(duì)提取的高層次IMU特征aI和圖像特征aV采用融合函數(shù)g(·)進(jìn)行特征融合得到融合特征z

z=g(aV，aI)

(3)

這里，采用直接拼接的方式進(jìn)行特征融合

gdirect(aV，aI)=[aV；aI]

(4)

其中[aV；aI]表示將aV和aI進(jìn)行拼接操作。該場(chǎng)景坐標(biāo)預(yù)測(cè)由FCN構(gòu)成，目的是回歸像素對(duì)應(yīng)的三維坐標(biāo)值，從而建立二維圖像與三維場(chǎng)景空間的2D-3D匹配關(guān)系。

在DSAC++中場(chǎng)景坐標(biāo)預(yù)測(cè)網(wǎng)絡(luò)初始化要求較為嚴(yán)格，往往利用RGB-D相機(jī)獲得精度較高的場(chǎng)景深度來(lái)初始化。如果初始化精度較低則使得網(wǎng)絡(luò)陷入局部最優(yōu)，定位精度急劇降低。因此在DSAC++中FCN采用11層深度的大網(wǎng)絡(luò)預(yù)測(cè)場(chǎng)景坐標(biāo)，擴(kuò)大網(wǎng)絡(luò)參數(shù)搜索空間同時(shí)增強(qiáng)了網(wǎng)絡(luò)表達(dá)能力。但由于自然場(chǎng)景多樣化，而數(shù)據(jù)集中場(chǎng)景較為單一，深層次網(wǎng)絡(luò)容易使重定位網(wǎng)絡(luò)過(guò)擬合，難以適應(yīng)新的自然場(chǎng)景。此外，一個(gè)環(huán)境可能存在多個(gè)相同結(jié)構(gòu)和特征的區(qū)域，如窗戶、座椅等，只用初始場(chǎng)景初始化的網(wǎng)絡(luò)從多種可能中挑選，增加六維相機(jī)姿態(tài)預(yù)測(cè)出錯(cuò)的概率。IMU高層次特征的引入，能縮小場(chǎng)景坐標(biāo)預(yù)測(cè)網(wǎng)絡(luò)所需的參數(shù)搜索空間，因此只需要輕量化網(wǎng)絡(luò)即可得到較高精度的場(chǎng)景坐標(biāo)值。實(shí)驗(yàn)中，將FCN后10層卷積層替換為4層可分離卷積層，網(wǎng)絡(luò)層數(shù)的降低自然提升前向推理速度，同時(shí)輕量化網(wǎng)絡(luò)具有更好的泛化能力，更能適應(yīng)新的自然場(chǎng)景。同時(shí)由于每一幀圖像都有時(shí)間對(duì)齊的IMU數(shù)據(jù)序列進(jìn)行輔助，因此對(duì)于存在多個(gè)相同結(jié)構(gòu)和特征的區(qū)域，也能給正確的估計(jì)姿態(tài)。

場(chǎng)景坐標(biāo)預(yù)測(cè)后，隨機(jī)挑選一個(gè)場(chǎng)景坐標(biāo)子集構(gòu)建一個(gè)相機(jī)姿態(tài)假設(shè)池{hi，i=1…N}，每一個(gè)假設(shè)hi由可微RANSAC策略依據(jù)全局一致性指標(biāo)(重投影誤差)給出一個(gè)可信度分?jǐn)?shù)s(hi)，該可信度分?jǐn)?shù)轉(zhuǎn)換為一種概率值P表示，基于最佳概率選擇最佳的相機(jī)姿態(tài)hi。該可微RANSAC策略采用一種簡(jiǎn)單且可微的柔性內(nèi)點(diǎn)計(jì)數(shù)策略實(shí)現(xiàn)為假設(shè)打分。對(duì)相機(jī)姿態(tài)假設(shè)hi及對(duì)應(yīng)像素j的重投影誤差為

(5)

其中C是相機(jī)內(nèi)參矩陣，pj是該設(shè)定相機(jī)姿態(tài)下圖像平面坐標(biāo)系下的像素坐標(biāo)值。在齊次坐標(biāo)下，進(jìn)行透射投影后作差計(jì)算重投影誤差，計(jì)算該誤差的模。內(nèi)點(diǎn)計(jì)算利用階躍函數(shù)實(shí)現(xiàn)

∑j1(τ-rj(hj，w))

(6)

其中1(·)為階躍函數(shù)，τ為內(nèi)點(diǎn)閾值。由于階躍函數(shù)不可導(dǎo)，使用sigmoid函數(shù)替換階躍函數(shù)，因此最終柔性內(nèi)點(diǎn)計(jì)數(shù)策略為

(7)

其中超參β調(diào)節(jié)sigmoid函數(shù)的柔性程度。最終假設(shè)的選擇依據(jù)softmax分布P(j；w，a)進(jìn)行

(8)

其中超參α是固定分布尺度的參數(shù)，其通過(guò)限制或加強(qiáng)得分較低對(duì)得分較高的假設(shè)的影響調(diào)節(jié)在端到端訓(xùn)練中網(wǎng)絡(luò)參數(shù)的梯度流動(dòng)。由于不同環(huán)境下內(nèi)點(diǎn)分?jǐn)?shù)幅度波動(dòng)較大，為保證分?jǐn)?shù)在有效范圍內(nèi)進(jìn)而保證端到端訓(xùn)練更加穩(wěn)定和易于收斂，利用信息熵策略自適應(yīng)地調(diào)節(jié)分布的尺度，對(duì)該softmax輸出概率采用自適應(yīng)調(diào)節(jié)超參α：

(9)

相機(jī)重定位最終獲得的位姿是一個(gè)姿態(tài)假設(shè)hi，其由相機(jī)的三維位移t和歐拉角表示的方向矩陣R構(gòu)成

hi=[R|t]

(10)

姿態(tài)hi相對(duì)世界坐標(biāo)原點(diǎn)定義，世界坐標(biāo)可旋轉(zhuǎn)任意全局參考幀，實(shí)驗(yàn)中選取三維場(chǎng)景所有相機(jī)位置的平均中心位置。

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

實(shí)驗(yàn)采用數(shù)據(jù)集Cambridge Landmarks dataset和7 Scenes dataset，Cambridge Landmarks dataset使用智能手機(jī)拍攝室外場(chǎng)景視頻，并用SfM算法標(biāo)注位姿。該數(shù)據(jù)集具有行人、車輛、光照變化、天氣變化等難度較高的場(chǎng)景。7 Scenes dataset是使用Kinect V1拍攝的一組室內(nèi)場(chǎng)景數(shù)據(jù)集，包括7個(gè)辦公室場(chǎng)景，每個(gè)場(chǎng)景在一間房?jī)?nèi)拍攝，該數(shù)據(jù)集包括許多無(wú)紋理場(chǎng)景。圖2中為兩個(gè)數(shù)據(jù)集中隨機(jī)抽取的示例圖片，由圖可見兩個(gè)數(shù)據(jù)集有光照變化、遮擋、動(dòng)態(tài)物體、天氣變化等多種改變環(huán)境表觀的因素，使相機(jī)重定位任務(wù)具有較大挑戰(zhàn)性和代表性。

圖2 數(shù)據(jù)集中隨機(jī)抽取的示例圖片

所有實(shí)驗(yàn)均在一塊Nvidia 1080 Ti上運(yùn)行，網(wǎng)絡(luò)框架使用Pytorch實(shí)現(xiàn)。超參初始為0.1，設(shè)置為0.5，內(nèi)點(diǎn)閾值設(shè)置為10個(gè)像素。使用ADAM進(jìn)行優(yōu)化，學(xué)習(xí)率設(shè)置為10-6。

表1為在兩個(gè)數(shù)據(jù)集下的不同場(chǎng)景測(cè)試得到的相機(jī)位姿估計(jì)的中值誤差，在兩個(gè)數(shù)據(jù)集上，均得到了與DSAC++極具競(jìng)爭(zhēng)力的性能。在7Scenes數(shù)據(jù)集中獲得更好的實(shí)驗(yàn)效果，特別是在角度誤差上明顯比DSAC++的誤差要小，這是因?yàn)?Scenes數(shù)據(jù)集在室內(nèi)場(chǎng)景拍攝，存在較多無(wú)紋理區(qū)域，因此圖像信息提供的位姿估計(jì)能力相對(duì)較弱，此時(shí)，IMU姿態(tài)信息對(duì)位姿的貢獻(xiàn)得到明顯體現(xiàn)，如在Stairs場(chǎng)景中，DSAC++位置誤差和角度誤差分別是0.29m和5.1°，在本文的重定位網(wǎng)絡(luò)中位置誤差降低為0.21m，角度誤差則大大降低為原來(lái)的一半?？梢?，本文的重定位網(wǎng)絡(luò)對(duì)于無(wú)紋理或弱紋理場(chǎng)景具有良好的位姿精度。在Cambridge Landmarks數(shù)據(jù)集中部分場(chǎng)景也獲得了較好結(jié)果。Cambridge Landmarks在室外拍攝，室外自然場(chǎng)景紋理更加豐富，圖像信息就能夠獲得較好的位姿估計(jì)。

表2 平均位姿估計(jì)時(shí)間表

本文提出的重定位網(wǎng)絡(luò)，將原來(lái)場(chǎng)景坐標(biāo)預(yù)測(cè)的11層FCN網(wǎng)絡(luò)替換為4層可分離卷積構(gòu)成的輕量化網(wǎng)絡(luò)模型，使得網(wǎng)絡(luò)前向推理速度更快。在位姿估計(jì)速度測(cè)試實(shí)驗(yàn)中，將不同場(chǎng)景的測(cè)試數(shù)據(jù)均分別送入網(wǎng)絡(luò)，最后計(jì)算所有測(cè)試數(shù)據(jù)的前向推理時(shí)間的平均值，由表2可見，原始DSAC++前向推理時(shí)間需要485.07ms，這雖然滿足大部分機(jī)器人定位需要，但是對(duì)機(jī)動(dòng)性要求較高的敏捷型機(jī)器人而言，無(wú)法滿足實(shí)時(shí)用。而本文提出的網(wǎng)絡(luò)平均前向推理時(shí)間只需13.53ms，縮小了30多倍。顯然本文提出的重定位網(wǎng)絡(luò)在敏捷型機(jī)器人的應(yīng)用上具有更高的優(yōu)勢(shì)。

4 結(jié)束語(yǔ)

本文提出一種基于IMU輔助的快速相機(jī)重定位方法，利用LSTM提取IMU姿態(tài)數(shù)據(jù)的高層次特征，與DSAC++中場(chǎng)景坐標(biāo)預(yù)測(cè)網(wǎng)絡(luò)進(jìn)行特征融合，縮小網(wǎng)絡(luò)參數(shù)搜索空間，從而利用輕量化的淺層網(wǎng)絡(luò)即可實(shí)現(xiàn)快速且高精度的相機(jī)位姿估計(jì)。所提方法在較多無(wú)紋理區(qū)域的場(chǎng)景圖像中，重定位精度仍然存在波動(dòng)較大，主要是因?yàn)镮MU姿態(tài)數(shù)據(jù)中噪聲的影響。在接下來(lái)的工作中，將設(shè)計(jì)如何在本框架中集成可端到端訓(xùn)練的IMU去噪模塊，提升IMU姿態(tài)信息的精度，從而在有較多無(wú)紋理場(chǎng)景下也能獲得更高的重定位精度。