融合二維圖像和三維點(diǎn)云的相機(jī)位姿估計(jì)

周佳樂，朱兵，吳芝路

（哈爾濱工業(yè)大學(xué) 電子與信息工程學(xué)院，黑龍江 哈爾濱 150001）

1 引言

相機(jī)位姿估計(jì)是機(jī)器人應(yīng)用、自主導(dǎo)航以及增強(qiáng)現(xiàn)實(shí)中的關(guān)鍵技術(shù)，其目的是通過當(dāng)前相機(jī)所采集的圖像信息，以及已知環(huán)境的先驗(yàn)信息，來估計(jì)出當(dāng)前相機(jī)設(shè)備在三維真實(shí)場(chǎng)景中的位置和姿態(tài)，由于相機(jī)的位姿信息包含六個(gè)自由度，因此也稱為相機(jī)的六自由度估計(jì)。隨著自動(dòng)駕駛、精確定位導(dǎo)航等領(lǐng)域?qū)Χㄎ坏木纫蟛粩嗵岣?，僅僅使用二維圖像已經(jīng)無法對(duì)現(xiàn)實(shí)的三維世界進(jìn)行準(zhǔn)確描述，因此包含深度信息的三維場(chǎng)景點(diǎn)云成為了定位技術(shù)更為關(guān)注的重點(diǎn)，通過三維場(chǎng)景點(diǎn)云以及圖像處理的相關(guān)算法可以實(shí)現(xiàn)更加精確的應(yīng)用，這也成為了相機(jī)位姿估計(jì)技術(shù)的一個(gè)發(fā)展方向。

近年，相機(jī)位姿估計(jì)技術(shù)在國(guó)內(nèi)外發(fā)展迅速，傳統(tǒng)的相機(jī)位姿估計(jì)方法首先采用SFM（Structure From Motion）算法對(duì)圖像數(shù)據(jù)庫構(gòu)建三維點(diǎn)云數(shù)據(jù)，同時(shí)將每一個(gè)3D點(diǎn)云與特征算子相關(guān)聯(lián)。對(duì)于查詢圖像，提取其特征描述子并與三維點(diǎn)云匹配，使用PnP（Perspective-n-Point）位姿求解算法解得最終的位姿［1-4］。這種方法稱為基于結(jié)構(gòu)的相機(jī)位姿估計(jì)。然而這種方法存在較大局限，一旦點(diǎn)云與特征描述符的關(guān)系確定，對(duì)于位姿查詢圖像，只能提取特定的特征描述符來進(jìn)行2D-3D匹配，這種構(gòu)建2D-3D匹配點(diǎn)的思路在關(guān)聯(lián)特征描述符不易提取的情況下表現(xiàn)往往較差。因此本文結(jié)合圖像檢索的思路與特征匹配算法，充分利用深度圖像信息，在2D-3D匹配點(diǎn)構(gòu)建思路上進(jìn)行改進(jìn)；同時(shí)提出位姿優(yōu)化目標(biāo)函數(shù)，對(duì)PnP位姿解算的結(jié)果進(jìn)行進(jìn)一步優(yōu)化，提出多階段的相機(jī)位姿估計(jì)方法。

隨著深度學(xué)習(xí)方法的廣泛應(yīng)用，大量學(xué)者投身于采用深度學(xué)習(xí)的方式來解決相機(jī)位姿估計(jì)問 題。Kendall等 人［5］將GoogleNet網(wǎng) 絡(luò) 進(jìn) 行 改進(jìn)，使用神經(jīng)網(wǎng)絡(luò)直接預(yù)測(cè)相機(jī)的六自由度位姿，然而該方法最終的相機(jī)位姿估計(jì)精度較低，文獻(xiàn)［6］進(jìn)一步證明了直接使用神經(jīng)網(wǎng)絡(luò)估計(jì)相機(jī)位姿的方法準(zhǔn)確率只能逼近使用圖像檢索的方法［7］。Li等人［8］通過孿生神經(jīng)網(wǎng)絡(luò)來擬合查詢圖像與參考圖像之間的相對(duì)位姿關(guān)系，進(jìn)一步實(shí)現(xiàn)相機(jī)的位姿估計(jì)，由于圖像中并不包含場(chǎng)景的三維場(chǎng)景信息，因此單獨(dú)使用圖像進(jìn)行位姿估計(jì)的誤差較大。Tang等人［9］利用深度檢索算法得到參考圖像，在查詢圖像與參考圖像之間構(gòu)建一個(gè)損失向量訓(xùn)練神經(jīng)網(wǎng)絡(luò)并獲得稠密場(chǎng)景匹配，進(jìn)一步實(shí)現(xiàn)相機(jī)位姿估計(jì)，其本質(zhì)是采用神經(jīng)網(wǎng)絡(luò)擬合查詢圖像與參考圖像的關(guān)系，因此前端圖像檢索中存在較大誤差時(shí)，其位姿估計(jì)精度往往較差?？傊?，目前將神經(jīng)網(wǎng)絡(luò)模型直接用于相機(jī)位姿估計(jì)的端到端學(xué)習(xí)方式，由于網(wǎng)絡(luò)模型參數(shù)的有限性，無法完全擬合所有的三維場(chǎng)景信息，導(dǎo)致其位姿估計(jì)結(jié)果的精度較低或速度較差。為實(shí)現(xiàn)更加精確且高效的相機(jī)位姿估計(jì)方法，本文將深度學(xué)習(xí)算法用于傳統(tǒng)基于結(jié)構(gòu)的相機(jī)位姿估計(jì)算法當(dāng)中，設(shè)計(jì)卷積神經(jīng)網(wǎng)絡(luò)用于獲得更為稠密的2D-3D匹配點(diǎn)，提出基于稠密場(chǎng)景回歸的多階段相機(jī)空間位姿估計(jì)方法，以此提高相機(jī)位姿估計(jì)算法的精確性。

