移動(dòng)機(jī)器人視覺(jué)里程計(jì)綜述

丁文東　徐德　劉希龍　張大朋　陳天

移動(dòng)機(jī)器人想要完成自主導(dǎo)航[1],首先要確定自身的位置和姿態(tài),即實(shí)現(xiàn)定位.一方面,一些移動(dòng)機(jī)器人尤其是空中機(jī)器人[2]的穩(wěn)定運(yùn)行需要位姿信息作為反饋,以形成閉環(huán)控制系統(tǒng).另一方面,隨著移動(dòng)機(jī)器人的快速發(fā)展,移動(dòng)機(jī)器人需要完成的任務(wù)多種多樣,例如物體抓取[3]、空間探索[4]、農(nóng)業(yè)植保[5]、搜索救援[6]等,這些任務(wù)對(duì)移動(dòng)機(jī)器人的定位提出了更高要求.

常用的定位方法有全球定位系統(tǒng)(Global position system,GPS)、基于慣性導(dǎo)航系統(tǒng)(Inertia navigation system,INS)的定位、激光雷達(dá)定位、基于人工標(biāo)志[7?8]的定位方法、視覺(jué)里程計(jì)(Visual odometry,VO)定位[9]等.GPS定位裝置接收多顆衛(wèi)星的信號(hào),可解算出機(jī)器人的三維位置和速度.定位精度在米量級(jí),誤差不隨時(shí)間積累,但GPS信號(hào)被遮擋的地方無(wú)法使用.基于INS的定位利用加速度計(jì)和陀螺儀經(jīng)過(guò)積分計(jì)算出機(jī)器人的位置、速度、姿態(tài)等,數(shù)據(jù)更新率高、短期精度和穩(wěn)定性較好,但定位誤差會(huì)隨時(shí)間積累.激光雷達(dá)通過(guò)掃描獲得機(jī)器人周?chē)h(huán)境的三維點(diǎn)云數(shù)據(jù),根據(jù)這些數(shù)據(jù)實(shí)現(xiàn)機(jī)器人相對(duì)于環(huán)境的定位,精度高,實(shí)時(shí)性強(qiáng),但成本較高.基于人工標(biāo)志定位的方法利用二維碼等作為路標(biāo)實(shí)現(xiàn)機(jī)器人的定位,二維碼需要安裝于環(huán)境中,可以簡(jiǎn)單有效地完成定位,但是一定程度上限制了這些定位方法的使用范圍.視覺(jué)里程計(jì)[9?10]通過(guò)跟蹤序列圖像幀間的特征點(diǎn)估計(jì)相機(jī)的運(yùn)動(dòng),并對(duì)環(huán)境進(jìn)行重建.與輪式里程計(jì)類(lèi)似,視覺(jué)里程計(jì)通過(guò)累計(jì)幀間的運(yùn)動(dòng)估計(jì)當(dāng)前時(shí)刻的位姿.VO在系統(tǒng)運(yùn)行中形成三維點(diǎn)云,作為路標(biāo)點(diǎn)保存在系統(tǒng)中.在新的視角下,基于這些點(diǎn)可利用PnP(Perspectivenpoints)[11]方法進(jìn)行定位.視覺(jué)里程計(jì)具有廣泛的用途,可應(yīng)用于無(wú)人車(chē)[12]、無(wú)人機(jī)[13?15]、增強(qiáng)現(xiàn)實(shí)[16]等.

本文針對(duì)VO展開(kāi)討論,組織結(jié)構(gòu)如下:第1節(jié)簡(jiǎn)要介紹定位問(wèn)題的數(shù)學(xué)描述.第2節(jié)論述主流的視覺(jué)定位方法,重點(diǎn)介紹三類(lèi)視覺(jué)里程計(jì)的原理與特點(diǎn).第3節(jié)討論在傳感器建模和視覺(jué)里程計(jì)前端后端等方面的魯棒性設(shè)計(jì)技巧.第4節(jié)介紹結(jié)合視覺(jué)語(yǔ)義分析的位姿估計(jì)方法和深度學(xué)習(xí)網(wǎng)絡(luò)在位姿估計(jì)中的應(yīng)用.第5節(jié)介紹位姿估計(jì)的性能評(píng)價(jià)方法,常用的數(shù)據(jù)集和常用的工具庫(kù).第6節(jié)給出視覺(jué)定位目前存在的問(wèn)題和未來(lái)的發(fā)展方向.

1　定位問(wèn)題數(shù)學(xué)描述

機(jī)器人k時(shí)刻的位姿為其中,Tk,k+1為k～k+1時(shí)刻機(jī)器人的相對(duì)位姿,初始狀態(tài)下機(jī)器人的位姿為T(mén)0.

使用式(1)遞推獲得當(dāng)前位姿.因此,該過(guò)程中不可避免地會(huì)出現(xiàn)誤差,且該誤差具有累積現(xiàn)象.為消除累積誤差,需要基于觀測(cè)值進(jìn)行濾波或BA(Bundle adjustment)優(yōu)化.

為了保證系統(tǒng)的實(shí)時(shí)性,視覺(jué)定位通常分為兩部分:1)基于特征匹配的運(yùn)動(dòng)估計(jì);2)對(duì)定位結(jié)果進(jìn)行優(yōu)化.特征匹配針對(duì)位姿變化前后的圖像獲取對(duì)應(yīng)特征點(diǎn)對(duì),利用n(n≥3)個(gè)匹配點(diǎn)對(duì)以及相機(jī)內(nèi)參數(shù)得到相機(jī)的運(yùn)動(dòng)量.當(dāng)相機(jī)運(yùn)動(dòng)距離較大,或能夠跟蹤到的點(diǎn)較少時(shí),則把這一幀圖像作為關(guān)鍵幀保存下來(lái).優(yōu)化部分利用特征點(diǎn)的重投影偏差最小化對(duì)關(guān)鍵幀對(duì)應(yīng)的相機(jī)位姿及特征點(diǎn)在相機(jī)坐標(biāo)系中的位置進(jìn)行估計(jì).第i關(guān)鍵幀對(duì)應(yīng)的投影矩陣為

其中,K表示相機(jī)的內(nèi)參數(shù)矩陣.

運(yùn)動(dòng)估計(jì)和優(yōu)化均可采用其中,Mj為路標(biāo)點(diǎn),mij為Mj在第i幀中的圖像坐標(biāo),e表示誤差,wij表示路標(biāo)點(diǎn)Mj在第i幀中權(quán)值.如果點(diǎn)j在第i幀中可見(jiàn),則wij=1,否則為0.

運(yùn)動(dòng)估計(jì)部分利用式(3)獲得相機(jī)的位姿[R,p],優(yōu)化部分則對(duì)位姿[R,ppp]和路標(biāo)點(diǎn)Mj同時(shí)進(jìn)行優(yōu)化.

在求解的過(guò)程中,對(duì)該系統(tǒng)線性化,然后可以用高斯–牛頓或LM(Levenberg-Marquardt)方法迭代求解.由于點(diǎn)與點(diǎn)之間、位姿與位姿之間相對(duì)誤差項(xiàng)是獨(dú)立的,相應(yīng)矩陣具有稀疏性,式(3)可以實(shí)時(shí)求解.

上述問(wèn)題也可以建模為因子圖(Factor graph)并使用圖優(yōu)化方法求解[17?18].圖模型[19?20]可直觀地表示視覺(jué)定位問(wèn)題,圖中的狀態(tài)節(jié)點(diǎn)表示機(jī)器人的位姿或路標(biāo),節(jié)點(diǎn)之間的邊對(duì)應(yīng)狀態(tài)之間的幾何約束.圖模型構(gòu)建之后,經(jīng)過(guò)優(yōu)化可得到與測(cè)量數(shù)據(jù)最匹配的狀態(tài)參數(shù),進(jìn)而形成路標(biāo)點(diǎn)地圖.一個(gè)常用圖網(wǎng)絡(luò)優(yōu)化工具為g2o(General graph optimization)[21],詳見(jiàn)第5.2節(jié).

2　VO代表性方法

VO系統(tǒng)中的數(shù)據(jù)關(guān)聯(lián)表示了3D點(diǎn)在不同幀之間的關(guān)系.在運(yùn)動(dòng)估計(jì)中,使用當(dāng)前幀圖像和過(guò)往幀圖像進(jìn)行數(shù)據(jù)關(guān)聯(lián)求解相機(jī)運(yùn)動(dòng)量,通過(guò)遞推每一步的運(yùn)動(dòng)量可以得到相機(jī)和機(jī)器人的位姿.數(shù)據(jù)關(guān)聯(lián)中的點(diǎn)所在空間有三種[10]:

1)2D?2D:當(dāng)前幀的點(diǎn)和過(guò)往幀的點(diǎn)都是在圖像空間中.在單目相機(jī)的初始化過(guò)程中經(jīng)常出現(xiàn)這種數(shù)據(jù)關(guān)聯(lián).

2)3D?3D:當(dāng)前幀和過(guò)往幀的點(diǎn)都在3D空間中,這種情形一般在深度相機(jī)VO系統(tǒng)的位姿估計(jì)或經(jīng)過(guò)三角測(cè)量的點(diǎn)進(jìn)行BA時(shí)出現(xiàn).

3)3D?2D:過(guò)往幀的點(diǎn)在3D空間中,當(dāng)前幀的點(diǎn)在圖像空間中,這樣問(wèn)題轉(zhuǎn)化為一個(gè)PnP問(wèn)題.

在VO系統(tǒng)初始化時(shí),地圖未建立,系統(tǒng)無(wú)法確定當(dāng)前狀態(tài),采用2D?2D數(shù)據(jù)關(guān)聯(lián),對(duì)基礎(chǔ)矩陣或單應(yīng)矩陣分解求解相機(jī)的相對(duì)位姿,三角化求解路標(biāo)點(diǎn)的三維坐標(biāo).若地圖中3D點(diǎn)可用,優(yōu)先使用3D點(diǎn)進(jìn)行位姿估計(jì).此時(shí),將3D路標(biāo)點(diǎn)投影到當(dāng)前幀圖像,在局部范圍內(nèi)搜索完成圖像點(diǎn)的匹配.這種3D?2D的數(shù)據(jù)關(guān)聯(lián)經(jīng)常用于VO系統(tǒng)正常狀態(tài)下的定位.3D?3D數(shù)據(jù)關(guān)聯(lián)常用于估計(jì)和修正累積誤差和漂移.3D路標(biāo)點(diǎn)會(huì)出現(xiàn)在多幀圖像中,通過(guò)這些3D點(diǎn)之間的數(shù)據(jù)關(guān)聯(lián)可以修正相機(jī)的運(yùn)動(dòng)軌跡以及3D點(diǎn)的三維位置.

例如,SVO(Semi-direct visual odometry)[22]中除了初始化過(guò)程,正常狀態(tài)下系統(tǒng)處理當(dāng)前的每一幀時(shí)三種數(shù)據(jù)關(guān)聯(lián)先后被使用,2D?2D數(shù)據(jù)關(guān)聯(lián)實(shí)現(xiàn)圖像空間的特征點(diǎn)匹配,通過(guò)3D?2D數(shù)據(jù)關(guān)聯(lián)計(jì)算相機(jī)的位姿,并經(jīng)過(guò)3D?3D數(shù)據(jù)關(guān)聯(lián)后利用BA進(jìn)行優(yōu)化.DTAM(Dense tracking and mapping)[23]的目標(biāo)函數(shù)中包含了多種數(shù)據(jù)關(guān)聯(lián)的誤差,包括圖像空間的匹配誤差和3D空間的位置誤差.當(dāng)幀間運(yùn)動(dòng)較小,成功匹配的3D點(diǎn)較多時(shí),估計(jì)位姿矩陣;當(dāng)幀間運(yùn)動(dòng)較大,匹配2D點(diǎn)較多時(shí),估計(jì)基礎(chǔ)矩陣.按照2D?2D數(shù)據(jù)關(guān)聯(lián)方式的不同,視覺(jué)定位方法可以分為直接法、非直接法和混合法.

