基于小滑窗優(yōu)化的視覺慣性SLAM研究

李金春，張志呈，丁彬淵，曹雄，劉興禹

（中國計(jì)量大學(xué)，浙江杭州，310018）

0 引言

港科技大學(xué)飛行機(jī)器人實(shí)驗(yàn)室（沈邵劫團(tuán)隊(duì)）在2018年開源了VINS-Mono算法。它使用一個(gè)單目相機(jī)+慣性測(cè)量單元實(shí)現(xiàn)了緊耦合的視覺和慣性聯(lián)合狀態(tài)估計(jì)。2019年該團(tuán)隊(duì)開源VINS-Fusion是VINS-Mono的擴(kuò)展，它支持多種視覺慣性傳感器類型，包括單目+IMU、雙目+IMU、純雙目、雙目+IMU+GPS四個(gè)模式。VINS-Fusion是一個(gè)完整的基于優(yōu)化算法的SLAM系統(tǒng)，也是一個(gè)經(jīng)典且適合學(xué)習(xí)的視覺慣性SLAM算法。

VINS-Mobile是在VINS-Mono基礎(chǔ)上開發(fā)用于增強(qiáng)現(xiàn)實(shí)中定位功能并能在手機(jī)端運(yùn)行的開源算法，并針對(duì)手機(jī)端算力不足進(jìn)行了優(yōu)化，并提出了一種輕量級(jí)的緊密耦合融合方法，將環(huán)路信息集成在滑動(dòng)窗口單目VINS估計(jì)器中[4]。使用該方法可以提高VINS-Mobile的定位數(shù)據(jù)更新頻率。

我們?cè)陧?xiàng)目中選擇了雙目+IMU這一模式進(jìn)行研究。雙目相機(jī)的距離估計(jì)是比較左右眼的圖像獲得的，并不依賴其他傳感設(shè)備，所以它既可以應(yīng)用在室內(nèi)，亦可應(yīng)用于室外[1]。相較于單目相機(jī)，雙目相機(jī)可以做到更高的精度，同時(shí)避免尺度不確定性。另外與IMU模塊結(jié)合不僅可以彌補(bǔ)純視覺導(dǎo)航系統(tǒng)的缺陷，而且可以提高特征點(diǎn)的匹配速度并增強(qiáng)算法的魯棒性[2]。

在項(xiàng)目中，由于設(shè)備運(yùn)行的任務(wù)較多，加之控制成本，我們的設(shè)備存在性能不足情況，在了解到VINS-Mobile的輕量級(jí)優(yōu)化方案后，我們嘗試對(duì)VINS-Fusion進(jìn)行改進(jìn)，盡管相對(duì)位姿誤差有所增加，但成功降低CPU使用率，我們主要做了以下工作：

在VINS-Fusion中實(shí)現(xiàn)了小滑窗PnP+IMU優(yōu)化。

對(duì)改進(jìn)后的VINS-Fusion使用EuRoc數(shù)據(jù)集進(jìn)行測(cè)試。

1 VINS-Fusion的后端優(yōu)化

VINS-Fusion可分為四個(gè)模塊：初始化，前端數(shù)據(jù)處理，后端優(yōu)化，回環(huán)檢測(cè)。VINS-fusion的前端使用特征點(diǎn)法，用Harris提取特征點(diǎn)，利用LK光流法進(jìn)行特征點(diǎn)追蹤，同時(shí)對(duì)IMU數(shù)據(jù)進(jìn)行IMU預(yù)積分。運(yùn)行時(shí)前端獲取的數(shù)據(jù)會(huì)傳入后端優(yōu)化模塊，我們主要研究VINS-Fusion的后端優(yōu)化模塊，它主要包括非線性優(yōu)化、邊緣化兩個(gè)操作。

1.1 非線性優(yōu)化