2 數(shù)據(jù)集的構(gòu)建

不同的真實(shí)場(chǎng)景對(duì)相機(jī)位姿估計(jì)算法精度的影響較大，為推進(jìn)特定場(chǎng)景下相機(jī)位姿估計(jì)算法的研究，本文首先構(gòu)建相機(jī)位姿估計(jì)數(shù)據(jù)集。

2.1 像素坐標(biāo)與世界坐標(biāo)關(guān)系

本文基于二維圖像和三維點(diǎn)云對(duì)單目相機(jī)進(jìn)行空間位姿估計(jì)，數(shù)據(jù)集中包含了單目相機(jī)的RGB圖像以及RGB圖像所對(duì)應(yīng)的三維空間坐標(biāo)信息。在三維圖像處理中要考慮到2個(gè)三維坐標(biāo)系和2個(gè)二維坐標(biāo)系。這4個(gè)坐標(biāo)系互相嵌套，相機(jī)坐標(biāo)系以世界坐標(biāo)系為基礎(chǔ)，圖像坐標(biāo)系以相機(jī)坐標(biāo)系為基礎(chǔ)，像素坐標(biāo)系則以圖像坐標(biāo)系為基礎(chǔ)。世界坐標(biāo)系一旦確定則不會(huì)變化，而相機(jī)坐標(biāo)系一直相對(duì)世界坐標(biāo)系發(fā)生變換，圖像坐標(biāo)系則相對(duì)相機(jī)坐標(biāo)系保持穩(wěn)定不變。圖1為各個(gè)坐標(biāo)系之間的關(guān)系：

圖1 各坐標(biāo)系間的關(guān)系Fig.1 Relationship between coordinate systems

圖中，Ow-Xw，Yw，Zw為世界坐標(biāo)系，原點(diǎn)可以認(rèn)為是相機(jī)的真實(shí)世界坐標(biāo)；Oc-Xc，Yc，Zc為相機(jī)坐標(biāo)系，原點(diǎn)為相機(jī)的光心；o-x，y為圖像坐標(biāo)系。

3個(gè)坐標(biāo)系的變換關(guān)系可以通過外參矩陣以及內(nèi)參矩陣來進(jìn)行描述，外參矩陣是相機(jī)相對(duì)起始點(diǎn)的位置與姿勢(shì)的變化，用來描述世界坐標(biāo)系和相機(jī)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系；內(nèi)參矩陣是相機(jī)本身的固定參數(shù)，是指相機(jī)坐標(biāo)系下的物體投影到圖像平面下的內(nèi)部參數(shù)，通常用來描述相機(jī)坐標(biāo)系和圖像坐標(biāo)系之間的關(guān)系。通過坐標(biāo)系關(guān)系轉(zhuǎn)換，可以得到像素坐標(biāo)系與世界坐標(biāo)系的關(guān)系為：

在這種轉(zhuǎn)換關(guān)系中，內(nèi)參矩陣可以通過相機(jī)標(biāo)定的方式精準(zhǔn)獲得，在相機(jī)應(yīng)用環(huán)境不發(fā)生變化時(shí)，可認(rèn)為其不發(fā)生變化。然而，在相機(jī)位姿發(fā)生變化時(shí)，其外參矩陣也將發(fā)生變化。本文的目標(biāo)是通過模型估計(jì)出當(dāng)前相機(jī)的外參矩陣，因此數(shù)據(jù)集中包括相機(jī)所獲得的RGB圖像（像素坐標(biāo)）、RGB圖像所對(duì)應(yīng)的相機(jī)外參矩陣以及相機(jī)所處場(chǎng)景的三維真實(shí)場(chǎng)景信息（世界坐標(biāo)）。

2.2 SFM三維重建