2.1　直接法

作為數(shù)據(jù)關(guān)聯(lián)方式的一種,直接法假設(shè)幀間光度值具有不變性,即相機(jī)運(yùn)動(dòng)前后特征點(diǎn)的灰度值是相同的.數(shù)據(jù)關(guān)聯(lián)時(shí),根據(jù)灰度值對(duì)特征點(diǎn)進(jìn)行匹配.但這種假設(shè)與實(shí)際情況存在差異,特征點(diǎn)容易出現(xiàn)誤匹配.Engel等[24?25]使用了一種更精確的光度值模型,該模型對(duì)相機(jī)成像過(guò)程建模了相機(jī)曝光參數(shù)、Gamma矯正以及鏡頭衰減.該模型使用輻照度不變性假設(shè),可以表示為Ii(mmm)=G(tiV(mmm)B(mmm)),其中像素點(diǎn)mmm的輻照度為B,鏡頭的衰減為V,曝光時(shí)間為ti,CCD(Charge coupled device)的響應(yīng)函數(shù)為G.對(duì)該模型進(jìn)行逆向求解得到校正后的圖像灰度值,進(jìn)行數(shù)據(jù)關(guān)聯(lián).

為了快速求解上述問(wèn)題,Lucas等[26]引入FAIA(Forward additional image alignment)方法,使用單一運(yùn)動(dòng)模型代替獨(dú)立像素位移差.Baker等[27]提出統(tǒng)一的框架,在FAIA基礎(chǔ)上引入FCIA(Forward composition image alignment),ICIA(Inverse compositional image alignment)和IAIA(Inverse additional image alignment)[27].SVO和(Parallel tracking and mapping)[28]利用ICIA實(shí)現(xiàn)塊匹配,DPPTAM(Dense piecewise planar tracking and mapping)[29]利用ICIA完成顯著梯度點(diǎn)的半稠密重建.

LSD(Large scale direct)SLAM(Simultaneous localization and mapping)[30?31]采用直接方法進(jìn)行數(shù)據(jù)關(guān)聯(lián),建立深度估計(jì)、跟蹤和建圖三個(gè)線程.該方法對(duì)圖像點(diǎn)建立隨機(jī)深度圖,并在后續(xù)幀中對(duì)深度進(jìn)行調(diào)整直至收斂.該方法的初始化不需要兩視幾何約束,不會(huì)陷入兩視幾何退化的困境,但初始化過(guò)程需要多個(gè)關(guān)鍵幀之后深度圖才會(huì)收斂,此期間跟蹤器產(chǎn)生的地圖是不可靠的.LSD SLAM通過(guò)權(quán)值高斯–牛頓迭代方法最小化光度值誤差.光度值誤差是當(dāng)前幀和參考關(guān)鍵幀之間所有對(duì)應(yīng)點(diǎn)的灰度值差的平方和.LSD SLAM建圖對(duì)關(guān)鍵幀及非關(guān)鍵幀分開(kāi)處理,對(duì)于前者,過(guò)往關(guān)鍵幀的深度圖投影到當(dāng)前關(guān)鍵幀,并作為深度圖的初始值;對(duì)于后者,則進(jìn)行圖像匹配并計(jì)算位姿,對(duì)當(dāng)前幀更新深度信息,對(duì)深度信息進(jìn)行平滑并移除外點(diǎn).

DSO(Direct sparse odometry)[24]系統(tǒng)基于直接法的拓展,使用光度值誤差最小化幾何和光度學(xué)參數(shù).DSO對(duì)圖像中有梯度、邊緣或亮度平滑變化的點(diǎn)均勻采樣以降低計(jì)算量.DSO對(duì)光度學(xué)模型校正、曝光時(shí)間、透鏡畸光暈和非線性響應(yīng)都做了校準(zhǔn).為了提高速度,降低計(jì)算量,DSO使用滑動(dòng)窗口方法,對(duì)固定幀數(shù)的位姿進(jìn)行優(yōu)化.

DPPTAM[29]基于超像素對(duì)平面場(chǎng)景進(jìn)行稠密重建.該方法對(duì)圖像中梯度明顯的點(diǎn)進(jìn)行半稠密重建,然后對(duì)圖像中其他點(diǎn)進(jìn)行超像素分割,通過(guò)最小化能量函數(shù)完成稠密重建,該能量函數(shù)在第3.3.2節(jié)中介紹.

直接法使用了簡(jiǎn)單的成像模型,適用于幀間運(yùn)動(dòng)較小的情形,但在場(chǎng)景的照明發(fā)生變化時(shí)容易失敗.

2.2　非直接法

另外一種幀間數(shù)據(jù)關(guān)聯(lián)是非直接法,又稱(chēng)為特征法,該方法提取圖像中的特征進(jìn)行匹配,最小化重投影誤差得到位姿.圖像中的特征點(diǎn)以及對(duì)應(yīng)描述子用于數(shù)據(jù)關(guān)聯(lián),通過(guò)特征描述子的匹配,完成初始化中2D?2D以及之后的3D?2D的數(shù)據(jù)關(guān)聯(lián).常用的旋轉(zhuǎn)、平移、尺度等不變性特征及描述子,例如ORB(Oriented FAST and rotated BRIEF)[32]、FAST(Features from accelerated segment test)[33]、BRISK(Binary robust invariant scalable keypoints)[34]、SURF(Speeded up robust features)[35],可用于完成幀間點(diǎn)匹配.

PTAM[28]是一個(gè)基于關(guān)鍵幀的SLAM 系統(tǒng),是很多性能良好的SLAM系統(tǒng)的原型,PTAM首先引入了跟蹤和建圖分線程處理的方法.原始的版本經(jīng)過(guò)修改之后增加了邊緣特征、旋轉(zhuǎn)估計(jì)和更好的重定位方法.PTAM 的地圖點(diǎn)對(duì)應(yīng)圖像中的FAST角點(diǎn),FAST特征計(jì)算速度很快,但沒(méi)有形成特征描述子,因此使用塊相關(guān)完成匹配.

ORB特征[32]是一種快速的特征提取方法,具有旋轉(zhuǎn)不變性,并可以利用金字塔構(gòu)建出尺度不變性.在整個(gè)定位過(guò)程以及建圖的過(guò)程中,ORB SLAM[36]使用了統(tǒng)一的ORB特征,在跟蹤的時(shí)候提取ORB特征,完成點(diǎn)的匹配、跟蹤、三角測(cè)量和閉環(huán)檢測(cè)等關(guān)鍵過(guò)程.

DT(Deferred triangulation)SLAM[37]在地圖中的路標(biāo)點(diǎn)不僅使用三維點(diǎn),而且使用二維圖像特征點(diǎn).在位姿估計(jì)中,目標(biāo)函數(shù)中包括三維點(diǎn)的重建誤差以及二維特征重投影誤差.DT SLAM維護(hù)了三個(gè)跟蹤器,每個(gè)跟蹤器包含一種位姿估計(jì)方法:位姿估計(jì)、本質(zhì)矩陣估計(jì)和純旋轉(zhuǎn)估計(jì).當(dāng)足夠數(shù)量的3D點(diǎn)匹配存在時(shí)候,可以使用位姿估計(jì);當(dāng)3D點(diǎn)數(shù)量不足,但是2D點(diǎn)數(shù)量較多的時(shí)候可以利用對(duì)極約束估計(jì)本質(zhì)矩陣.如果判定當(dāng)前情況為純旋轉(zhuǎn),那么使用純旋轉(zhuǎn)估計(jì).

當(dāng)圖像中沒(méi)有足夠的點(diǎn)特征時(shí),線特征是一個(gè)好的補(bǔ)充[38?39].通常使用的線段檢測(cè)器有比較高的精度,但是很耗時(shí)間.Gomez-Ojeda等[40]對(duì)每條線段計(jì)算LBD(Line band descriptor)描述子[41],最小化點(diǎn)特征以及線段特征的重投影誤差得到運(yùn)動(dòng)估計(jì).Zhou等[42?43]使用消失點(diǎn)定義圖像中的線結(jié)構(gòu),使用J-linkage[44]將所得線段分類(lèi),計(jì)算消失點(diǎn)的粗略值,然后通過(guò)非線性最小二乘優(yōu)化得到消失點(diǎn)在圖像中的表示以及相機(jī)的方向.

Camposeco等[45]使用消失點(diǎn)來(lái)提高VO系統(tǒng)的精度,首先使用線段檢測(cè)器檢測(cè)圖像中的線段,然后使用最小二乘法計(jì)算消失點(diǎn),將EKF(Extended Kalman fi lter)中的誤差狀態(tài)向量(核心狀態(tài))中增加消失點(diǎn)作為增廣狀態(tài),在更新EKF核心狀態(tài)時(shí)同時(shí)更新增廣狀態(tài)方程.Gr¨ater等[46]使用消失點(diǎn)提高單目VO系統(tǒng)的尺度計(jì)算的魯棒性和精度.但是由于計(jì)算實(shí)際的尺度值時(shí)使用了相機(jī)到地面的高度作為先驗(yàn)知識(shí),該方法僅限于平面運(yùn)動(dòng)機(jī)器人.

直接法和非直接法的優(yōu)缺點(diǎn)對(duì)比詳見(jiàn)表1.

2.3　混合法

SVO[22]是一種混合式的VO,該方法首先提取FAST特征,使用特征點(diǎn)周?chē)膱D像塊進(jìn)行像素匹配,并對(duì)幀間的相對(duì)位姿累積以初步估計(jì)當(dāng)前位姿,累積誤差會(huì)導(dǎo)致系統(tǒng)產(chǎn)生漂移.SVO通過(guò)匹配當(dāng)前幀與地圖中的點(diǎn)約束當(dāng)前幀的位姿,降低累積誤差.SVO初始化時(shí)使用單應(yīng)矩陣分解求解相機(jī)的位姿,假設(shè)初始化場(chǎng)景中的點(diǎn)分布在一個(gè)平面內(nèi),因此適合平面場(chǎng)景的初始化.

3　魯棒性改進(jìn)措施

VO系統(tǒng)在實(shí)際應(yīng)用中的主要問(wèn)題是魯棒性不足,限制條件過(guò)多.本文從傳感器的特性建模、系統(tǒng)的前端、后端等方面,包括卷簾快門(mén)相機(jī)建模、系統(tǒng)初始化、運(yùn)動(dòng)模型假設(shè)、目標(biāo)函數(shù)、深度圖模型,介紹增強(qiáng)魯棒性的方法.

3.1　視覺(jué)傳感器建模