VINS采用緊耦合方式對(duì)圖像和IMU進(jìn)行融合，緊耦合直接對(duì)測(cè)量數(shù)據(jù)進(jìn)行融合具有精度高、復(fù)雜度高的特點(diǎn)。前端獲得圖像和IMU數(shù)據(jù)傳遞到后端作為狀態(tài)變量。根據(jù)針孔相機(jī)模型建立具體參數(shù)化后的視覺觀測(cè)模型，并通過 IMU 各變量的運(yùn)動(dòng)學(xué)模型推導(dǎo)預(yù)積分的計(jì)算方法，建立參數(shù)化后的 IMU 觀測(cè)模型[6]，后端將狀態(tài)變量都視作優(yōu)化變量，對(duì)它們進(jìn)行非線性優(yōu)化，構(gòu)建非線性最小二乘，并使用高斯-牛頓法優(yōu)化，最后將優(yōu)化的結(jié)果通過ROS的話題發(fā)布。

需要優(yōu)化的狀態(tài)變量包括滑動(dòng)窗口內(nèi)的n+1幀所有相機(jī)的狀態(tài)xk（包括位置p、朝向q、速度v、加速度計(jì)bias 和陀螺儀bias）、相機(jī)到IMU的外參、m+1 個(gè)3D點(diǎn)的逆深度λ：

VINS-Fusion在進(jìn)行非線性優(yōu)化時(shí)使用慣性光束法平差公式，最小化所有測(cè)量殘差的先驗(yàn)和馬氏范數(shù)之和，以獲得最大后驗(yàn)估計(jì)[5]，目標(biāo)函數(shù)，式（1）：

式(1)中三個(gè)誤差項(xiàng)分別是邊緣化的先驗(yàn)信息、IMU的測(cè)量殘差和視覺的重投影殘差。

IMU測(cè)量殘差是兩幀數(shù)據(jù)之間的 PVQ和 bias變化量的差。視覺殘差是重投影誤差，對(duì)于第l個(gè)路標(biāo)點(diǎn)P，將P從第一次觀看到它的第i個(gè)相機(jī)坐標(biāo)系，轉(zhuǎn)換到當(dāng)前的第j個(gè)相機(jī)坐標(biāo)系下的像素坐標(biāo)[2]這一重投影過程所帶來的誤差。程序中使用非線性優(yōu)化庫Ceres進(jìn)行優(yōu)化求解。

1.2 邊緣化

在后端優(yōu)化中，不僅需要優(yōu)化狀態(tài)變量，還需要將過去生成的位姿信息和即將產(chǎn)生的位姿信息相結(jié)合，對(duì)整個(gè)運(yùn)動(dòng)軌跡進(jìn)行優(yōu)化[2]。VINS-Fusion使用滑動(dòng)窗口法實(shí)現(xiàn)，并增加了邊緣化的操作。運(yùn)行時(shí)邊緣化掉滑動(dòng)窗口中的最老幀或次新幀，從而達(dá)到不再計(jì)算這一幀的位姿或相關(guān)的路標(biāo)點(diǎn)的目的，但又保留了其對(duì)窗口內(nèi)其他幀的約束關(guān)系。這一過程使用Schur消元，如下所示：

其中Xb是需要保留部分，Xa是需要消去的項(xiàng)，整理得到式(4):

Sch ur消元將滑窗中邊緣化的幀轉(zhuǎn)化為滑窗內(nèi)待優(yōu)化變量的先驗(yàn)分布，既可以減少數(shù)據(jù)量，又不損失約束信息。整個(gè)過程如圖 1 所示，圖中Xp代表路標(biāo)點(diǎn)，Xm代表相機(jī)位姿，當(dāng)邊緣化Xp1時(shí)，與Xp1相連的Xp2，Xm1，Xm2和Xm3之間就會(huì)建立新的約束[2]。

圖1 邊緣化過程[2]

通過邊緣化這一操作，VINS的狀態(tài)變量的數(shù)目不僅得到控制，而且它們的約束關(guān)系得到優(yōu)化。

但運(yùn)行時(shí)，后端優(yōu)化的狀態(tài)變量較多，求解Schur時(shí)的矩陣維數(shù)較高，頻繁的非線性優(yōu)化及邊緣化操作對(duì)性能有一定的要求，當(dāng)設(shè)備性能不足時(shí)會(huì)影響運(yùn)行速度，過多地減少后端滑動(dòng)窗口數(shù)量又會(huì)影響運(yùn)行時(shí)的穩(wěn)定性和精度。