通過分析像素坐標(biāo)和世界坐標(biāo)之間的關(guān)系，可以知道構(gòu)建數(shù)據(jù)集的關(guān)鍵是獲得單張RGB圖像的外參矩陣以及相機(jī)所處場(chǎng)景的三維真實(shí)場(chǎng)景。本文采用SFM算法對(duì)相機(jī)所拍攝場(chǎng)景進(jìn)行三維重建，并獲得重建后每一張RGB圖在三維場(chǎng)景中的外參信息。SFM算法是一種基于各種收集到的無序圖片進(jìn)行三維重建的離線算法，通過相機(jī)的移動(dòng)采集不同視角的圖像，求解相機(jī)的相對(duì)位姿信息并恢復(fù)出場(chǎng)景的三維信息。通過SFM算法，可以求解得到每一張RGB圖像的三維稀疏點(diǎn)云數(shù)據(jù)以及其相對(duì)于該三維稀疏點(diǎn)云下的位姿信息，圖2是采用該算法進(jìn)行三維重建的效果示意圖（彩圖見期刊電子版），圖中紅色軌跡為每一張RGB圖像所對(duì)應(yīng)的真實(shí)相機(jī)位姿，黑色點(diǎn)為三維稀疏點(diǎn)云：

圖2 三維稀疏點(diǎn)云重建及位姿數(shù)據(jù)集構(gòu)建Fig.2 3D sparse point cloud reconstruction and pose dataset construction

基于SFM算法進(jìn)行三維重建的三維點(diǎn)云是由特征匹配所提供的，由于這些匹配點(diǎn)的提取是稀疏的，因此采用SFM算法進(jìn)行三維重建得到的點(diǎn)云數(shù)據(jù)也將是稀疏的。為了研究更加精準(zhǔn)的位姿估計(jì)算法，本文在構(gòu)建數(shù)據(jù)集的時(shí)還需產(chǎn)生稠密的點(diǎn)云數(shù)據(jù)。SFM算法可以解算得到每一幀RGB圖像在當(dāng)前三維稀疏點(diǎn)云下的位姿矩陣，為得到圖像的稠密三維坐標(biāo)信息，還需要采用深度相機(jī)同步采集每一幀RGB圖像的深度信息［10］，通過前述三維重建過程中得到的位姿以及坐標(biāo)間的變換關(guān)系，求解出場(chǎng)景的稠密三維點(diǎn)云。本文實(shí)驗(yàn)使用Astra Mini深度相機(jī)作為RGB圖像以及深度圖像采集設(shè)備。經(jīng)過實(shí)際標(biāo)定實(shí)驗(yàn)，該深度相機(jī)的深度有效測(cè)量范圍為0.6～8 m，其精度為±(1～3)mm/m，深度圖像數(shù)據(jù)為16位深度值，其深度值為相機(jī)與物體的實(shí)際距離，單位為mm。由于實(shí)驗(yàn)中不可避免存在無效的測(cè)量深度值（深度圖像中黑色邊緣部分），在實(shí)際實(shí)驗(yàn)過程中，將有效深度值對(duì)RGB圖像進(jìn)行掩摸處理，舍棄無效的深度值。本文對(duì)實(shí)驗(yàn)室進(jìn)行采集，最終重建的結(jié)果如圖3所示。

圖3 稠密點(diǎn)云數(shù)據(jù)構(gòu)建Fig.3 Dense point cloud data construction

通過以上方法，構(gòu)建得到最終的數(shù)據(jù)集，該數(shù)據(jù)集包含4 000張RGB圖像、深度圖以及每張RGB圖像對(duì)應(yīng)的位姿信息，本文采集的數(shù)據(jù)為連續(xù)幀圖像，將圖像以1 000幀為單位劃分為4個(gè)序列，其中1，3序列作為訓(xùn)練集，2，4序列作為測(cè)試集，具體信息如表1所示。

表1 Labcore數(shù)據(jù)集Tab.1 Detail of labcore dataset

3 多階段相機(jī)位姿估計(jì)

傳統(tǒng)基于結(jié)構(gòu)的相機(jī)位姿估計(jì)方法是找到2D-3D匹配點(diǎn)，通過PnP位姿求解算法求解方程，以解算得到當(dāng)前相機(jī)的具體位姿［1-4］，該方法在構(gòu)建點(diǎn)云數(shù)據(jù)時(shí)將3D點(diǎn)與特定的2D特征點(diǎn)進(jìn)行關(guān)聯(lián)，一旦數(shù)據(jù)構(gòu)建完成，其特征描述符便無法更換，而不同場(chǎng)景下采用不同特征描述符所得的位姿估計(jì)結(jié)果差異較大，這也成為其在特征描述符不易提取的環(huán)境中表現(xiàn)較差的主要原因。因此本文首次將圖像檢索算法引入2D-3D匹配點(diǎn)的構(gòu)建當(dāng)中，并采用傳統(tǒng)基于結(jié)構(gòu)算法的相機(jī)位姿估計(jì)思路，同時(shí)構(gòu)建目標(biāo)函數(shù)優(yōu)化位姿估計(jì)結(jié)果，提出基于多階段的相機(jī)位姿估計(jì)方法，該方法結(jié)合圖像檢索方法與基于結(jié)構(gòu)相機(jī)位姿估計(jì)方法的優(yōu)勢(shì)，提升了2D-3D匹配點(diǎn)的匹配速度與精度，具體過程為：第一階段，構(gòu)建2D-3D匹配點(diǎn)，采用深度圖像檢索算法檢索出查詢圖像的最近鄰幀作為參考圖像，通過特征匹配算法及坐標(biāo)映射關(guān)系找到用于位姿解算的2D-3D匹配點(diǎn)；第二階段，粗位姿求解并剔除外點(diǎn)，使用PnP位姿求解算法求解相機(jī)位姿，對(duì)求解得到的位姿進(jìn)行評(píng)分，采用隨機(jī)抽樣一致（Random Sample Consensus，RANSAC）算法的思路剔除誤匹配點(diǎn)，得到初始位姿估計(jì)；第三階段，位姿對(duì)齊優(yōu)化，將前一階段求解的位姿作為初始位姿，設(shè)計(jì)目標(biāo)函數(shù)，用三維點(diǎn)云將查詢圖像位姿與參考圖像位姿對(duì)齊，采用最小二乘法對(duì)初始位姿估計(jì)結(jié)果進(jìn)行進(jìn)一步優(yōu)化，得到最佳位姿估計(jì)結(jié)果。圖4是整個(gè)算法的流程圖。