很多現(xiàn)代的相機(jī)使用CMOS(Complementary metal oxide semiconductor)圖像傳感器,成本較低,但使用卷簾快門(mén)時(shí),圖像中每一行像素曝光時(shí)間窗口不一樣.假設(shè)快門(mén)啟動(dòng)的時(shí)間為t0,圖像第i行的成像時(shí)刻為ti,假設(shè)圖像有Nr行,傳感器數(shù)據(jù)讀出的時(shí)間為ts.因此ti=t0+tsi/Nr.根據(jù)Karpenko等[47?48]的分析可知,在快門(mén)轉(zhuǎn)動(dòng)的時(shí)間段內(nèi),平移運(yùn)動(dòng)的影響對(duì)于相機(jī)模型的影響較小,可以忽略.假設(shè)在快門(mén)開(kāi)啟時(shí),存在三維點(diǎn)MMM,該點(diǎn)的成像時(shí)刻為ti,對(duì)應(yīng)圖像空間中的點(diǎn)為mmmi.因此有

其中,R0,i為t0到ti時(shí)刻的旋轉(zhuǎn)矩陣,K為內(nèi)參數(shù),λi為常數(shù).Kerl等[49]針對(duì)RGBD(Red-greenblue depth)圖像使用B樣條近似相機(jī)運(yùn)動(dòng)軌跡,補(bǔ)償卷簾快門(mén)的影響.系統(tǒng)使用了深度值誤差以及光度值誤差優(yōu)化計(jì)算相機(jī)的運(yùn)動(dòng),得到平滑連續(xù)的軌跡.Pertile等[50]使用IMU(Inertial measurement unit)來(lái)計(jì)算R0,i,也就是從快門(mén)開(kāi)啟t0到時(shí)刻ti相機(jī)運(yùn)動(dòng)的旋轉(zhuǎn)矩陣.另外,Kim等[51]定義了行位姿,相機(jī)的位姿依賴(lài)于圖像行變量.將滑動(dòng)幀窗口方法擴(kuò)展為近鄰窗口,該窗口包含固定個(gè)數(shù)的B樣條控制點(diǎn).該系統(tǒng)使用IMU對(duì)相機(jī)在快門(mén)動(dòng)作期間內(nèi)估計(jì)相機(jī)的運(yùn)動(dòng),但是由于CMOS的快門(mén)時(shí)間戳和IMU的時(shí)間戳的同步比較困難,且相機(jī)的時(shí)間戳不太準(zhǔn)確,Guo等[52]對(duì)時(shí)間戳不精確的卷簾快門(mén)相機(jī)設(shè)計(jì)了一種VIO(Visual inertial odometry)系統(tǒng),其位姿使用線性插值方法近似相機(jī)的運(yùn)動(dòng)軌跡,姿態(tài)使用旋轉(zhuǎn)角度和旋轉(zhuǎn)軸表示,旋轉(zhuǎn)軸不變,對(duì)旋轉(zhuǎn)角度線性插值,使用MSCKF(Multi-state constrained Kalman fi lter)建模卷簾快門(mén)相機(jī)的測(cè)量模型.

Dai等[53]對(duì)線性卷簾快門(mén)模型和均勻卷簾快門(mén)模型的相機(jī)計(jì)算了雙視幾何的本質(zhì)矩陣.線性卷簾快門(mén)模型中,假設(shè)相機(jī)的運(yùn)動(dòng)為勻速直線運(yùn)動(dòng),均勻卷簾模型中,相機(jī)的運(yùn)動(dòng)為一個(gè)勻角速度運(yùn)動(dòng)和一個(gè)勻速直線運(yùn)動(dòng).在全局快門(mén)相機(jī)中,本質(zhì)矩陣是一個(gè)3×3的奇異矩陣.在使用線性卷簾模型的相機(jī)下,本質(zhì)矩陣為一個(gè)5×5的矩陣,在使用均勻卷簾模型的相機(jī)下,本質(zhì)矩陣為一個(gè)7×7的矩陣.因此,在使用卷簾模型時(shí),5點(diǎn)法無(wú)法求解本質(zhì)矩陣.線性卷簾模型和均勻卷簾模型分別需要11和17個(gè)點(diǎn)求解本質(zhì)矩陣.

表1　直接法與非直接法優(yōu)缺點(diǎn)對(duì)比Table 1　The comparison between direct methods and indirect methods

3.2　視覺(jué)里程計(jì)前端

3.2.1　初始化

單目系統(tǒng)初始化時(shí)完成運(yùn)動(dòng)估計(jì)常用的方法主要有兩種:1)將當(dāng)前場(chǎng)景視為一個(gè)平面場(chǎng)景[54],估計(jì)單應(yīng)矩陣并分解得到運(yùn)動(dòng)估計(jì),使用這種方法的有SVO、PTAM等.2)使用極線約束關(guān)系,估計(jì)基礎(chǔ)矩陣或者本質(zhì)矩陣[55?56],分解得到運(yùn)動(dòng)估計(jì),使用這種方法的有DT SLAM等.初始化中遇到的普遍問(wèn)題是雙視幾何中的退化問(wèn)題.當(dāng)特征共面或相機(jī)發(fā)生純旋轉(zhuǎn)的時(shí)候,解出的基礎(chǔ)矩陣的自由度下降,如果繼續(xù)求解基礎(chǔ)矩陣,那么多出來(lái)的自由度主要由噪聲決定.為了避免退化現(xiàn)象造成的影響,一些VO系統(tǒng)同時(shí)估計(jì)基礎(chǔ)矩陣和單應(yīng)矩陣,例如ORB SLAM和DPPTAM,使用一個(gè)懲罰函數(shù),判斷當(dāng)前的情形,選擇重投影誤差比較小的一方作為運(yùn)動(dòng)估計(jì)結(jié)果.

單目系統(tǒng)在初始化中還要完成像素點(diǎn)的深度估計(jì),單目系統(tǒng)無(wú)法直接從單張圖像中恢復(fù)深度,因此需要一個(gè)初始估計(jì).解決該問(wèn)題的一種辦法是跟蹤一個(gè)已知的結(jié)構(gòu)[57],另外一種方法是初始化點(diǎn)為具有較大誤差的逆深度[30?31],在之后過(guò)程中優(yōu)化直到收斂至真值.

VO系統(tǒng)的初始化依賴(lài)于精確的相機(jī)標(biāo)定和狀態(tài)初始值.對(duì)于系統(tǒng)的初始化,Shen等[18?19]在系統(tǒng)的運(yùn)動(dòng)中,建立相鄰兩幀圖像間的關(guān)系,對(duì)從上一幀慣性坐標(biāo)系至當(dāng)前幀相機(jī)坐標(biāo)系進(jìn)行變換.

根據(jù)相機(jī)和IMU多次運(yùn)動(dòng)分別獲得的IMU測(cè)量的變換矩陣bTk,k+1及相機(jī)測(cè)量的變換矩陣cTk,k+1,可以標(biāo)定相機(jī)和IMU之間的變換矩陣bTc.

3.2.2　運(yùn)動(dòng)模型

機(jī)器人的導(dǎo)航中,實(shí)際的運(yùn)動(dòng)經(jīng)常不符合恒速運(yùn)動(dòng)模型假設(shè),需設(shè)計(jì)應(yīng)對(duì)失敗的策略.ORB SLAM的運(yùn)動(dòng)估計(jì)通過(guò)跟蹤若干匹配的特征點(diǎn)來(lái)檢測(cè)這種失敗,這種情況下可跟蹤的點(diǎn)的數(shù)量較少.因此ORB SLAM設(shè)置一定閾值,如果能夠跟蹤的點(diǎn)的個(gè)數(shù)小于該閾值,則會(huì)在一個(gè)更大的范圍內(nèi)進(jìn)行特征的搜索匹配.DSO系統(tǒng)中如果恒速模型失敗,會(huì)使用27種不同方向不同大小的旋轉(zhuǎn)來(lái)嘗試恢復(fù).這些嘗試在較高的金字塔層上完成,所以耗時(shí)很短.SVO等方法假設(shè)當(dāng)前時(shí)刻的位姿等于上一時(shí)刻的位姿,通過(guò)最小化光度值誤差估計(jì)幀間的位姿變化,使用高斯–牛頓方法完成ICIA的迭代.ICIA的使用也限制了幀間視差的最大值,或需要較高的幀率(典型的大于70fps).表2給出幾種常用運(yùn)動(dòng)模型在VO系統(tǒng)使用的情形.

表2　常用運(yùn)動(dòng)模型先驗(yàn)假設(shè)Table 2　The common used motion model assumption

3.3　視覺(jué)里程計(jì)后端

3.3.1　目標(biāo)函數(shù)

上文討論了直接法以及間接法中使用的目標(biāo)函數(shù),目標(biāo)函數(shù)的設(shè)計(jì)影響了VO系統(tǒng)魯棒性.在最大后驗(yàn)估計(jì)的定位問(wèn)題中,似然函數(shù)中如果假設(shè)噪聲的分布為高斯分布,那么目標(biāo)函數(shù)中負(fù)對(duì)數(shù)似然函數(shù)等價(jià)于?2范數(shù).如果假設(shè)噪聲的分布為拉普拉斯分布,負(fù)對(duì)數(shù)似然函數(shù)對(duì)應(yīng)?1范數(shù).在優(yōu)化中,?2范數(shù)對(duì)噪聲敏感,噪聲的存在導(dǎo)致估計(jì)的結(jié)果與實(shí)際參數(shù)相差較大,因而改用M估計(jì)器替換平方殘差函數(shù)ρ(ri).表3給出幾種常用魯棒估計(jì)器的具體表達(dá)式.

¨Ozyesil等[58]使用?1和?2兩種范數(shù)結(jié)合的一種范數(shù)IRLS(Iteratively reweighted least squares)[59],通過(guò)迭代的方式解決帶權(quán)重的?p范數(shù)(參見(jiàn)表3)的優(yōu)化問(wèn)題.VO系統(tǒng)常用的魯棒目標(biāo)函數(shù)如表4所示.在恢復(fù)相機(jī)的運(yùn)動(dòng)中,相機(jī)的位置估計(jì)容易被噪聲干擾,方向的估計(jì)在精度和魯棒性方面則相對(duì)比較準(zhǔn)確.¨Ozyesil等[58]引入兩步估計(jì)方法,首先估計(jì)點(diǎn)對(duì)的相對(duì)方向,然后從點(diǎn)對(duì)的相對(duì)方向中恢復(fù)每個(gè)點(diǎn)的3D位置.位置估計(jì)的目標(biāo)函數(shù)形式化為最小化方向的誤差,其中位置表示為方向和距離的乘積,因?yàn)榉较蛞阎?因此優(yōu)化對(duì)象變?yōu)榫嚯x,使用IRLS方法迭代優(yōu)化目標(biāo)值.S¨underhauf等[60]使用可切換約束的目標(biāo)函數(shù),在優(yōu)化中識(shí)別并丟棄外點(diǎn).另外該系統(tǒng)利用可切換的閉環(huán)檢測(cè)約束以及可切換的先驗(yàn)約束,避免對(duì)閉環(huán)檢測(cè)的誤報(bào).

3.3.2　深度圖

在基于直接法的VO系統(tǒng)(DSO、LSD SLAM)中,常常需要估計(jì)點(diǎn)的深度,原始的深度并不表現(xiàn)為類(lèi)高斯分布,而是帶有長(zhǎng)拖尾.在室外應(yīng)用中,存在很多無(wú)窮遠(yuǎn)點(diǎn),初始值難以設(shè)定,因此使用高斯分布描述不準(zhǔn)確.逆深度(原始深度的倒數(shù))的分布更加接近高斯分布,具備更好的數(shù)值穩(wěn)定性.常用的深度圖模型如表5所示.

表3　常用的魯棒估計(jì)器Table 3　The common used robust estimators

表4　VO系統(tǒng)中的魯棒目標(biāo)函數(shù)設(shè)計(jì)Table 4　The common used robust objection function in VO systems