2 小滑窗PnP+IMU優(yōu)化

2.1 Vins_Mobile程序分析

為控制后端滑窗優(yōu)化的計(jì)算量，通常是每隔幾幀（比如3幀）選一幀送到后端處理。為計(jì)算被舍棄的幀的位姿，在VINS-Mobile版本中，加入一種輕量級(jí)的基于小滑窗的PnP+IMU聯(lián)合優(yōu)化方案[2]。VINS-Fusion在程序中選擇每兩幀送一幀數(shù)據(jù)至后端的方案，由于兩幀之間的圖像相似且相機(jī)幀率較高，對(duì)系統(tǒng)影響較小，但繼續(xù)增加發(fā)送間隔會(huì)造成定位數(shù)據(jù)更新滯后。

VINS-Mobile則采用了一種輕量級(jí)優(yōu)化方案，以10Hz的頻率執(zhí)行角點(diǎn)檢測(cè)，以30Hz的頻率執(zhí)行特征跟蹤[4]，但VINS-Mobile的論文中沒有具體詳細(xì)說明優(yōu)化方案，我們閱讀VINS-Mobile的源碼后，在VINS-Fusion中移植和實(shí)現(xiàn)這一優(yōu)化方案。

小滑窗PnP優(yōu)化在VINS-Moblie源碼中對(duì)應(yīng)的代碼主要位于vins_pnp.cpp文件中，通過vinsPnP類封裝。不過在其他文件如feature_traker.cpp中也有一部分涉及小滑窗PnP的代碼。

FeatureTracker::solveVinsPnP()尋找與最新幀中與滑窗中相同的路標(biāo)點(diǎn)，計(jì)算其在歸一化相機(jī)坐標(biāo)系中的點(diǎn)，將結(jié)果通過feature_msg傳遞[2]。

FeatureTracker::readImade()讀取圖片，進(jìn)行特征點(diǎn)檢測(cè)和追蹤，同時(shí)函數(shù)調(diào)用FeatureTracker::solveVinsPnP()；

vinsPnP::processImage()對(duì)小滑窗內(nèi)的圖片進(jìn)行優(yōu)化，該函數(shù)的一個(gè)參數(shù)就是feature_msg；

vinsPnP：：updateFeatures()更新小滑窗內(nèi)與最新幀有共視關(guān)系的3D點(diǎn)坐標(biāo)；

vinsPnP::solve_ceres()優(yōu)化小滑窗內(nèi)的PVQ，但不優(yōu)化路標(biāo)點(diǎn)；

vinsPnP::slideWindow()小滑窗PnP內(nèi)的滑窗操作；

2.2 算法流程

小滑窗PnP優(yōu)化運(yùn)行時(shí)每間隔3幀選一幀送至后端且間隔的3幀只追蹤特征點(diǎn)，從而減少后端和前端的計(jì)算量，但是增加小滑窗PnP優(yōu)化模塊，提高實(shí)時(shí)性。顧名思義也擁有一個(gè)滑窗用于儲(chǔ)存前端獲得的最新數(shù)據(jù)，并結(jié)合后端優(yōu)化結(jié)果，使用IMU的幀間約束（幀與幀之間的IMU預(yù)積分）和每一幀的2D-3D觀測(cè)（每一幀的 PnP）的視覺重投影誤差約束，構(gòu)成了PnP+IMU優(yōu)化。

后端完成優(yōu)化后，將優(yōu)化好的路標(biāo)點(diǎn)3D點(diǎn)坐標(biāo)、相機(jī)的狀態(tài)、加速度計(jì)bias 和陀螺儀bias傳遞給小滑窗PnP優(yōu)化模塊。小滑窗PnP優(yōu)化模塊更新與之有共視關(guān)系的3D點(diǎn)坐標(biāo)、相同幀的位姿及其它參數(shù)，并將它們作為不優(yōu)化的常量[4]。小滑窗PnP+IMU優(yōu)化算法示意圖如圖2所示。