圖4 多階段相機(jī)位姿估計(jì)方法流程Fig.4 Flow of multistage camera pose estimation

3.1 構(gòu)建2D-3D匹配點(diǎn)

為檢索得到查詢圖像的最近鄰圖像（與查詢圖像真實(shí)位姿最接近的圖像），本文采用卷積神經(jīng)網(wǎng)絡(luò)對(duì)查詢圖像以及已知位姿圖像數(shù)據(jù)庫的全局特征進(jìn)行提取。采用VGG網(wǎng)絡(luò)結(jié)構(gòu)進(jìn)行卷積神經(jīng)網(wǎng)絡(luò)的搭建，為了使得深度圖像檢索算法能夠適應(yīng)大部分的圖像場(chǎng)景，將VGG網(wǎng)絡(luò)在ImageNet數(shù)據(jù)集上進(jìn)行預(yù)訓(xùn)練，通過一個(gè)全連接層對(duì)圖像數(shù)據(jù)庫以及查詢圖像的每一張圖像提取出一個(gè)1×512的圖像全局特征向量。在圖像檢索階段，將圖像數(shù)據(jù)庫中與查詢圖像相似度最高的圖像作為參考圖像。

完成最近鄰幀檢索之后，對(duì)查詢圖像及參考圖像進(jìn)行特征匹配以進(jìn)一步找到2D-3D匹配點(diǎn)。為了進(jìn)一步分析不同特征描述子對(duì)特征匹配結(jié)果及最終的位姿解算結(jié)果的影響，本文分別采用ORB（Oriented Fast and Rotated Brief）算 子、SIFT（Scale Invariant Feature Transform）算子以及SURF（Speeded-Up Robust Features）算 子［11］對(duì)圖像的特征進(jìn)行提取，并使用暴力匹配法進(jìn)行匹配。

3.2 粗位姿求解并剔除外點(diǎn)

使用特征匹配算法對(duì)查詢圖像及參考圖像的特征點(diǎn)進(jìn)行匹配，并計(jì)算像素和世界坐標(biāo)間的坐標(biāo)映射關(guān)系獲得2D-3D匹配點(diǎn)，利用PnP算法對(duì)2D-3D匹配點(diǎn)進(jìn)行位姿解算得到當(dāng)前相機(jī)具體位姿。然而實(shí)際上特征匹配算法的匹配結(jié)果中可能存在誤匹配點(diǎn)，導(dǎo)致最終使用PnP算法進(jìn)行位姿求解時(shí)的結(jié)果誤差較大，為了進(jìn)一步剔除誤 匹 配 點(diǎn)，本 文 采 用RANSAC算 法［1-3，13］的思 想對(duì)相機(jī)位姿估算的結(jié)果進(jìn)行進(jìn)一步的優(yōu)化。圖5是采用RANSAC算法思想進(jìn)行位姿估計(jì)結(jié)果優(yōu)化的流程圖。

圖5 RANSAC算法流程Fig.5 Flow of RANSAC algorithm

模型估計(jì)：在特征匹配算法得到的2D-3D特征匹配點(diǎn)中選取一定數(shù)量的點(diǎn)對(duì)作為PnP求解問題的輸入，并采用PnP求解算法解得相機(jī)位姿假設(shè)。

模型評(píng)價(jià)：為了判斷樣本數(shù)據(jù)點(diǎn)是樣本內(nèi)點(diǎn)還是樣本外點(diǎn)，定義重投影誤差作為判斷依據(jù)，重投影誤差的定義如式（2）：

式中：pi為查詢二維圖像中第i個(gè)像素在圖像中的2D位置，C為相機(jī)的內(nèi)參矩陣，hJ為相機(jī)位姿假設(shè)數(shù)學(xué)模型，yi為pi對(duì)應(yīng)的三維場(chǎng)景坐標(biāo)真實(shí)值。由此，設(shè)置誤差閾值τ，本文實(shí)驗(yàn)中τ=5，如果ei＜τ，則稱對(duì)應(yīng)點(diǎn)對(duì)pi-yi為內(nèi)點(diǎn)；反之，對(duì)應(yīng)點(diǎn)pi-yi為外點(diǎn)。記錄每個(gè)數(shù)學(xué)模型的內(nèi)點(diǎn)數(shù)量作為模型的評(píng)分s(hJ)。