像素點(diǎn)的深度估計(jì)方法有濾波器方法和非線性?xún)?yōu)化方法.其中SVO、DSO將深度建模為一個(gè)類(lèi)高斯模型,然后使用濾波器估計(jì).另外一種方法對(duì)深度圖構(gòu)建一個(gè)能量函數(shù),例如LSD SLAM、DTAM、DPPTAM 等,然后使用非線性?xún)?yōu)化方法最小化能量函數(shù).該函數(shù)包括一個(gè)光度值誤差項(xiàng)以及一個(gè)正則項(xiàng),用來(lái)平滑所得結(jié)果.

表5　深度圖模型Table 5　The common used models of depth map

DPPTAM[29]首先對(duì)圖像中梯度明顯的點(diǎn)估計(jì)深度,由此得到半稠密的深度圖.梯度明顯的點(diǎn)占圖像所有點(diǎn)的比例較小,因此要更新的點(diǎn)數(shù)較少,可以實(shí)時(shí)完成位姿估計(jì).另外這些點(diǎn)還用于估計(jì)平面結(jié)構(gòu),其深度圖使用一致性假設(shè),包括三個(gè)方面.

1)極線方向和梯度方向垂直的點(diǎn)的逆深度值是不可靠的.

2)時(shí)間一致性.相鄰若干時(shí)刻同一個(gè)像素點(diǎn)的逆深度是相似的.

3)空間一致性.相鄰像素的逆深度值是相似的.

對(duì)于其他點(diǎn)的深度估計(jì)通過(guò)最小化一個(gè)由光度值誤差、深度距離和梯度正則項(xiàng)組成的能量函數(shù)完成.光度值誤差同直接法中光度值不變性假設(shè).另外兩項(xiàng)為正則項(xiàng),深度距離計(jì)算了被估計(jì)深度距離分段平面的距離.梯度正則計(jì)算了深度圖的梯度,用于平滑深度圖.DTAM[23]中的能量函數(shù)除光度值誤差、梯度正則外,還使用了一個(gè)對(duì)偶項(xiàng),避免了線性化目標(biāo)函數(shù)并迭代優(yōu)化導(dǎo)致的重建結(jié)果損失深度圖細(xì)節(jié),這樣還可以使用原始對(duì)偶方法快速完成優(yōu)化.原始對(duì)偶方法不同于原始方法以及對(duì)偶優(yōu)化方法,基本思想是從對(duì)偶問(wèn)題的一個(gè)可行解開(kāi)始,同時(shí)計(jì)算原問(wèn)題和對(duì)偶問(wèn)題,求出原問(wèn)題滿(mǎn)足松弛條件的可行解,這個(gè)可行解就是最優(yōu)解.

4　語(yǔ)義分析與深度學(xué)習(xí)

上文介紹了改進(jìn)視覺(jué)里程計(jì)魯棒性的措施,視覺(jué)語(yǔ)義分析以及深度學(xué)習(xí)的應(yīng)用同樣對(duì)提高系統(tǒng)的魯棒性具有幫助.本節(jié)圍繞語(yǔ)義分析和深度學(xué)習(xí)方面的相關(guān)問(wèn)題展開(kāi)介紹.

4.1　語(yǔ)義分析

語(yǔ)義分析根據(jù)結(jié)構(gòu)型數(shù)據(jù)的相似特性對(duì)像素(區(qū)域)進(jìn)行標(biāo)記,對(duì)場(chǎng)景中的區(qū)域分類(lèi).粗粒度的語(yǔ)義分析應(yīng)該包括物體檢測(cè)、區(qū)域分割等.語(yǔ)義分析和位姿估計(jì)之間相互影響,可以體現(xiàn)在兩個(gè)方面:1)語(yǔ)義分析能夠提高位姿及建圖的精度[62];2)VO的測(cè)量結(jié)果降低語(yǔ)義分析的難度.

在基于稀疏特征的VO系統(tǒng)中,場(chǎng)景重建為稀疏點(diǎn)云;在稠密的VO系統(tǒng)中,場(chǎng)景重建為連續(xù)的表面;而在含有語(yǔ)義分析的系統(tǒng)中會(huì)建立一個(gè)語(yǔ)義地圖,該地圖中組成元素為物體,而不是度量地圖中的稠密或稀疏的點(diǎn).SLAM++系統(tǒng)[62]中,語(yǔ)義地圖表示為一個(gè)圖網(wǎng)絡(luò),其中節(jié)點(diǎn)有兩種:1)相機(jī)在世界坐標(biāo)系的位姿;2)物體在世界坐標(biāo)系的位姿.物體在相機(jī)坐標(biāo)系的位姿作為網(wǎng)絡(luò)中的一個(gè)約束,連接相機(jī)節(jié)點(diǎn)和物體節(jié)點(diǎn).另外網(wǎng)絡(luò)中還加入了平面結(jié)構(gòu)等約束提高定位的精度.

MO-SLAM(Multi object SLAM)[63]對(duì)于場(chǎng)景中重復(fù)出現(xiàn)的物體進(jìn)行檢測(cè),該方法不需要離線訓(xùn)練以及預(yù)制物體數(shù)據(jù)庫(kù).系統(tǒng)將重建的路標(biāo)點(diǎn)分類(lèi),標(biāo)記該點(diǎn)所屬的物體類(lèi)別.一個(gè)物體表示為一個(gè)路標(biāo)點(diǎn)集合,相同的物體的不同實(shí)例的路標(biāo)點(diǎn)之間存在如下關(guān)系

高層特征具備更好的區(qū)分性,同時(shí)幫助機(jī)器人更好完成數(shù)據(jù)關(guān)聯(lián).DARNN[66]引入數(shù)據(jù)聯(lián)合(Data association,DA)下的RNN(Recurrent neural network),同時(shí)對(duì)RGBD圖像進(jìn)行語(yǔ)義標(biāo)注和場(chǎng)景重建.將RGB圖像和深度圖像分別輸入全卷積網(wǎng)絡(luò),在反卷積層加入數(shù)據(jù)聯(lián)合RNN層,將不同幀圖像的特征進(jìn)行融合,同時(shí)能夠融合RGBD圖像和深度圖像.該文章使用KinectFusion[67]完成相機(jī)的跟蹤,估計(jì)當(dāng)前相機(jī)的6DOF位姿,將3D場(chǎng)景表示為3D體素,保存于TSDF(Truncated signed distance function).McCormac等[68]使用Elastic-Funsion完成SLAM的稠密重建及位姿估計(jì)任務(wù),使用FCN(Fully convolutional network)完成語(yǔ)義分割,不同的種類(lèi)使用面元(Surfel)表示,使用貝葉斯更新器跟蹤分割該面元的概率分布,使用SLAM生成的點(diǎn)匹配更新面元的概率分布.針對(duì)建圖規(guī)模大、稠密重建速度慢和室外環(huán)境建圖困難等問(wèn)題,Vineet等[69]使用基于CRF(Conditional random field)的體積平均場(chǎng)方法進(jìn)行圖像分割,同時(shí)基于KinectFusion方法完成稠密重建.

4.2　深度學(xué)習(xí)方法

人類(lèi)可以不監(jiān)督的完成認(rèn)知任務(wù),通過(guò)在代理任務(wù)(例如本體運(yùn)動(dòng)估計(jì))的監(jiān)督學(xué)習(xí)可以解決其他的任務(wù)(例如深度理解),避免了顯式的監(jiān)督學(xué)習(xí).一些任務(wù)學(xué)習(xí)的泛化能力強(qiáng),可以作為其他任務(wù)的基礎(chǔ).另外深度網(wǎng)絡(luò)的應(yīng)用中,Zamir等[70]提出了一種多任務(wù)學(xué)習(xí)的方法,經(jīng)過(guò)特征匹配任務(wù)訓(xùn)練的網(wǎng)絡(luò)不需要重新調(diào)整參數(shù)就完成相機(jī)位姿的估計(jì),此過(guò)程體現(xiàn)了深度網(wǎng)絡(luò)的抽象能力.該網(wǎng)絡(luò)表現(xiàn)為一種通用的能夠泛化至新的任務(wù)的深度網(wǎng)絡(luò)感知系統(tǒng).

基于深度學(xué)習(xí)的方法要解決的一個(gè)基本問(wèn)題是如何得到訓(xùn)練使用的大規(guī)模數(shù)據(jù)集合,KITTI(Karlsruhe institute of technology and Toyota technological institute)和TUM(Technische Universit¨at M¨unchen)數(shù)據(jù)集中除了圖像序列,還給出了圖像的深度和相機(jī)采集圖像時(shí)的位姿,詳見(jiàn)第5.3節(jié).如果不存在VICON或高精度IMU等數(shù)據(jù)作為真值,只有單純圖像序列的數(shù)據(jù)集,可以使用SFM(Structure from motion)方法計(jì)算每一幀圖像的對(duì)應(yīng)相機(jī)運(yùn)動(dòng)參數(shù).

現(xiàn)有的深度學(xué)習(xí)還無(wú)法完成一個(gè)完整的視覺(jué)定位系統(tǒng),但有望能夠解決傳統(tǒng)的VO方法難以解決的問(wèn)題,例如重定位[71]、長(zhǎng)極線匹配[72?73]、數(shù)據(jù)融合[74]等.在一個(gè)完整的VO系統(tǒng)中,深度網(wǎng)絡(luò)一般作為一個(gè)輔助系統(tǒng),利用高層次的語(yǔ)義分析,目標(biāo)識(shí)別的功能形成基于語(yǔ)義級(jí)的定位約束提高系統(tǒng)的精度和魯棒性.表6為一些深度學(xué)習(xí)網(wǎng)絡(luò)定位系統(tǒng)的特點(diǎn),包括要解決的問(wèn)題,輸出結(jié)果等.

在視差大(基線寬),而運(yùn)動(dòng)模型預(yù)測(cè)不好的狀態(tài)下,由于搜索區(qū)域較大,VO系統(tǒng)中容易發(fā)生點(diǎn)匹配失效.另外一些情況,例如局部外觀變化或自遮擋,點(diǎn)匹配也容易失效.Choy等[72]針對(duì)該問(wèn)題結(jié)合CNN(Convolutional neural network)和RNN網(wǎng)絡(luò),利用物體的形狀信息對(duì)單幀圖像完成三維重建.由于LSTM(Long short term memory)網(wǎng)絡(luò)可以學(xué)習(xí)長(zhǎng)期歷史信息,在訓(xùn)練中網(wǎng)絡(luò)針對(duì)同一物體不同視角的圖像的信息進(jìn)行處理,輸出物體的一個(gè)3D柵格.如果已知物體的外表和形狀,使用這些先驗(yàn)信息,在大視差下仍然可以完成特征匹配以及三維重建.使用深度網(wǎng)絡(luò)進(jìn)行深度圖估計(jì)可以省略中間步驟,例如形狀外表的學(xué)習(xí)和特征匹配,直接進(jìn)行三維重建[72,75?76],但需要使用預(yù)知的3D模型數(shù)據(jù).

表6　深度網(wǎng)絡(luò)定位系統(tǒng)特點(diǎn)Table 6　The comparison of the learning based localization methods