圖2 小滑窗PnP+IMU優(yōu)化算法示意圖

計(jì)算沒有后端優(yōu)化結(jié)果的位姿時(shí)，將后端優(yōu)化結(jié)果作為先驗(yàn)信息對(duì)小滑窗PnP優(yōu)化模塊內(nèi)相機(jī)的位置、朝向、速度進(jìn)行優(yōu)化，不優(yōu)化路標(biāo)點(diǎn)。如圖2中所示，小滑窗的第四幀（從左向右數(shù)）加入后端的優(yōu)化結(jié)果，信賴并固定住第四幀的位置、朝向、速度，同時(shí)固定住此后小滑窗內(nèi)的bias，并作為先驗(yàn)信息優(yōu)化小滑窗內(nèi)其他幀。

小滑窗PnP優(yōu)化模塊僅優(yōu)化相機(jī)位姿不優(yōu)化路標(biāo)點(diǎn)化減少優(yōu)化變量，從而降低計(jì)算量。另一方面沒有對(duì)所有圖片進(jìn)行特征提取，也減少來自特征點(diǎn)檢測(cè)中的部分計(jì)算量。

3 實(shí)現(xiàn)與測(cè)試

3.1 實(shí)現(xiàn)

我們仿照VINS-Mobile，將小滑窗PnP優(yōu)化模塊添加到視覺前端與后端優(yōu)化之間。新的流程簡圖如圖3所示。

圖3 加入小滑窗PnP+IMU優(yōu)化后的程序流程圖

由于VINS-Fusion的默認(rèn)檢測(cè)的特征點(diǎn)數(shù)為150個(gè)遠(yuǎn)多于VINS-Mobile的60個(gè)，我們將小滑窗PnP優(yōu)化模塊的窗口大小設(shè)置為6，并每4幀送一幀至后端，后端滑動(dòng)窗口的大小為10。VINS-Fusion的運(yùn)行結(jié)果由小滑窗PnP優(yōu)化模塊發(fā)布，后端優(yōu)化模塊向其更新優(yōu)化數(shù)據(jù)，作為小滑窗的先驗(yàn)信息。

初始化與原VINS-fusion一致，系統(tǒng)方式以每兩幀送一幀至后端，避免受到小滑窗PnP優(yōu)化模塊的影響造成初始化時(shí)間增加和發(fā)生初始化異常。

其中，優(yōu)化結(jié)果的更新通過全局變量在小滑窗PnP優(yōu)化模塊與后端優(yōu)化中傳遞，并使用互斥鎖維護(hù)數(shù)據(jù)的安全。盡管后端優(yōu)化和小滑窗PnP優(yōu)化模塊所需的圖像與IMU數(shù)據(jù)有重疊，但各自需要的時(shí)刻不一致且二者數(shù)據(jù)重疊部分較少（如圖2所示），并且小滑窗PnP模塊的部分?jǐn)?shù)據(jù)需要后端優(yōu)化結(jié)果作為更新，所以我們將兩者所需的圖像和IMU數(shù)據(jù)單獨(dú)維護(hù)。兩者的數(shù)據(jù)單獨(dú)維護(hù)，雖然會(huì)增加內(nèi)存的使用，但小滑窗內(nèi)的數(shù)據(jù)較少，并且兩者在運(yùn)行時(shí)更加獨(dú)立、數(shù)據(jù)更安全，編寫多線程及后期維護(hù)時(shí)更加容易，運(yùn)行時(shí)，數(shù)據(jù)訪問不易沖突盡量避免 “死鎖”的發(fā)生。

3.2 測(cè)試平臺(tái)

測(cè)試的處理器為i5-8265U 1.60GHz，內(nèi)存8GB。數(shù)據(jù)集為EuRoc中的MH_01～MH_05。配置文件euroc_stereo_i imu_config.ymal的相關(guān)參數(shù)為默認(rèn)參數(shù)。

由于使用小滑窗優(yōu)化后，定位數(shù)據(jù)發(fā)布頻率與數(shù)據(jù)集中圖像發(fā)布率相同為20Hz，所以我們?cè)黾右唤M將圖像數(shù)據(jù)全部送入后端的測(cè)試作為相同頻率的對(duì)照。測(cè)試均未開啟回環(huán)檢測(cè)。