模型篩選：重復(fù)以上步驟直至抵達(dá)迭代次數(shù)（本文實(shí)驗(yàn)設(shè)置為10 000次）上限或所有匹配點(diǎn)的重投影誤差均小于預(yù)設(shè)閾值。統(tǒng)計(jì)不同的位姿假設(shè)數(shù)學(xué)模型下的樣本內(nèi)點(diǎn)數(shù)量，樣本內(nèi)點(diǎn)數(shù)量最多，即模型的評(píng)分最大的模型作為最佳數(shù)學(xué)模型，保留所有樣本內(nèi)點(diǎn)，剔除樣本外點(diǎn)，并將剔除外點(diǎn)后的對(duì)應(yīng)點(diǎn)對(duì)用于下一步的點(diǎn)云配準(zhǔn)操作。

3.3 位姿對(duì)齊優(yōu)化

位姿優(yōu)化的目標(biāo)是尋找最優(yōu)的相機(jī)估計(jì)位姿(R?，T?)，使得估計(jì)的位姿矩陣與真實(shí)位姿矩陣的誤差達(dá)到最小。光束平差法（Bundle Adjustment，BA）是相機(jī)位姿優(yōu)化領(lǐng)域的一個(gè)經(jīng)典方法，該方法在大量文獻(xiàn)［11-16］中的應(yīng)用也已證明其位姿優(yōu)化的有效性。因此本文參考文獻(xiàn)［16］的優(yōu)化目標(biāo)函數(shù)，設(shè)計(jì)目標(biāo)函數(shù)構(gòu)建一個(gè)光束平差問題來優(yōu)化第二階段中相機(jī)位姿估計(jì)結(jié)果，使得三維點(diǎn)云對(duì)于查詢圖像q與參考圖像r的重投影差異最小化，定義的目標(biāo)函數(shù)如式（3）所示：

式中：piq=ChJ yi為參考圖像r視場(chǎng)范圍內(nèi)的第i個(gè)3D點(diǎn)yi在當(dāng)前相機(jī)位姿估計(jì)下的重投影坐標(biāo)值，[?]為亞像素級(jí)別插值，本文實(shí)驗(yàn)過程中采用線性插值，Iq為當(dāng)前像素點(diǎn)的歸一化像素值。在歐式群SE（3）中，采用對(duì)應(yīng)的李代數(shù)對(duì)位姿更新δ∈R6進(jìn)行參數(shù)化［13］。本文采用高斯牛頓法來解決上述最小二乘問題，雅克比矩陣和黑塞矩陣計(jì)算如式（4）所示：

通過雅克比矩陣和黑塞矩陣計(jì)算位姿更新：

則最終的位姿更新為：

采用上述過程對(duì)估計(jì)的相機(jī)位姿(?)進(jìn)行更新，直到位姿更新量δ足夠小時(shí)停止更新，本文 實(shí) 驗(yàn) 中 當(dāng)dT＜0.005，dR＜0.05時(shí) 停 止更新。

4 基于稠密場(chǎng)景回歸的多階段相機(jī)位姿估計(jì)

4.1 稠密場(chǎng)景回歸

第3節(jié)中所述采用深度圖像檢索及特征匹配算法來確定2D-3D匹配點(diǎn)，進(jìn)一步實(shí)現(xiàn)相機(jī)的空間位姿估計(jì)。然而當(dāng)圖像中特征點(diǎn)不易提取以及查詢圖像與參考圖像場(chǎng)景變化較大的情況下，該方法存在大量的誤匹配點(diǎn)，導(dǎo)致最終的位姿解算結(jié)果誤差較大甚至出現(xiàn)錯(cuò)誤。同時(shí)由于特征匹配點(diǎn)較少，當(dāng)存在大量誤匹配點(diǎn)時(shí)，通過RANSAC算法剔除了異常點(diǎn)［17］，便無法得到足量用于PnP解算的2D-3D匹配點(diǎn)。然而特征點(diǎn)在每一幀圖像像素點(diǎn)中占比其實(shí)是較少的，即提取的特征點(diǎn)相對(duì)與圖像像素點(diǎn)來說是稀疏的，獲得的2D-3D匹配點(diǎn)也是稀疏的。為了獲得稠密的2D-3D匹配點(diǎn)，以此來提高相機(jī)空間位姿估計(jì)的精度，本文采用卷積神經(jīng)網(wǎng)絡(luò)對(duì)輸入的RGB圖像進(jìn)行場(chǎng)景坐標(biāo)回歸，構(gòu)建稠密的2D-3D匹配點(diǎn)，以實(shí)現(xiàn)第3節(jié)所述多階段相機(jī)位姿估計(jì)方法中的第一階段2D-3D匹配點(diǎn)的構(gòu)建，提出基于稠密場(chǎng)景回歸的多階段相機(jī)位姿估計(jì)方法。