Doumanoglou等[77?78]利用隱類(lèi)型霍夫森林(Latent class Hough forest,LCHF)同時(shí)進(jìn)行物體識(shí)別和位姿估計(jì),LCHF在訓(xùn)練中使用正樣本和回歸保持類(lèi)分布在葉節(jié)點(diǎn)上.在測(cè)試中類(lèi)分布作為隱變量被迭代更新.Doumanoglou等[77]通過(guò)稀疏自編碼器提取對(duì)應(yīng)的特征向量,然后對(duì)特征向量構(gòu)成HF.在Hough空間中統(tǒng)計(jì)各節(jié)點(diǎn)投票數(shù),得到最終的物體類(lèi)別的位姿.使用深度網(wǎng)絡(luò)可以從單幀圖像中估計(jì)物體的位姿,該網(wǎng)絡(luò)在識(shí)別物體的同時(shí)估計(jì)物體的位姿.Wohlhart等[84]使用3D描述子表示物體的特征和物體的位姿,使用歐拉距離計(jì)算描述子之間的相似度.使用深度網(wǎng)絡(luò)完成位姿估計(jì)的一種方法是利用其他任務(wù)訓(xùn)練的網(wǎng)絡(luò)及參數(shù),遷移至定位估計(jì),例如(PoseNet[71],FuseNet[85]).使用端到端的訓(xùn)練方式中,圖像對(duì)應(yīng)的相機(jī)位姿數(shù)據(jù)作為回歸結(jié)果,損失函數(shù)為

Liu等[87]從深度值的連續(xù)性出發(fā),將深度值預(yù)測(cè)轉(zhuǎn)化為條件隨機(jī)場(chǎng)問(wèn)題,使用深度結(jié)構(gòu)化學(xué)習(xí)模式,構(gòu)造連續(xù)條件隨機(jī)場(chǎng)的一元和二元?jiǎng)莺瘮?shù).根據(jù)相鄰區(qū)域的像素的深度估計(jì)一致性信息,點(diǎn)的深度差作為一元?jiǎng)莺瘮?shù),計(jì)算區(qū)域間顏色差異,顏色直方圖差異和紋理差異,這些差異構(gòu)成二元?jiǎng)莺瘮?shù).

Handa等[85,88?89]提出了空間變換層,SO(3)層對(duì)應(yīng)旋轉(zhuǎn)變換,參數(shù)可以表示為一個(gè)三維向量,SE(3)層在SO(3)層的基礎(chǔ)上增加了一個(gè)平移,參數(shù)為一個(gè)6維向量.Sim(3)層在SE(3)的頂層有一個(gè)尺度因子,投影層將3D點(diǎn)投影到圖像平面,參數(shù)為焦距和光心位置.

雙塔結(jié)構(gòu)的網(wǎng)絡(luò)(例如MatchNet[90],LSM[83])的輸入為當(dāng)前幀圖像和參考幀圖像,雙塔CNN網(wǎng)絡(luò)使用了相同的參數(shù),為保證在訓(xùn)練結(jié)束后仍然保持相同的參數(shù),在訓(xùn)練時(shí)同步更新兩個(gè)子網(wǎng)絡(luò)參數(shù).Xiang等[66]雙塔結(jié)構(gòu)輸入的兩個(gè)通道分別是RGB圖像和深度圖像,在卷積層后使用數(shù)據(jù)聯(lián)合融合兩個(gè)通道的卷積信息和RNN處理幀間的信息實(shí)現(xiàn)深度重建.

另外一種常用結(jié)構(gòu)為編解碼器結(jié)構(gòu),例如FuseNet[80]、3D-R2N2[77],使用卷積層作為編碼器,反卷積層作為解碼器,LSTM置于編碼器和解碼器中,并融合來(lái)自深度圖像和RGB圖像信息.Choy等[72]利用LSTM網(wǎng)絡(luò)存儲(chǔ)信息的特點(diǎn),卷積層作為編碼器,經(jīng)過(guò)LSTM網(wǎng)絡(luò),數(shù)據(jù)進(jìn)入反卷積層.編碼器將圖像轉(zhuǎn)換至低維的特征空間,然后更新網(wǎng)絡(luò)狀態(tài),通過(guò)反卷積層解碼隱含層得到重建的三維點(diǎn).

5　定位方法性能評(píng)價(jià)

本節(jié)介紹視覺(jué)定位方法的驗(yàn)證方法.首先介紹一些性能的評(píng)價(jià)方法,然后介紹相關(guān)的數(shù)據(jù)集和工具庫(kù).

5.1　性能評(píng)價(jià)

如果驗(yàn)證數(shù)據(jù)集中提供了相機(jī)位姿的真值,那么可以直接比較測(cè)量值和真值,稱(chēng)為絕對(duì)軌跡誤差.這時(shí)進(jìn)行性能評(píng)價(jià)是比較直接的,但是實(shí)際上運(yùn)動(dòng)相機(jī)在連續(xù)采集圖像過(guò)程中難以獲得相機(jī)位姿的真值,參見(jiàn)表6.為完成算法的驗(yàn)證,Engel等[25]使用一個(gè)閉環(huán)的運(yùn)動(dòng),相機(jī)運(yùn)動(dòng)的開(kāi)始和結(jié)束在同一個(gè)位置,被測(cè)試算法只需要比較開(kāi)始和最終狀態(tài)下的位姿就可以計(jì)算出整個(gè)算法的漂移的大小.Engel等[25]給出了一種統(tǒng)一計(jì)算尺度誤差、位置、姿態(tài)的誤差的方法.該方法首先通過(guò)最小化測(cè)量結(jié)果和實(shí)際值之間的位姿,計(jì)算出初始時(shí)刻位姿Ts和結(jié)束時(shí)刻位姿Te.然后計(jì)算兩者之間的漂移Te,s=(Te)?1Ts.為了避免分別計(jì)算尺度、位置和旋轉(zhuǎn)的漂移,文章定義了對(duì)齊誤差.

這種測(cè)量方式可以應(yīng)用于具有不同的觀測(cè)方式的定位系統(tǒng),被評(píng)估的系統(tǒng)可以是雙目系統(tǒng)也可以是VIO系統(tǒng),對(duì)于尺度、位置、旋轉(zhuǎn)的誤差影響是均衡的.

另外一種難于驗(yàn)證的情形是相對(duì)位姿的驗(yàn)證,Burgard等[91?92]提出了一種基于圖模型的相對(duì)位姿計(jì)算方法,但該方法是基于二維空間中三自由度的運(yùn)動(dòng),我們將之拓展至三維空間六自由度的運(yùn)動(dòng).兩個(gè)位姿之間的相對(duì)誤差為

其中,?表示標(biāo)準(zhǔn)運(yùn)動(dòng)組合算子⊕的逆算子.我們假設(shè)對(duì)于一個(gè)SE(3)量的擾動(dòng)量?T,對(duì)應(yīng)的李代數(shù)表示為δξ=[δρ,δφ],一個(gè)原始的位姿T1=[R1,P1],擾動(dòng)之后的位姿為

其中,exp(?∧)表示so(3)李代數(shù)計(jì)算出反對(duì)稱(chēng)矩陣,然后進(jìn)行指數(shù)變換.?T=T2?T1,因此

5.2　開(kāi)源庫(kù)及相關(guān)工具

視覺(jué)方面,ORB、BRISK等特征描述子、LK光流法[26]等在OpenCV[93]均有實(shí)現(xiàn).另外一個(gè)重要的問(wèn)題是相機(jī)和IMU的標(biāo)定問(wèn)題,相機(jī)的標(biāo)定中對(duì)于針孔相機(jī)OpenCV Calib和MATLAB相機(jī)標(biāo)定工具箱使用了標(biāo)準(zhǔn)的模型.Kalibr[94]是一個(gè)工具箱,它能夠標(biāo)定多目相機(jī)系統(tǒng)、相機(jī)IMU相對(duì)位姿和卷簾快門(mén)相機(jī).常用的SFM工具有Bundler[95]、OpenMVG[96]和MATLAB多視幾何工具箱[97]等.Bunlder增量式地處理一組圖像,提取其中的特征點(diǎn)進(jìn)行匹配,完成三維重建并輸出一個(gè)稀疏的場(chǎng)景結(jié)構(gòu).OpenMVG則偏重于多視幾何問(wèn)題的求解.

優(yōu)化方面,Sophus庫(kù)為三維空間的剛體變換及李群李代數(shù)一個(gè)C++的實(shí)現(xiàn).Eigen為線性代數(shù)和(稀疏)矩陣的實(shí)現(xiàn),對(duì)LAPACK實(shí)現(xiàn)了C++的封裝.g2o[21]是一個(gè)針對(duì)非線性最小二乘優(yōu)化問(wèn)題的C++代碼實(shí)現(xiàn).VO問(wèn)題可以用圖表示,g2o把非線性最小二乘問(wèn)題表示為一個(gè)圖或超圖,圖的邊可以連接多個(gè)節(jié)點(diǎn),一個(gè)超圖是圖的拓展問(wèn)題,其他的優(yōu)化實(shí)現(xiàn)還包括ceres[98]、GTSAM[99]、iSAM[100]、SLAM++[101](這里的SLAM++不同于文獻(xiàn)SLAM++[62],前者是一個(gè)非線性?xún)?yōu)化方法,后者對(duì)應(yīng)一種語(yǔ)義SLAM系統(tǒng)).常用的優(yōu)化開(kāi)源庫(kù)及其使用場(chǎng)合,如表7所示.

表7　視覺(jué)定位系統(tǒng)工具庫(kù)Table 7　The common used tools in visual localization

5.3　驗(yàn)證數(shù)據(jù)集

大規(guī)模數(shù)據(jù)的存在使得深度網(wǎng)絡(luò)在各種視覺(jué)任務(wù)中達(dá)到較好的效果,同樣在機(jī)器人的定位技術(shù)發(fā)展的同時(shí)產(chǎn)生多種可用的數(shù)據(jù)集.這些數(shù)據(jù)使得研究者在沒(méi)有機(jī)器人硬件平臺(tái)的情況下仍然可以開(kāi)發(fā)出可以實(shí)際應(yīng)用的方法.我們從數(shù)據(jù)集的發(fā)布時(shí)間,數(shù)據(jù)的類(lèi)型,相機(jī)的類(lèi)型,真值的來(lái)源等方面介紹幾個(gè)VO系統(tǒng)中常用的驗(yàn)證數(shù)據(jù)集,如表8所示.

這些數(shù)據(jù)集具有不同的特點(diǎn),COLD數(shù)據(jù)集采集了來(lái)自不同光照條件下(白天、晚上、多云)的圖像.該數(shù)據(jù)包含了室內(nèi)的一些常見(jiàn)物體的圖像,一些語(yǔ)義地圖方法使用它作為驗(yàn)證數(shù)據(jù)集,驗(yàn)證語(yǔ)義建圖方法的效果.ICL NUM 數(shù)據(jù)規(guī)模適于訓(xùn)練深度網(wǎng)絡(luò),完成圖像的匹配,圖像的光流計(jì)算等.

6　未來(lái)發(fā)展方向

綜上所述,移動(dòng)機(jī)器人的視覺(jué)方法仍然存在多個(gè)方面的問(wèn)題,魯棒性方面的問(wèn)題主要集中在如何完成圖像的配準(zhǔn)以及系統(tǒng)初始化、卷簾快門(mén)等問(wèn)題,效率方面主要集中在如何實(shí)時(shí)的完成稠密、半稠密重建、圖像點(diǎn)的選擇、如何進(jìn)行邊緣化等問(wèn)題.

隨著深度學(xué)習(xí)在物體檢測(cè)、語(yǔ)義分割、物體跟蹤等方向的發(fā)展,環(huán)境中語(yǔ)義和環(huán)境理解更多地與視覺(jué)定位相結(jié)合提高視覺(jué)定位的魯棒性,并建立更精簡(jiǎn)的地圖.另外,嵌入式VO系統(tǒng)以及組合定位也將成為視覺(jué)定位系統(tǒng)的發(fā)展方向.

