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

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

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

0 引言

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

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

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

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

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

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

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

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

1.1 非線性優(yōu)化

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

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

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

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

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

1.2 邊緣化

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

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

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

圖1 邊緣化過程[2]

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

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

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

2.1 Vins_Mobile程序分析

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

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

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

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

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

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

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

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

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

2.2 算法流程

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

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

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

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

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

3 實現(xiàn)與測試

3.1 實現(xiàn)

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

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

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

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

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

3.2 測試平臺

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

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

3.3 測試結(jié)果

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

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

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

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

3.4 測試結(jié)果分析

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

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

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

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

4 結(jié)束語

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

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

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