本文采用ResNet神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)，在保證網(wǎng)絡(luò)深度的同時(shí)防止網(wǎng)絡(luò)的退化。為實(shí)現(xiàn)相機(jī)稠密坐標(biāo)回歸并減少計(jì)算量，對(duì)ResNet網(wǎng)絡(luò)結(jié)構(gòu)進(jìn)行改進(jìn)，將全連接層刪除，構(gòu)建全卷積神經(jīng)網(wǎng)絡(luò)，通過卷積層輸出一個(gè)3通道的特征圖作為最終的預(yù)測(cè)結(jié)果，網(wǎng)絡(luò)輸出為160×120的三維場(chǎng)景坐標(biāo)回歸，即是輸入圖像下采樣后的三維場(chǎng)景坐標(biāo)。圖6是本文構(gòu)建的稠密場(chǎng)景回歸網(wǎng)絡(luò)結(jié)構(gòu)。

圖6 稠密場(chǎng)景回歸網(wǎng)絡(luò)結(jié)構(gòu)Fig.6 Structure dense scene regression network

采用以上神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)對(duì)查詢圖像的三維坐標(biāo)值進(jìn)行回歸，首先通過4個(gè)卷積層對(duì)圖像的特征進(jìn)行提取，之后經(jīng)過3個(gè)殘差網(wǎng)絡(luò)結(jié)構(gòu)對(duì)場(chǎng)景回歸進(jìn)行更好地?cái)M合，通過卷積層輸出最終的三維坐標(biāo)預(yù)測(cè)值。本文的Loss函數(shù)定義如公式（7）［18］：

式中：gt_coords表示查詢圖像每一個(gè)像素區(qū)域?qū)?yīng)的三維坐標(biāo)真實(shí)值，pred_coords表示網(wǎng)絡(luò)模型對(duì)查詢圖像每一個(gè)像素區(qū)域（4×4像素點(diǎn)區(qū)域）三維坐標(biāo)的預(yù)測(cè)值，Norm(·)為歸一化函數(shù)，s為損失因子，i表示第i個(gè)像素區(qū)域，n為像素區(qū)域總數(shù)。

4.2 場(chǎng)景回歸結(jié)果

本文采用訓(xùn)練完成后的網(wǎng)絡(luò)對(duì)數(shù)據(jù)集下的測(cè)試圖像進(jìn)行場(chǎng)景回歸。本文將真實(shí)場(chǎng)景坐標(biāo)以及預(yù)測(cè)的場(chǎng)景坐標(biāo)的三維信息（x，y，z）映射到圖像的RGB三通道，以RGB圖像進(jìn)行展示，各數(shù)據(jù)集下回歸的結(jié)果如圖7所示。

圖7 各場(chǎng)景下場(chǎng)景回歸的結(jié)果Fig.7 Result of scene regression

由場(chǎng)景回歸的結(jié)果可以看出，采用神經(jīng)網(wǎng)絡(luò)對(duì)圖像的三維坐標(biāo)進(jìn)行回歸結(jié)果邊緣信息丟失較為嚴(yán)重，這是由于本文為了減少后續(xù)位姿求解過程的計(jì)算量，將圖像劃分成像素區(qū)域，對(duì)每一個(gè)像素區(qū)域的三維坐標(biāo)真實(shí)值進(jìn)行回歸，即是原始圖像三維坐標(biāo)下采樣后的結(jié)果，因此丟失了大部分邊緣信息；而對(duì)于其余部分，在本文實(shí)驗(yàn)的大部分場(chǎng)景下，圖像的三維坐標(biāo)預(yù)測(cè)值和三維坐標(biāo)真實(shí)值基本一致。

采用卷積神經(jīng)網(wǎng)絡(luò)對(duì)圖像中的像素點(diǎn)進(jìn)行三維坐標(biāo)回歸也可能存在一定錯(cuò)誤，因此在回歸得到查詢圖像場(chǎng)景坐標(biāo)的基礎(chǔ)上，本文采用隨機(jī)抽樣的方式，隨機(jī)抽取回歸結(jié)果中一定數(shù)量的場(chǎng)景坐標(biāo)預(yù)測(cè)值作為PnP求解過程的輸入，解得一組相機(jī)的位姿假設(shè)hi。重復(fù)以上步驟構(gòu)造n組位姿假設(shè)集合（本文實(shí)驗(yàn)過程中n=64），通過第三節(jié)中的RANSAC算法思路對(duì)位姿假設(shè)進(jìn)行篩選，同時(shí)采用位姿對(duì)齊對(duì)估計(jì)位姿進(jìn)行優(yōu)化，得到最終的相機(jī)位姿估計(jì)結(jié)果。

5 位姿估計(jì)實(shí)驗(yàn)結(jié)果

5.1 實(shí)驗(yàn)環(huán)境

本文在公開數(shù)據(jù)集7scenes［14］以及自建數(shù)據(jù)集Labcore下對(duì)網(wǎng)絡(luò)參數(shù)進(jìn)行訓(xùn)練，其中7scenes數(shù)據(jù)集涵蓋了7種不同的室內(nèi)場(chǎng)景，是用于相機(jī)位姿估計(jì)的一個(gè)標(biāo)準(zhǔn)數(shù)據(jù)集。實(shí)驗(yàn)選取的硬件工作平臺(tái)為臺(tái)式計(jì)算機(jī)，配有NVIDIA3090顯卡一張，網(wǎng)絡(luò)模型通過PyTorch平臺(tái)搭建。