6.1　嵌入式系統(tǒng)

隨著移動(dòng)處理的發(fā)展,嵌入式系統(tǒng)的性能變得更加接近PC,但是計(jì)算能力仍然較弱.而移動(dòng)機(jī)器人和無(wú)人機(jī)等常常使用嵌入式系統(tǒng)作為視覺(jué)處理系統(tǒng).使用SIMD(Single instruction multiple data)指令可對(duì)3D重建和后端的優(yōu)化進(jìn)行加速.除了SIMD,另外一種加速方法是使用GPU.早期的VO方法只能進(jìn)行實(shí)時(shí)稀疏的三維點(diǎn)云重建,GPU的使用使得單目視覺(jué)能夠?qū)崟r(shí)完成稠密重建.嵌入式系統(tǒng)的GPU和CPU共享RAM存儲(chǔ)器,不需要像PC機(jī)那樣消耗很長(zhǎng)的時(shí)間完成數(shù)據(jù)在CPU和GPU之間的交換.Jetson TK1,TX1/2[112]使得開(kāi)發(fā)者可以在嵌入式系統(tǒng)中使用GPU,便于在無(wú)人機(jī)和移動(dòng)機(jī)器人對(duì)功耗和載重等要求嚴(yán)格的系統(tǒng)完成視覺(jué)定位算法.Pizzoli等[113]對(duì)深度圖建立深度濾波器,使用正則化方法,利用GPU實(shí)時(shí)完成稠密三維點(diǎn)云重建.DTAM[23]使用GPU針對(duì)特征缺失和圖像模糊等情況下實(shí)現(xiàn)穩(wěn)定的跟蹤.

6.2　組合定位

由于單一定位方法難以滿(mǎn)足機(jī)器人對(duì)定位精度的要求,所以組合定位方式[114]應(yīng)運(yùn)而生.一種組合定位方式是以INS為主,引入另一種輔助定位方式以修正慣性測(cè)量數(shù)據(jù)的累積誤差[115],例如GPS、視覺(jué)定位等.另一種組合定位方式以視覺(jué)定位為主,配合GPS、INS等,改善定位精度和魯棒性.第一種方式實(shí)時(shí)性好,較常見(jiàn)于無(wú)人機(jī)系統(tǒng).第二種方式信息量豐富,抗干擾能力強(qiáng),在移動(dòng)機(jī)器人系統(tǒng)中較常采用.

表8　VO系統(tǒng)常用驗(yàn)證數(shù)據(jù)集Table 8　The common used dataset in VO system

視覺(jué)信息和IMU數(shù)據(jù)融合在數(shù)據(jù)交互的方式上主要可以分為兩種方式,松耦合[115?117]和緊耦合[18?19].松耦合的方法采用獨(dú)立的慣性定位模塊和定位導(dǎo)航模塊,兩個(gè)模塊更新頻率不一致,模塊之間存在一定的信息交換.在松耦合方式中以慣性數(shù)據(jù)為核心,視覺(jué)測(cè)量數(shù)據(jù)修正慣性測(cè)量數(shù)據(jù)的累積誤差.松耦合方法中視覺(jué)定位方法作為一個(gè)黑盒模塊,由于不考慮IMU信息的輔助,因此在視覺(jué)定位困難的地方不夠魯棒,另外該方法無(wú)法糾正視覺(jué)測(cè)量引入的漂移.

緊耦合方式使用IMU完成視覺(jué)VO中的運(yùn)動(dòng)估計(jì),IMU在圖像幀間的積分的誤差比較小,IMU的數(shù)據(jù)可用于預(yù)測(cè)幀間運(yùn)動(dòng),加速完成點(diǎn)匹配,完成VO位姿估計(jì).相對(duì)于松耦合,緊耦合的另外一個(gè)優(yōu)點(diǎn)是IMU的尺度度量信息可以用于輔助視覺(jué)中的尺度的估計(jì).

6.3　語(yǔ)義分析與深度學(xué)習(xí)

語(yǔ)義分析和深度學(xué)習(xí)網(wǎng)絡(luò)在視覺(jué)定位中的作用越來(lái)越重要.在未來(lái)發(fā)展中,語(yǔ)義分析與視覺(jué)定位的結(jié)合可能表現(xiàn)有以下幾種形式:通過(guò)語(yǔ)義分割完成圖像的區(qū)域分割,物體檢測(cè)結(jié)果和圖像區(qū)域的分割結(jié)果建立新的約束實(shí)現(xiàn)相機(jī)更加精確的定位.另外可以通過(guò)對(duì)重建的三維點(diǎn)云分割建立更加緊湊的語(yǔ)義地圖,降低對(duì)空間資源的需求.

通過(guò)深度卷積網(wǎng)絡(luò)的特征提取有望取代手工設(shè)計(jì)的特征提取和匹配,通過(guò)離線或在線的訓(xùn)練,定位系統(tǒng)利用的特征更加貼近應(yīng)用場(chǎng)景,提高在相應(yīng)的應(yīng)用場(chǎng)景下的魯棒性和定位精度.通過(guò)RNN網(wǎng)絡(luò)在未來(lái)有望取代視覺(jué)里程計(jì)的幀間數(shù)據(jù)關(guān)聯(lián),通過(guò)LSTM等網(wǎng)絡(luò)的記憶特性,使得深度網(wǎng)絡(luò)更加方便地處理圖像幀序列并保存其中的歷史信息.通過(guò)深度網(wǎng)絡(luò)的端到端的訓(xùn)練實(shí)現(xiàn)場(chǎng)景識(shí)別,有望實(shí)現(xiàn)大規(guī)模的建圖,消除定位過(guò)程的累積誤差.

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

本文首先簡(jiǎn)述了定位問(wèn)題,對(duì)定位問(wèn)題進(jìn)行建模,按照數(shù)據(jù)關(guān)聯(lián)方式分類(lèi)介紹了幾種常用的VO系統(tǒng).然后圍繞魯棒性展開(kāi)介紹幾個(gè)方面的VO系統(tǒng)的特點(diǎn),這些方面在不同程度上影響了系統(tǒng)的魯棒性.接著介紹了語(yǔ)義分析在視覺(jué)定位中作用以及如何使用深度網(wǎng)絡(luò)進(jìn)行視覺(jué)定位.本文最后介紹了性能評(píng)價(jià)的方法,相關(guān)的開(kāi)源庫(kù)、開(kāi)源工具,以及驗(yàn)證數(shù)據(jù)集.

在過(guò)去的多年里,視覺(jué)定位系統(tǒng)取得了許多進(jìn)步,無(wú)論是早期的基于特征方法,還是采用光度值匹配的直接法都得到了較快發(fā)展.稀疏矩陣及相關(guān)的優(yōu)化工具使得VO系統(tǒng)可以使用圖優(yōu)化方法代替濾波器方法,顯著提升精度的同時(shí)保持實(shí)時(shí)性.視覺(jué)系統(tǒng)的研究已經(jīng)取得很多進(jìn)展,但是系統(tǒng)的魯棒性和資源消耗等方面還存在需要提高的地方.例如,應(yīng)對(duì)成像模型尤其是卷簾快門(mén)相機(jī)的建模方法、控制優(yōu)化規(guī)模同時(shí)不損失過(guò)多的精度、尺度漂移等,雖然有一些解決方法能夠在一定程度上提高系統(tǒng)的性能,但仍存在提升的空間.

深度學(xué)習(xí)在場(chǎng)景識(shí)別中的進(jìn)展,為我們提供了許多使用深度學(xué)習(xí)網(wǎng)絡(luò)完成定位的思路.語(yǔ)義分析與視覺(jué)定位的結(jié)合、深度學(xué)習(xí)應(yīng)用于視覺(jué)定位、嵌入式視覺(jué)定位系統(tǒng)和組合定位等都是未來(lái)定位和視覺(jué)定位系統(tǒng)的重要發(fā)展方向,這些方向有望在進(jìn)一步提升系統(tǒng)魯棒性的同時(shí)降低所需的計(jì)算資源.

1 Burri M,Oleynikovah,Achtelik M W,Siegwart R.Realtime visual-inertial mapping,re-localization and planning onboard MAVs in unknown environments.In:Proceedings of the 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Hamburg,Germany:IEEE,2015.1872?1878

2 Dunkley O,Engel J,Sturm J,Cremers D.Visual-inertial navigation for a camera-equipped 25g Nano-quadrotor.In:Proceedings of IROS2014 Aerial Open Source Robotics Workshop.Chicago,USA:IEEE,2014.1?2

3 Pinto L,Gupta A.Supersizing self-supervision:learning to grasp from 50K tries and 700 robot hours.In:Proceedings of the 2016 IEEE International Conference on Robotics and Automation(ICRA).Stockholm,Sweden:IEEE,2016.3406?3413

4 Ai-Chang M,Bresina J,Charest L,Chase A,Hsu J C J,Jonsson A,Kanefsky B,Morris P,Rajan K,Yglesias J,Cha fi n B G,Dias W C,Maldague P F.MAPGEN:mixed-initiative planning and scheduling for the mars exploration rover mission.IEEE Intelligent Systems,2004,19(1):8?12

5 Slaughter D C,Giles D K,Downey D.Autonomous robotic weed control systems:a review.Computers and Electronics in Agriculture,2008,61(1):63?78

6 Kamegawat,Yarnasakit,Igarashih,Matsuno F.Development of the snake-like rescue robot “kohga”.In:Proceedings of the 2004 IEEE International Conference on Robotics and Automation.New Orleans,LA,USA:IEEE,2004.5081?5086

7 Olson E.AprilTag:a robust and fl exible visual fi ducial system.In:Proceedings of the 2011 IEEE International Conference on Robotics and Automation(ICRA).Shanghai,China:IEEE,2011.3400?3407

8 Kikkerih,Parent G,Jalobeanu M,Birch fi eld S.An inexpensive method for evaluating the localization performance of a mobile robot navigation system.In:Proceedings of the 2014 IEEE International Conference on Robotics and Automation(ICRA).Hong Kong,China:IEEE,2014.4100?4107

9 Scaramuzza D,Faundorfer F.Visual odometry:Part I:the fi rst 30 years and fundamentals.IEEE Robotics and Automation Magazine,2011,18(4):80?92

10 Fraundorfer F,Scaramuzza D.Visual odometry:Part II:matching,robustness,optimization,and applications.IEEE Robotics and Automation Magazine,2012,19(2):78?90

11 Hesch J A,Roumeliotis S I.A direct least-squares(DLS)method for PnP In:Proceedings of the 2011 International Conference on Computer Vision(ICCV).Barcelona,Spain:IEEE,2011.383?390

12 Craighead J,Murphy R,Burke J,Goldiez B.A survey of commercial and open source unmanned vehicle simulators.In:Proceedings of the 2007 IEEE International Conference on Robotics and Automation.Roma,Italy:IEEE,2007.852?857

13 Faessler M,Mueggler E,Schwabe K,Scaramuzza D.A monocular pose estimation system based on infrared LEDs.In:Proceedings of the 2014 IEEE International Conference on Robotics and Automation(ICRA).Hong Kong,China:IEEE,2014.907?913

14 Meier L,Tanskanen P,Heng L,Lee Gh,Fraundorfer F,Pollefeys M.PIXHAWK:a micro aerial vehicle design for autonomous fl ight using onboard computer vision.Autonomous Robots,2012,33(1?2):21?39

