基于小滑窗優化的視覺慣性SLAM研究

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

（中國計量大學，浙江杭州，310018）

0 引言

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

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

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

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

在VINS-Fusion中實現了小滑窗PnP+IMU優化。

對改進后的VINS-Fusion使用EuRoc數據集進行測試。

1 VINS-Fusion的后端優化

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

1.1 非線性優化

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

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

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

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

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

1.2 邊緣化

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

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

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

圖1 邊緣化過程[2]

通過邊緣化這一操作，VINS的狀態變量的數目不僅得到控制，而且它們的約束關系得到優化。

但運行時，后端優化的狀態變量較多，求解Schur時的矩陣維數較高，頻繁的非線性優化及邊緣化操作對性能有一定的要求，當設備性能不足時會影響運行速度，過多地減少后端滑動窗口數量又會影響運行時的穩定性和精度。

2 小滑窗PnP+IMU優化

2.1 Vins_Mobile程序分析

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

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

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

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

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

vinsPnP::processImage()對小滑窗內的圖片進行優化，該函數的一個參數就是feature_msg；

vinsPnP：：updateFeatures()更新小滑窗內與最新幀有共視關系的3D點坐標；

vinsPnP::solve_ceres()優化小滑窗內的PVQ，但不優化路標點；

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

2.2 算法流程

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

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

圖2 小滑窗PnP+IMU優化算法示意圖

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

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

3 實現與測試

3.1 實現

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

圖3 加入小滑窗PnP+IMU優化后的程序流程圖

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

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

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

3.2 測試平臺

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

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

3.3 測試結果

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

表1 三者的CPU和內存使用率

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

表2 三者的APE和RPE指標

3.4 測試結果分析

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

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

圖4 MH_03數據集的軌跡疊加圖

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

4 結束語

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

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

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