5.2 位姿估計(jì)精度定量分析

相機(jī)位姿估計(jì)結(jié)果的精度可用位姿估計(jì)誤差低于誤差閾值（同時(shí)滿足平移誤差閾值和旋轉(zhuǎn)誤差閾值）幀數(shù)所占總幀數(shù)的百分比以及位姿估計(jì)誤差中值來進(jìn)行評(píng)價(jià)［15-16，19-20］。首先利用隨 著誤差閾值增大時(shí)，位姿估計(jì)誤差低于誤差閾值數(shù)所占測(cè)試集總數(shù)的百分比來衡量相機(jī)位姿估計(jì)的準(zhǔn)確性。圖8為本文所提方法在自建數(shù)據(jù)集Labcore以及公開數(shù)據(jù)集7scenes上實(shí)驗(yàn)的相機(jī)位姿估計(jì)準(zhǔn)確率結(jié)果。從圖中可以看出，采用多階段相機(jī)位姿估計(jì)方法的過程中，對(duì)于大部分場(chǎng)景，使用SIFT算子以及SURF算子作為特征點(diǎn)提取時(shí)位姿估計(jì)準(zhǔn)確率較高，在位姿估計(jì)誤差閾值為5 cm/5°的范圍內(nèi)，自建數(shù)據(jù)集Labcore上的估計(jì)精度最大為87.3%，公開數(shù)據(jù)集7scenes上，數(shù)據(jù)集中環(huán)境變化較大時(shí)其表現(xiàn)較差（后3個(gè)場(chǎng)景）；同時(shí)，將稠密場(chǎng)景回歸網(wǎng)絡(luò)引入多階段相機(jī)位姿估計(jì)方法，以此構(gòu)建更為稠密的2D-3D匹配點(diǎn)之后，不論在自建數(shù)據(jù)集還是在公開數(shù)據(jù)集上相機(jī)位姿估計(jì)的精度均有大幅度提升。

圖8 位姿估計(jì)精度隨誤差閾值變化Fig.8 Accuracy of pose estimation varies with the error threshold

圖9為引入稠密場(chǎng)景回歸后多階段相機(jī)位姿估計(jì)方法與其他方法在公開數(shù)據(jù)集以及自建數(shù)據(jù)集上的位姿估計(jì)準(zhǔn)確率對(duì)比（誤差閾值范圍為5 cm/5°，7Scenes下為7個(gè)場(chǎng)景的平均準(zhǔn)確率，Labcore下為單個(gè)場(chǎng)景準(zhǔn)確率）。可以看到在公開數(shù)據(jù)集下，本文方法的位姿估計(jì)準(zhǔn)確率為82.7%，優(yōu)于DSM［9］，DSAC［12］，DSAC++［15］，SANet［20］，Pixloc［16］，同時(shí)本文將現(xiàn)存幾種位姿估計(jì)較為優(yōu)越的方法在自建數(shù)據(jù)集上進(jìn)行估計(jì)，結(jié)果表明本文方法能達(dá)到94.8%的位姿估計(jì)準(zhǔn)確率，較其余方法更加優(yōu)越。其中DSAC++在自建數(shù)據(jù)集上表現(xiàn)相對(duì)較差，這是由于自建數(shù)據(jù)集環(huán)境變化較大，該網(wǎng)絡(luò)無法完全擬合數(shù)據(jù)集中的場(chǎng)景。

圖9 位姿估計(jì)準(zhǔn)確率對(duì)比（5 cm/5°）Fig.9 Pose estimation accuracy comparison（5 cm/5°）

其次用平移誤差和旋轉(zhuǎn)誤差中值來衡量相機(jī)位姿估計(jì)算法的精度。圖10是本文方法在自建數(shù)據(jù)集Labcore以及7scenes數(shù)據(jù)集上進(jìn)行位姿估計(jì)后的估計(jì)誤差中值結(jié)果。從圖中可以看出，對(duì)于大部分場(chǎng)景，本文方法的位姿估計(jì)誤差中值小于5 cm/5°。為了驗(yàn)證本文提出方法的精度，根據(jù)平移誤差和旋轉(zhuǎn)誤差中值來與其他相機(jī)位姿估計(jì)方法相比較，表2為本文方法以及其他相機(jī)位姿估計(jì)方法的相機(jī)位姿估計(jì)精度，表格中的數(shù)據(jù)均來自于原始論文，加粗部分為取得各對(duì)比算法中最優(yōu)的結(jié)果。從表2可以看出，采用相機(jī)位姿估計(jì)的誤差中值作為評(píng)價(jià)標(biāo)準(zhǔn)時(shí)，相較于其余相機(jī)位姿估計(jì)方法，本文方法在大部分場(chǎng)景中位姿估計(jì)精度均有所提升。