3.3 測(cè)試結(jié)果

我們分別對(duì)小滑窗PnP優(yōu)化模塊、默認(rèn)每兩幀送一幀（10Hz）、全部送入后端(20Hz)三種方案運(yùn)行時(shí)間內(nèi)的內(nèi)存使用率和CPU使用率進(jìn)行的測(cè)試。數(shù)據(jù)通過編寫Linux的Bash腳本，調(diào)用指令“top -n 400 0.5|grep vins_node ＞CpuMem.csv”獲取，再通過WPS整理數(shù)據(jù)。測(cè)試過程中，多種因素如CPU運(yùn)行產(chǎn)生溫度、數(shù)據(jù)集的時(shí)間長度及難易等使數(shù)據(jù)有所浮動(dòng)。數(shù)據(jù)為多次測(cè)量平均值，如表1所示。

表1 三者的CPU和內(nèi)存使用率

我們使用軌跡評(píng)估工具evo對(duì)三者的絕對(duì)位姿誤差（APE）和相對(duì)位姿誤差（RPE）進(jìn)行了評(píng)估，以標(biāo)準(zhǔn)差作為評(píng)估的指標(biāo)，如表2所示。

表2 三者的APE和RPE指標(biāo)

3.4 測(cè)試結(jié)果分析

盡管CPU的使用率有所浮動(dòng)，但總體上使用小滑窗PnP優(yōu)化模塊可以使用更少的CPU來獲得較高頻率的定位數(shù)據(jù)輸出，相應(yīng)的內(nèi)存有所增加，是一種以空間換取CPU性能的策略。

同時(shí)絕對(duì)位姿誤差增加較少，對(duì)整條軌跡的全局一致性影響較弱，但反映局部準(zhǔn)確性的相對(duì)位姿誤差明顯增加，這是因?yàn)樾』癙nP優(yōu)化模塊使用后端數(shù)據(jù)優(yōu)化局部，整體位姿受影響較小，但局部有一定程度的影響。雖然局部位姿有所影響，但其計(jì)算得到的軌跡與真實(shí)軌跡的誤差可以接受，如圖4所示。

圖4 MH_03數(shù)據(jù)集的軌跡疊加圖

盡管改進(jìn)后降低對(duì)設(shè)備算力的消耗，但對(duì)設(shè)備性能仍有要求，我們?cè)趈etson nano上運(yùn)行GPU版的VINS-Fusion,并且使用小滑窗PnP+IMU聯(lián)合優(yōu)化方案，效果并不理想。

4 結(jié)束語

僅憑一個(gè)模塊想同時(shí)提高性能和精度是很難的事，有失有得是常有的。這是我們幾人第一次參與此類項(xiàng)目，除了學(xué)到了不少知識(shí)，還認(rèn)識(shí)到細(xì)節(jié)對(duì)結(jié)果不少的影響，實(shí)際運(yùn)行時(shí)還需要考慮設(shè)備的參數(shù)如圖像與IMU數(shù)據(jù)的時(shí)間戳對(duì)齊，不然程序容易發(fā)生異常。不過這些還只算剛剛?cè)腴T，定有不少不足之處，今后我們會(huì)不斷改進(jìn)，為將來參與真正的科研做好準(zhǔn)備。

同時(shí)也要感謝崔華坤老師的《vins論文推導(dǎo)及代碼解析》給我們提供很多思路，對(duì)我們研究和學(xué)習(xí)VINS有很大的幫助。

當(dāng)然，我們也能從測(cè)試中看出VINS的前端消耗量較多的算力，盡管VINS使用光流法實(shí)現(xiàn)特征點(diǎn)的匹配，但特征點(diǎn)的提取仍占據(jù)大多數(shù)的計(jì)算時(shí)間。最后我們用《視覺SLAM十四講》中 的“如何提高特征提取、匹配算法的速度，將是特征點(diǎn)方法的一個(gè)重要的主題[1]”結(jié)束本文。