15 Lee Gh,Achtelik M,Fraundorfer F,Pollefeys M,Siegwart R.A benchmarking tool for MAV visual pose estimation.In:Proceedings of the 11th International Conference on Control Automation Robotics and Vision(ICARCV).Singapore,Singapore:IEEE,2010.1541?1546

16 Klein G,Murray D.Parallel tracking and mapping for small AR workspaces.In:Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality(ISMAR).Nara,Japan:IEEE,2007.225?234

17 Leutenegger S,Lynen S,Bosse M,Siegwart R,Furgale P.Keyframe-based visual-inertial odometry using nonlinear optimization.The International Journal of Robotics Research,2015,34(3):314?334

18 Yang Z F,Shen S J.Monocular visual-inertial state estimation with online initialization and camera-IMU extrinsic calibration.IEEE Transactions on Automation Science and Engineering,2017,14(1):39?51

19 Shen S J,Michael N,Kumar V.Tightly-coupled monocular visual-inertial fusion for autonomous fl ight of rotorcraft MAVs.In:Proceedings of the 2015 IEEE International Conference on Robotics and Automation(ICRA).Seattle,WA,USA:IEEE,2015.5303?5310

20 Concha A,Loianno G,Kumar V,Civera J.Visual-inertial direct SLAM.In:Proceedings of the 2016 IEEE International Conference on Robotics and Automation(ICRA).Stockholm,Sweden:IEEE,2016.1331?1338

21 K¨ummerle R,Grisetti G,Strasdath,Konolige K,Burgard W.G2o:a general framework for graph optimization.In:Proceedings of the 2011 IEEE International Conference on Robotics and Automation(ICRA).Shanghai,China:IEEE,2011.3607?3613

22 Forster C,Pizzoli M,Scaramuzza D.SVO:fast semi-direct monocular visual odometry.In:Proceedings of the 2014 IEEE International Conference on Robotics and Automation(ICRA).Hong Kong,China:IEEE,2014.15?22

23 Newcombe R A,Lovegrove S J,Davison A J.DTAM:dense tracking and mapping in real-time.In:Proceedings of the 2011 IEEE International Conference on Computer Vision(ICCV).Barcelona,Spain:IEEE,2011.2320?2327

24 Engel J,Koltun V,Cremers D.Direct sparse odometry.arXiv:1607.02565,2016.

25 Engel J,Usenko V,Cremers D.A photometrically calibrated benchmark for monocular visual odometry.arXiv:1607.02555,2016.

26 Lucas B D,Kanadet.An iterative image registration technique with an application to stereo vision.In:Proceedings of the 7th International Joint Conference on Arti fi cial Intelligence.Vancouver,BC,Canada:ACM,1981.674?679

27 Baker S,Matthews I.Lucas-Kanade 20 years on:a unifying framework.International Journal of Computer Vision,2004,56(3):221?255

28 Klein G,Murray D.Parallel tracking and mapping for small AR workspaces.In:Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality(ISMAR).Nara,Japan:IEEE,2007.225?234

29 Concha A,Civera J.DPPTAM:dense piecewise planar tracking and mapping from a monocular sequence.In:Proceedings of the 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Hamburg,Germany:IEEE,2015.5686?5693

30 Engel J,Sturm J,Cremers D.Semi-dense visual odometry for a monocular camera.In:Proceedings of the 2013 IEEE International Conference on Computer Vision.Sydney,NSW,Australia:IEEE,2013.1449?1456

31 Engel J,Sch¨opst,Cremers D.LSD-SLAM:large-scale direct monocular SLAM.In:Proceedings of the 13th European Conference on Computer Vision.Zurich,Switzerland:Springer,2014.834?849

32 Rublee E,Rabaud V,Konolige K,Bradski G.ORB:an effi-cient alternative to SIFT or SURF.In:Proceedings of the 2011 IEEE International Conference on Computer Vision.Barcelona,Spain:IEEE,2011.2564?2571

33 Rosten E,Porter R,Drummondt.Faster and better:a machine learning approach to corner detection.IEEE Transactions on Pattern Analysis and Machine Intelligence,2010,32(1):105?119

34 Leutenegger S,Chli M,Siegwart R Y.Brisk:binary robust invariant scalable keypoints.In:Proceedings of the 2011 International Conference on Computer Vision.Barcelona,Spain:IEEE,2011.2548?2555

35 Bayh,Tuytelaarst,Van Gool L.Surf:speeded up robust features.In:Proceedings of the 9th European Conference on Computer Vision.Graz,Austria:Springer,2006.404?417

36 Mur-Artal R,Montiel J M M,Tard′os J D.Orb-SLAM:a versatile and accurate monocular SLAM system.IEEE Transactions on Robotics,2015,31(5):1147?1163

37 Herrera C D,Kim K,Kannala J,Pulli K,Heikkil¨a J.DTSLAM:deferred triangulation for robust SLAM.In:Proceedings of the 2nd International Conference on 3D Vision(3DV).Tokyo,Japan:IEEE,2014.609?616

38 Yang S C,Scherer S.Direct monocular odometry using points and lines.arXiv:1703.06380,2017.

39 Lu Y,Song D Z.Robust RGB-D odometry using point and line features.In:Proceedings of the 2015 IEEE International Conference on Computer Vision(ICCV).Santiago,Chile:IEEE,2015.3934?3942

40 Gomez-Ojeda R,Gonzalez-Jimenez J.Robust stereo visual odometry through a probabilistic combination of points and line segments.In:Proceedings of the 2016 IEEE International Conference on Robotics and Automation(ICRA).Stockholm,Sweden:IEEE,2016.2521?2526

41 Zhang L L,Koch R.An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency.Journal of Visual Communication and Image Representation,2013,24(7):794?805

42 Zhouh Z,Zou D P,Pei L,Ying R D,Liu P L,Yu Wx.StructSLAM:visual slam with building structure lines.IEEE Transactions on Vehicular Technology,2015,64(4):1364?1375

43 Zhang Gx,Suh Ih.Building a partial 3D line-based map using a monocular SLAM.In:Proceedings of the 2011 IEEE International Conference on Robotics and Automation(ICRA).Shanghai,China:IEEE,2011.1497?1502

44 Toldo R,Fusiello A.Robust multiple structures estimation with J-linkage.In:Proceedings of the 10th European Conference on Computer Vision.Marseille,France:Springer,2008.537?547

45 Camposeco F,Pollefeys M.Using vanishing points to improve visual-inertial odometry.In:Proceedings of the 2015 IEEE International Conference on Robotics and Automation(ICRA).Seattle,WA,USA:IEEE,2015.5219?5225

46 Gr¨ater J,Schwarzet,Lauer M.Robust scale estimation for monocular visual odometry using structure from motion and vanishing points.In:Proceedings of the 2015 IEEE Intelligent Vehicles Symposium(IV).Seoul,South Korea:IEEE 2015.475?480

47 Karpenko A,Jacobs D,Baek J,Levoy M.Digital Video Stabilization and Rolling Shutter Correction Using Gyroscopes,Stanford University Computer Science Technical Report,CTSR 2011-03,Stanford University,USA,2011.

48 Forss′en P E,Ringaby E.Rectifying rolling shutter video from hand-held devices.In:Proceedings of the 2010 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).San Francisco,CA,USA:IEEE,2010.507?514

49 Kerl C,St¨ueckler J,Cremers D.Dense continuous-time tracking and mapping with rolling shutter RGB-D cameras.In:Proceedings of the 2015 IEEE International Conference on Computer Vision(ICCV).Santiago,Chile:IEEE,2015.2264?2272

50 Pertile M,Chiodini S,Giubilato R,Debei S.Effect of rolling shutter on visual odometry systems suitable for planetary exploration.In:Proceedings of the 2016 IEEE Metrology for Aerospace(MetroAeroSpace).Florence,Italy:IEEE,2016.598?603

51 Kim Jh,Cadena C,Reid I.Direct semi-dense SLAM

for rolling shutter cameras.In:Proceedings of the 2016 IEEE International Conference on Robotics and Automation(ICRA).Stockholm,Sweden:IEEE,2016.1308?1315

52 Guo Cx,Kottas D G,DuToit R C,Ahmed A,Li R P,Roumeliotis S I.Efficient visual-inertial navigation using a rolling-shutter camera with inaccurate timestamps.In:Proceedings of the 2014 Robotics:Science and Systems.Berkeley,USA:University of California,2014.1?9

53 Dai Y C,Lih D,Kneip L.Rolling shutter camera relative pose:generalized epipolar geometry.In:Proceedings of the 2016 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Las Vegas,NV,USA:IEEE,2016.4132?4140

54 Faugeras O D,Lustman F.Motion and structure from motion in a piecewise planar environment.International Journal of Pattern Recognition and Arti fi cial Intelligence,1988,2(3):485?508

55 Tan W,Liuh M,Dong Z L,Zhang G F,Baoh J.Robust monocular SLAM in dynamic environments.In:Proceedings of the 2013 IEEE International Symposium on Mixed and Augmented Reality(ISMAR).Adelaide,SA,Australia:IEEE,2013.209?218

56 Limh,Lim J,Kimh J.Real-time 6-DOF monocular visual SLAM in a large-scale environment.In:Proceedings of the 2014 IEEE International Conference on Robotics and Automation(ICRA).Hong Kong,China:IEEE,2014.1532?1539

57 Davison A J,Reid I D,Molton N D,Stasse O.MonoSLAM:real-time single camera SLAM.IEEE Transactions on Pattern Analysis and Machine Intelligence,2007,9(6):1052?1067

58¨Ozyesil O,Singer A.Robust camera location estimation by convex programming.In:Proceedings of the 2015 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Boston,MA,USA:IEEE,2015.2674?2683

59 Daubechies I,DeVore R,Fornasier M,G¨unt¨urk C S.Iteratively reweighted least squares minimization for sparse recovery.Communications on Pure and Applied Mathematics,2010,63(1):1?38

60 S¨underhauf N,Protzel P.Switchable constraints for robust pose graph SLAM.In:Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Vilamoura,Portugal:IEEE,2012.1879?1884

61 Chum O,Wernert,Matas J.Epipolar geometry estimation via RANSAC bene fi ts from the oriented epipolar constraint.In:Proceedings of the 17th International Conference on Pattern Recognition(ICPR).Cambridge,UK:IEEE,2004.112?115

62 Salas-Moreno R F,Newcombe R A,Strasdath,Kelly Ph J,Davison A J.SLAM++:simultaneous localisation and mapping at the level of objects.In:Proceedings of the 2013 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Portland,OR,USA:IEEE,2013.1352?1359

63 Dharmasirit,Lui V,Drummondt.Mo-SLAM:multi object SLAM with run-time object discovery through duplicates.In:Proceedings of the 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Daejeon,South Korea:IEEE,2016.1214?1221

64 Choudhary S,Trevor A J B,Christensenh I,Dellaert F.SLAM with object discovery,modeling and mapping.In:Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Chicago,IL,USA:IEEE,2014.1018?1025

65 Dame A,Prisacariu V A,Ren C Y,Reid I.Dense reconstruction using 3D object shape priors.In:Proceedings of the 2013 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Portland,OR,USA:IEEE,2013.1288?1295

66 Xiang Y,Fox D.DA-RNN:semantic mapping with data associated recurrent neural networks.arXiv:1703.03098,2017.