圖10 各場(chǎng)景下的中值誤差結(jié)果Fig.10 Results of median localization Errors

表2 相機(jī)位姿估計(jì)算法性能對(duì)比Tab.2 Performance comparison of camera pose estimation algorithms

5.3 位姿估計(jì)時(shí)耗定量分析

為對(duì)本文方法進(jìn)行位姿估計(jì)的實(shí)時(shí)性進(jìn)行評(píng)估，本文實(shí)驗(yàn)中在7scenes數(shù)據(jù)集上17 000張用于測(cè)試的場(chǎng)景圖像進(jìn)行位姿估計(jì)，并取位姿估計(jì)耗時(shí)均值作為評(píng)價(jià)指標(biāo)。表3列出了本文方法與DSM［9］，DSAC++［15］，Pixloc［16］對(duì)單幀場(chǎng)景圖像進(jìn)行位姿估計(jì)時(shí)每個(gè)步驟的耗時(shí)均值，實(shí)驗(yàn)環(huán)境均為5.1節(jié)中所述。

表3 相機(jī)位姿估計(jì)算法時(shí)耗對(duì)比Tab.3 Time consumption comparison of camera pose estimation algorithms （s）

6 結(jié)論

本文通過單目相機(jī)采集的RGB數(shù)據(jù)及先驗(yàn)的三維點(diǎn)云信息，實(shí)現(xiàn)相機(jī)位姿估計(jì)。首先根據(jù)深度圖像及SFM算法，構(gòu)建稠密的相機(jī)位姿估計(jì)數(shù)據(jù)集。然后將深度檢索算法與基于結(jié)構(gòu)的算法相結(jié)合，提出多階段相機(jī)位姿估計(jì)方法。最后，采用全卷積神經(jīng)網(wǎng)絡(luò)回歸圖像的稠密場(chǎng)景信息，提出基于稠密場(chǎng)景回歸的多階段相機(jī)位姿估計(jì)算法。

實(shí)驗(yàn)結(jié)果證明：

1.在自建數(shù)據(jù)集Labcore下，多階段相機(jī)位姿估計(jì)方法位姿估計(jì)中值誤差為平移誤差1.5 cm、旋轉(zhuǎn)誤差0.70°，且位姿估計(jì)準(zhǔn)確率為87.3%（誤差閾值為5 cm/5°時(shí)）；基于稠密場(chǎng)景回歸的多階段相機(jī)位姿估計(jì)算法位姿估計(jì)中值誤差為平移誤差0.3 cm、旋轉(zhuǎn)誤差0.60°，位姿估計(jì)準(zhǔn)確率為94.8%（誤差閾值為5 cm/5°時(shí)）。

2.在公開數(shù)據(jù)集7Scenes下，多階段相機(jī)位姿估計(jì)的中值誤差受場(chǎng)景變化影響較大，引入稠密場(chǎng)景回歸后，位姿估計(jì)的精度大幅度提高，7個(gè)公開場(chǎng)景下的中值誤差均小于5 cm/5°，同時(shí)平均位姿估計(jì)準(zhǔn)確率為82.7%（誤差閾值為5 cm/5°時(shí)）。相比于目前的相機(jī)位姿估計(jì)算法，本文算法在位姿估計(jì)環(huán)節(jié)耗時(shí)更短；在位姿估計(jì)精度上，不論從位姿估計(jì)準(zhǔn)確率還是中值誤差角度進(jìn)行評(píng)價(jià)，本文所提算法均有顯著的性能提升。由于本文在圖像檢索以及場(chǎng)景回歸階段均采用了卷積神經(jīng)網(wǎng)絡(luò)模型，因此在位姿估計(jì)的總時(shí)耗上相對(duì)較長(zhǎng)。后續(xù)工作將進(jìn)一步考慮實(shí)際應(yīng)用的具體需求，通過簡(jiǎn)化網(wǎng)絡(luò)模型的復(fù)雜度，提升本文位姿估計(jì)方法的效率。

從表3可以看出，本文在單幀場(chǎng)景圖像位姿估計(jì)的計(jì)算階段耗時(shí)為0.18 s，相比于其他3種方法來說耗時(shí)較短；同時(shí)，相比其余存在圖像檢索的相機(jī)位姿估計(jì)算法，本文方法相機(jī)位姿估計(jì)的總耗時(shí)最短。然而相比于文獻(xiàn)［15］中的DSAC++，由于本文增加了圖像檢索階段，因此相機(jī)位姿估計(jì)總耗時(shí)更長(zhǎng)。本文方法耗時(shí)主要源于在圖像檢索以及位姿估計(jì)階段均使用了卷積神經(jīng)網(wǎng)絡(luò)，復(fù)雜的網(wǎng)絡(luò)結(jié)構(gòu)造成了大量耗時(shí)。因此在后續(xù)工作中，綜合實(shí)際應(yīng)用場(chǎng)景的精度及時(shí)耗需求，通過簡(jiǎn)化圖像檢索以及場(chǎng)景回歸階段的網(wǎng)絡(luò)模型，可提升本文位姿估計(jì)方法的效率。