67 Newcombe R A,Izadi S,Hilliges O,Molyneaux D,Kim D,Davison A J,Kohi P,Shotton J,Hodges S,Fitzgibbon A.KinectFusion:real-time dense surface mapping and tracking.In:Proceedings of the 10th IEEE International Symposium on Mixed and Augmented Reality(ISMAR).Basel,Switzerland:IEEE,2011.127?136

68 McCormac J,Handa A,Davison A,Leutenegger S.SemanticFusion:dense 3D semantic mapping with convolutional neural networks.arXiv:1609.05130,2016.

69 Vineet V,Miksik O,Lidegaard M,Nie?ner M,Golodetz S,Prisacariu V A,K¨ahler O,Murray D W,Izadi S,P′erez P,Torr Ph S.Incremental dense semantic stereo fusion for large-scale semantic scene reconstruction.In:Proceedings of the 2015 IEEE International Conference on in Robotics and Automation(ICRA).Seattle,WA,USA:IEEE,2015.75?82

70 Zamir A R,Wekelt,Agrawal P,Wei C,Malik J,Savarese S.Generic 3D representation via pose estimation and matching.In:Proceedings of the 14th European Conference on Computer Vision.Amsterdam,Netherlands:Springer,2016.535?553

71 Kendall A,Grimes M,Cipolla R.PoseNet:a convolutional network for real-time 6-DOF camera relocalization.In:Proceedings of the 2015 IEEE International Conference on Computer Vision(ICCV).Santiago,Chile:IEEE,2015.2938?2946

72 Choy C B,Xu D F,Gwak J,Chen K,Savarese S.3DR2N2:a uni fi ed approach for single and multi-view 3D object reconstruction.arXiv:1604.00449,2016.

73 Altwaijryh,Trulls E,Hays J,Fua P,Belongie S.Learning to match aerial images with deep attentive architectures.In:Proceedings of the 2016 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Las Vegas,NV,USA:IEEE,2016.3539?3547

74 Rambach J R,Tewari A,Pagani A,Stricker D.Learning to fuse:a deep learning approach to visual-inertial camera pose estimation.In:Proceedings of the 2016 IEEE International Symposium on Mixed and Augmented Reality(ISMAR).Merida,Mexico:IEEE,2016.71?76

75 Kar A,Tulsiani S,Carreira J,Malik J.Category-speci fi c object reconstruction from a single image.In:Proceedings of the 2015 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Boston,MA,USA:IEEE,2015.1966?1974

76 Vicente S,Carreira J,Agapito L,Batista J.Reconstructing PASCAL VOC.In:Proceedings of the 2014 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Columbus,OH,USA:IEEE,2014.41?48

77 Doumanoglou A,Kouskouridas R,Malassiotis S,Kimt K.Recovering 6D object pose and predicting next-best-view in the crowd.In:Proceedings of the 2016 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Las Vegas,NV,USA:IEEE,2016.3583?3592

78 Tejani A,Tang D,Kouskouridas R,Kimt K.Latent-class hough forests for 3D object detection and pose estimation.In:Proceedings of the 13th European Conference on Computer Vision.Zurich,Switzerland:Springer,2014.462?477

79 Agrawal P,Carreira J,Malik J.Learning to see by moving.In:Proceedings of the 2015 IEEE International Conference on Computer Vision(ICCV).Santiago,Chile:IEEE,2015.37?45

80 Vijayanarasimhan S,Ricco S,Schmid C,Sukthankar R,Fragkiadaki K.SfM-Net:learning of structure and motion from video.arXiv:1704.07804,2017.

81 Byravan A,Fox D.SE3-Nets:learning rigid body motion using deep neural networks.arXiv:1606.02378,2016.

82 Chopra S,Hadsell R,LeCun Y.Learning a similarity metric discriminatively,with application to face veri fi cation.In:Proceedings of the 2005 IEEE Computer Society Conference on Computer Vision and Pattern Recognition(CVPR).San Diego,CA,USA:IEEE,2005.539?546

83 Lengyel E S.Voxel-based Terrain for Real-time Virtual Simulations[Ph.D.dissertation],University of California,USA,2010.67?82

84 Wohlhart P,Lepetit V.Learning descriptors for object recognition and 3D pose estimation.In:Proceedings of the 2015 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Boston,MA,USA:IEEE,2015.3109?3118

85 Hazirbas C,Ma L N,Domokos C,Cremers D.FuseNet:incorporating depth into semantic segmentation via fusionbased CNN architecture.In:Proceedings of the 13th Asian Conference on Computer Vision.Taipei,China:Springer,2016.213?228

87 Liu F Y,Shen Ch,Lin G S.Deep convolutional neural fields for depth estimation from a single image.In:Proceedings of the 2015 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Boston,MA,USA:IEEE,2015.5162?5170

88 Handa A,Bloesch M,Pˇatrˇaucean V,Stent S,McCormac J,Davison A.Gvnn:neural network library for geometric computer vision.Computer Vision-ECCV 2016 Workshops.Cham:Springer,2016.

89 Jaderberg M,Simonyan K,Zisserman A,Kavukcuoglu K.Spatial transformer networks.In:Proceedings of the 2015 Advances in Neural Information Processing Systems.Montreal,Canada:Curran Associates,Inc.,2015.2017?2025

90 Hanx F,Leungt,Jia Y Q,Sukthankar R,Berg A C.MatchNet:unifying feature and metric learning for patch-based matching.In:Proceedings of the 2015 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Boston,MA,USA:IEEE,2015.3279?3286

91 Burgard W,Stachniss C,Grisetti G,Steder B,K¨ymmerle R,Dornhege C,Ruhnke M,Kleiner A,Tard¨os J D.A comparison of SLAM algorithms based on a graph of relations.In:Proceedings of the 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.St.Louis,MO,USA:IEEE,2009.2089?2095

92 K¨ummerle R,Steder B,Dornhege C,Ruhnke M,Grisetti G,Stachniss C,Kleiner A.On measuring the accuracy of SLAM algorithms.Autonomous Robots,2009,27(4):387?407

93 Kaehler A,Bradski G.Open source computer vision library[Online],available:https://github.com/itseez/opencv,February 2,2018

94 Furgale P,Rehder J,Siegwart R.Uni fi ed temporal and spatial calibration for multi-sensor systems.In:Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Tokyo,Japan:IEEE,2013.1280?1286

95 Snavely N,Seitz S M,Szeliski R.Photo tourism:exploring photo collections in 3D.ACM Transactions on Graphics,2006,25(3):835?846

96 Moulon P,Monasse P,Marlet R.OpenMVG[Online],available:https://github.com/openMVG/openMVG,December 9,2017

97 Capel D,Fitzgibbon A,Kovesi P,Wernert,Wexler Y,Zisserman A.MATLAB functions for multiple view geometry[Online],available:http://www.robots.ox.ac.uk/～vgg/hzbook/code,October 14,2017

98 Agarwal S,Mierle K.Ceres solver[Online],available:http://ceres-solver.org,January 9,2018

99 Dellaert F.Factor Graphs and GTSAM:a Hands-on Introduction,Technical Report,GT-RIM-CP&R-2012-002,February 10,2018

100 Kaess M,Ranganathan A,Dellaert F.iSAM:incremental smoothing and mapping.IEEE Transactions on Robotics,2008,24(6):1365?1378

86 DeTone D,Malisiewiczt,Rabinovich A.Deep image homography estimation.arXiv:1606.03798,2016.

101 Polok L,Ila V,Solony M,Smrz P,Zemcik P.Incremental block cholesky factorization for nonlinear least squares in robotics.In:Proceedings of the 2013 Robotics:Science and Systems.Berlin,Germany:MIT Press,2013.1?7

102 Vedaldi A,Fulkerson B.VLFeat:an open and portable library of computer vision algorithms[Online],available:http://www.vlfeat.org/,November 5,2017

103 Geiger A,Lenz P,Urtasun R.Are we ready for autonomous driving?the KITTI vision benchmark suite.In:Proceedings of the 2012 IEEE Conference on Computer Vision and Pattern Recognition(CVPR).Providence,RI,USA:IEEE,2012.3354?3361

104 Sturm J,Engelhard N,Endres F,Burgard W,Cremers D.A benchmark for the evaluation of RGB-D slam systems.In:Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robot and Systems(IROS).Vilamoura,Portugal:IEEE,2012.573?580

105 Handa A,Whelant,McDonald J,Davison A J.A benchmark for RGB-D visual odometry,3D reconstruction and SLAM.In:Proceedings of the 2014 IEEE International Conference on Robotics and Automation(ICRA).Hong Kong,China:IEEE,2014.1524?1531

106 Burri M,Nikolic J,Gohl P,Schneidert,Rehder J,Omari S,Achtelik M W,Siegwart R.The EuRoC micro aerial vehicle datasets.The International Journal of Robotics Research,2016,35(10):1157?1163

107 Mayer N,Ilg E,H¨ausser P,Fischer P,Cremers D,Dosovitskiy A,Broxt.A large dataset to train convolutional networks for disparity,optical fl ow,and scene fl ow estimation.In:Proceedings of the 2016 IEEE International Conference on Computer Vision and Pattern Recognition(CVPR).Las Vegas,NV,USA:IEEE,2016.4040?4048

108 Pronobis A,Caputo B.COLD:the COsy localization database.The International Journal of Robotics Research,2009,28(5):588?594

109 Silberman N,Hoiem D,Kohli P,Fergus R.Indoor segmentation and support inference from RGBD images.In:Proceedings of the 12th European Conference on Computer Vision.Florence,Italy:ACM,2012.746?760

110 Silberman N,Fergus R.Indoor scene segmentation using a structured light sensor.In:Proceedings of the 2011 IEEE International Conference on Computer Vision Workshop.Barcelona,Spain:IEEE,2011.601?608

111 Xiang Y,Mottaghi R,Savarese S.Beyond PASCAL:a benchmark for 3D object detection in the wild.In:Proceedings of the 2014 IEEE Winter Conference on Applications of Computer Vision(WACV).Steamboat Springs,CO,USA:IEEE,2014.75?82

112 Nikolskiy V P,Stegailov V V,Vecher V S.Efficiency of the tegra K1 and X1 systems-on-chip for classical molecular dynamics.In:Proceedings of the 2016 International Conference on High Performance Computing and Simulation(HPCS).Innsbruck,Austria:IEEE,2016.682?689

113 Pizzoli M,Forster C,Scaramuzza D.REMODE:probabilistic,monocular dense reconstruction in real time.In:Proceedings of the 2014 IEEE International Conference on Robotics and Automation(ICRA).Hong Kong,China:IEEE,2014.2609?2616

114 Faessler M,Fontana F,Forster C,Mueggler E,Pizzoli M,Scaramuzza D.Autonomous,vision-based fl ight and live dense 3D mapping with a quadrotor micro aerial vehicle.Journal of Field Robotics,2016,33(4):431?450

115 Weiss S,Achtelik M W,Chli M,Siegwart R.Versatile distributed pose estimation and sensor self-calibration for an autonomous MAV.In:Proceedings of the 2012 IEEE International Conference on Robotics and Automation(ICRA).Saint Paul,MN,USA:IEEE,2012.31?38

116 Weiss S,Siegwart R.Real-time metric state estimation for modular vision-inertial systems.In:Proceedings of the 2011 IEEE International Conference on Robotics and Automation(ICRA).Shanghai,China:IEEE,2011.4531?4537

117 Lynen S,Achtelik M W,Weiss S,Chli M,Siegwart R.A robust and modular multi-sensor fusion approach applied to MAV navigation.In:Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Tokyo,Japan:IEEE,2013.3923?3929