張小國，劉啟漢，李尚哲，王 慶

（東南大學(xué)儀器科學(xué)與工程學(xué)院，南京 210000）

近些年，同時(shí)定位與建圖技術(shù)[1,2](Simultaneous localization and mapping,SLAM) 逐漸成為一個(gè)非常重要的研究方向，其中單目SLAM 因其方案簡單、成本低廉等優(yōu)點(diǎn)在移動(dòng)機(jī)器人領(lǐng)域得到廣泛應(yīng)用。但單目SLAM 在弱紋理區(qū)域難以工作，且存在快速運(yùn)動(dòng)時(shí)圖像模糊、動(dòng)態(tài)場景易發(fā)生匹配錯(cuò)誤等問題[3]。針對(duì)上述問題，學(xué)者們提出了將慣性傳感器與單目相機(jī)融合的方案[4,5]。這一類方案在視覺難以工作時(shí)可采用IMU 積分保持定位能力，在兩者均能工作時(shí)可互相校正提供更好的定位和制圖能力。將視覺里程計(jì)與慣性傳感器的數(shù)據(jù)結(jié)合統(tǒng)稱為視覺-慣性里程計(jì)(Visual-Inertial Odometry，VIO/VI-SLAM)。VISLAM 的組合導(dǎo)航技術(shù)已成為當(dāng)前SLAM 領(lǐng)域的一大研究熱點(diǎn)。根據(jù)不同需求有不同VI-SLAM 的研究方法，如研究視覺慣導(dǎo)融合的SLAM，或研究視覺慣導(dǎo)融合里程計(jì)，前者注重定位和建圖，后者注重導(dǎo)航。根據(jù)圖像和慣性測量單元（IMU）數(shù)據(jù)的耦合程度不同，又分為松耦合和緊耦合。其中，松耦合通常將兩種傳感器單獨(dú)計(jì)算出導(dǎo)航信息，再將導(dǎo)航信息融合，如半直接視覺里程計(jì)(SVO)[6]算法；而緊耦合通常將兩種傳感器數(shù)據(jù)同時(shí)放在一起做優(yōu)化，最后得出導(dǎo)航信息，如VINS-Mono 后端非線性優(yōu)化算法[7]。在數(shù)據(jù)的融合方法中，可以分為基于濾波和基于圖優(yōu)化兩大類方法?；跒V波的方法以擴(kuò)展卡爾曼濾波(EKF)為代表[8]，S.Lynen 等人提出了多狀態(tài)限制卡爾曼濾波(MSCKF)[9]算法，并且這種方法后來與傳統(tǒng)的EKF-SLAM 相結(jié)合，使系統(tǒng)性能得到很大提升，適合計(jì)算資源有限的移動(dòng)設(shè)備。但MSCKF 使用全局信息優(yōu)化，缺少回環(huán)檢測。隨后Apostolopoulos 等人提出了魯棒的視覺慣性里程計(jì)ROVIO[10]，在擴(kuò)展的卡爾曼濾波器更新中使用直接強(qiáng)度誤差作為視覺度量，但ROVIO 只適用于視覺特征點(diǎn)非常多的環(huán)境。除了濾波算法外，基于圖優(yōu)化中的非線性優(yōu)化方法被越來越多廣泛應(yīng)用于SLAM系統(tǒng)。S.Leutenegger 等人提出了OKVIS[11]，建立了兼顧視覺重投影誤差和慣性誤差的優(yōu)化函數(shù)，在滑動(dòng)窗口上進(jìn)行非線性優(yōu)化，提高了OKVIS 的定位精度，但由于非線性優(yōu)化增大了計(jì)算量，系統(tǒng)效率較低。相較于OKVIS，秦天等人提出了基于非線性優(yōu)化的滑動(dòng)窗口估計(jì)系統(tǒng)VINS-Mono，該系統(tǒng)在前端增加了一種動(dòng)態(tài)的松耦合傳感器融合初始化程序，使IMU 數(shù)據(jù)預(yù)積分后才與圖像數(shù)據(jù)緊耦合，降低了對(duì)系統(tǒng)初始條件的限制。VINS-Mono 不僅從輸出結(jié)果精度上優(yōu)于基于EKF 的MSCKF、ROVIO，且在運(yùn)行效率上遠(yuǎn)優(yōu)于OKVIS。

VINS-Mono 雖然能在保持一定的運(yùn)行效率的基礎(chǔ)上實(shí)現(xiàn)較高精度SLAM，但存在非線性優(yōu)化運(yùn)算量較大的問題。為進(jìn)一步提高系統(tǒng)的運(yùn)行效率和定位精度，研究人員開展了兩個(gè)方向的研究：一是提高硬件配置，如采用GPU 加速[12]或多平臺(tái)協(xié)作運(yùn)行[13]，二是改進(jìn)軟件算法，通過增加數(shù)據(jù)的有效利用以提高精度或改進(jìn)運(yùn)算方法減少計(jì)算時(shí)間，如設(shè)計(jì)混合滑動(dòng)窗口優(yōu)化器[14]，優(yōu)化數(shù)據(jù)處理方式，或是改進(jìn)后端算法[15]等。

基于VINS-Mono 系統(tǒng)原有算法，本文在系統(tǒng)后端使用雙步邊緣化的算法[16]，將Hessian 矩陣中需要邊緣化的矩陣分步邊緣化，從而將大矩陣拆分計(jì)算提高系統(tǒng)運(yùn)行效率和集束調(diào)整的精度。同時(shí)，在系統(tǒng)前端，本文改進(jìn)了圖像關(guān)鍵幀篩選策略，提升圖像特征點(diǎn)選取的可靠性并且更有效利用前端數(shù)據(jù)，從而提高了SLAM 解算的精度和魯棒性。

1 系統(tǒng)框架

本文算法基于VINS-mono 系統(tǒng)改進(jìn)，系統(tǒng)框架如圖1所示。系統(tǒng)主要包括四個(gè)線程：前端圖像跟蹤(數(shù)據(jù)預(yù)處理和初始化)、后端非線性優(yōu)化、閉環(huán)檢測和閉環(huán)優(yōu)化。跟蹤線程采用光流跟蹤圖像相鄰幀，后端優(yōu)化用于優(yōu)化誤差保證最優(yōu)估計(jì)，閉環(huán)檢測和閉環(huán)優(yōu)化用于檢測整個(gè)系統(tǒng)環(huán)路并較正誤差。

本文采用雙步邊緣化的方法，通過邊緣化Hessian 矩陣進(jìn)行非線性優(yōu)化，降低矩陣的計(jì)算量。此外，在數(shù)據(jù)預(yù)處理階段，采用了添加非關(guān)鍵幀滑動(dòng)窗口的關(guān)鍵幀篩選策略來保證相鄰關(guān)鍵幀之間的視差大小，能夠適當(dāng)降低系統(tǒng)的數(shù)據(jù)冗余，提高運(yùn)行速度，保證系統(tǒng)平穩(wěn)運(yùn)行。

圖1 系統(tǒng)框架Fig.1 System framework diagram

2 改進(jìn)方法

2.1 基于雙步邊緣化的集束調(diào)整

VINS 系統(tǒng)在后端非線性優(yōu)化時(shí)采用了固定的滑動(dòng)窗口大小，當(dāng)新關(guān)鍵幀進(jìn)入滑動(dòng)窗口時(shí)，需要舍棄滑動(dòng)窗口里的最舊幀，以確保Hessian 矩陣的稀疏性。但是若直接刪除最舊幀，會(huì)直接破壞最舊幀對(duì)滑動(dòng)窗口內(nèi)相鄰幀的約束。

因此采用邊緣化的方法將最舊幀包含的約束信息繼續(xù)保存在Hessian 矩陣中，使系統(tǒng)能夠在不計(jì)算最舊幀幀內(nèi)信息的前提下，保留最舊幀對(duì)滑動(dòng)窗口內(nèi)相鄰幀的約束。同時(shí)，本文采用了一種雙步邊緣化的方法，相關(guān)流程圖如圖2所示。當(dāng)滑動(dòng)窗口有新的關(guān)鍵幀進(jìn)入時(shí)，系統(tǒng)需要邊緣化窗口中的最舊幀，根據(jù)因子圖顯示的關(guān)系將需要邊緣化的矩陣塊移動(dòng)至Hessian 矩陣左上角。

同時(shí)，為了減少計(jì)算成本，本文根據(jù)狀態(tài)之間的相關(guān)性將要邊緣化的狀態(tài)分為獨(dú)立狀態(tài)或從屬狀態(tài)。根據(jù)Schur 邊緣化的方法，按照先邊緣化獨(dú)立狀態(tài)，再邊緣化從屬狀態(tài)的方法分步計(jì)算，以此來降低矩陣計(jì)算量，提高集束調(diào)整的精度。

圖2 雙步邊緣化算法流程圖Fig.2 Flow chart of two-step marginalization algorithm

2.1.1 系統(tǒng)后端誤差優(yōu)化方程的構(gòu)建

基于視覺/慣性里程計(jì)前端得到的跟蹤定位結(jié)果，在后端對(duì)系統(tǒng)狀態(tài)量和觀測量進(jìn)行優(yōu)化。系統(tǒng)定義的狀態(tài)變量主要包括：滑動(dòng)窗口內(nèi)n+1 個(gè)相機(jī)的位姿xi（0≤i≤n）、加速度計(jì)偏差ba和陀螺儀偏差bg、以及相機(jī)到IMU 的外參（包括位置和旋轉(zhuǎn)）和m+1 個(gè)3D 點(diǎn)的逆深度λj（0≤j≤m），因此可以表示為：

本文中，為了求解最優(yōu)的狀態(tài)變量X*，定義系統(tǒng)后端的誤差優(yōu)化方程(后續(xù)簡稱為目標(biāo)函數(shù))為：

式(1)中，目標(biāo)函數(shù)中包含的三項(xiàng)依次為邊緣化先驗(yàn)誤差、IMU 測量殘差和視覺重投影誤差，使目標(biāo)函數(shù)取最小值的X即為最優(yōu)狀態(tài)變量X?。

目標(biāo)函數(shù)的優(yōu)化求解使用 Levenberg-Marquardt(LM)法。假設(shè)系統(tǒng)有一個(gè)較好的初始變量，系統(tǒng)通過最小化目標(biāo)函數(shù)，求解最優(yōu)狀態(tài)變量X*。將目標(biāo)函數(shù)記為f（X），進(jìn)行Taylor 展開后得：

其中J（X）為f（X）關(guān)于X的導(dǎo)數(shù)。因此求解的非線性最小二乘問題為：

對(duì)式(3)關(guān)于ΔX求導(dǎo)，并取零，得：

式(4)可簡寫為：

其中，H=J（X）TJ（X），b=-J（X）f（X）。

2.1.2 基于Schur 的邊緣化

當(dāng)新的關(guān)鍵幀進(jìn)入滑動(dòng)窗口，系統(tǒng)開始邊緣化最舊幀。邊緣化的對(duì)象包括最舊幀的相機(jī)位姿x和觀測到的部分路標(biāo)點(diǎn)。當(dāng)最舊幀與滑動(dòng)窗口內(nèi)其他幀存在共視，即觀測到相同的路標(biāo)點(diǎn)，邊緣化會(huì)將約束保留在與最舊幀存在共視的其他關(guān)鍵幀上。

確定需要邊緣化的路標(biāo)點(diǎn)和最舊幀位姿后，將優(yōu)化方程HΔX=b根據(jù)邊緣化狀態(tài)，寫為：

式(6)將優(yōu)化方程中的?X按照最舊幀中需要邊緣化和保留的部分分為δxa、δxb，對(duì)應(yīng)的H矩陣塊分別是Λa、Λb和Λc，對(duì)應(yīng)的b矩陣塊分別是ga和gb。

對(duì)式(6)采用Schur 消元：

式(6)～(8)，實(shí)現(xiàn)了Schur 邊緣化。

2.1.3 雙步邊緣化算法

在Hessian 矩陣中，主要的元素包括路標(biāo)點(diǎn)、相機(jī)位姿以及速度、加速度和IMU 偏差。

為了減少計(jì)算量，本文根據(jù)狀態(tài)之間的相關(guān)性，將需要邊緣化的狀態(tài)分為獨(dú)立狀態(tài)或從屬狀態(tài)。獨(dú)立集合中的任何兩個(gè)狀態(tài)都不相關(guān)，即獨(dú)立集內(nèi)的每個(gè)狀態(tài)僅與從屬狀態(tài)相關(guān)。本文定義路標(biāo)點(diǎn)的狀態(tài)是獨(dú)立的，而相機(jī)位姿以及速度、加速度和IMU偏差是從屬狀態(tài)。同時(shí)，本文以分步的方式邊緣化狀態(tài)量，即獨(dú)立狀態(tài)在從屬狀態(tài)之前被邊緣化。

如圖3所示，虛線框住的部分表示要邊緣化的狀態(tài)，其中Li（1≤i≤4）為路標(biāo)點(diǎn)，Pj（1≤j≤4）為相機(jī)位姿，bk（1≤k≤4）為IMU 偏差。其對(duì)應(yīng)的Hessian矩陣如圖4所示。邊緣化的狀態(tài)為x1=[L1,L2,L3,P1,b1]，其中僅P1和b1是從屬狀態(tài)的。直接與邊緣化狀態(tài)x1的有關(guān)聯(lián)的狀態(tài)為x2=[P2,b2,P3,b3,P4]。圖4-6 矩陣中的黃色塊代表路標(biāo)點(diǎn)自相關(guān)的矩陣塊，綠色塊表示將邊緣化的路標(biāo)點(diǎn)與位姿之間的矩陣塊，其余藍(lán)色塊表示非零矩陣塊。紅色框表示會(huì)被邊緣化影響的部分。

圖3 含邊緣化部分的示意圖Fig.3 Schematic diagram with marginalized parts

圖4 對(duì)應(yīng)于圖3的Hessian 矩陣Fig.4 The Hessian matrix corresponding to Fig.3

圖5 邊緣化L1后的Hessian 矩陣Fig.5 The Hessian matrix after marginalization of L1

在邊緣化L1后，再計(jì)算實(shí)現(xiàn)對(duì)路標(biāo)點(diǎn)L2的邊緣化，同理將所有路標(biāo)點(diǎn)邊緣化后的H矩陣如圖6所示。

圖6 邊緣化所有路標(biāo)點(diǎn)后的Hessian 矩陣Fig.6 The Hessian matrix after marginalization of all landmark points

完成所有獨(dú)立狀態(tài)的邊緣化后，再對(duì)圖6矩陣中的P1和b1進(jìn)行邊緣化，其中Ann、Anm、Amn為需要邊緣化的H矩陣，Amm為保留的H矩陣。通過計(jì)算完成邊緣化，即一步完成所有從屬狀態(tài)邊緣化。以上就是雙步邊緣化算法，其算法步驟可以由算法1 表示。

算法1:Two-step marginalization of Hessian matrix Input：-邊緣化變量的維數(shù)m-全體變量的維數(shù)pos-滑動(dòng)窗口中的關(guān)鍵幀矩陣H.-邊緣化路標(biāo)點(diǎn)個(gè)數(shù)n-當(dāng)前邊緣化的路標(biāo)點(diǎn)Li Output:-完成雙步邊緣化的矩陣H根據(jù)m 和pos 計(jì)算第二步邊緣化矩陣的維數(shù)r根據(jù)n 個(gè)獨(dú)立狀態(tài)和r 維從屬狀態(tài)排列pos 維的邊緣化數(shù)據(jù)for 0;;ii n i=＜++根據(jù)Schur 計(jì)算路標(biāo)點(diǎn) i LP 的邊緣化end for計(jì)算r 維從屬狀態(tài)的邊緣化

2.2 基于滑動(dòng)窗口的關(guān)鍵幀篩選

圖像幀輸入頻率變高會(huì)提高輸入數(shù)據(jù)的冗余程度，但冗余信息不能提升系統(tǒng)性能且浪費(fèi)了計(jì)算資源，因此需要在眾多圖像幀中篩選出關(guān)鍵幀。篩選關(guān)鍵幀的目的是降低圖像信息冗余度，提高系統(tǒng)運(yùn)行效率，該過程一般遵循兩個(gè)最基本的原則：其一是保持滑動(dòng)窗口內(nèi)相鄰的關(guān)鍵幀存在一定的視差，防止信息冗余；其二是當(dāng)快速運(yùn)動(dòng)導(dǎo)致采集的相鄰幀視差過大時(shí)，確保跟蹤的特征點(diǎn)不會(huì)丟失。

在篩選關(guān)鍵幀過程中，涉及到關(guān)鍵幀滑動(dòng)窗口和非關(guān)鍵幀滑動(dòng)窗口，在本方法中只有關(guān)鍵幀滑動(dòng)窗口參與后端非線性優(yōu)化和邊緣化，而非關(guān)鍵幀滑動(dòng)窗口不參與，且窗口大小固定為2。當(dāng)前幀定義為非關(guān)鍵幀滑動(dòng)窗口內(nèi)的最新幀，當(dāng)有圖像幀進(jìn)行關(guān)鍵幀判斷時(shí)，無論是不是關(guān)鍵幀，非關(guān)鍵幀滑動(dòng)窗口都進(jìn)行幀的替換。當(dāng)圖像幀參與關(guān)鍵幀篩選時(shí)，該幀已經(jīng)通過了系統(tǒng)前端的特征點(diǎn)提取和光流跟蹤。系統(tǒng)通過遍歷當(dāng)前幀的特征點(diǎn)計(jì)算數(shù)值參數(shù)作為關(guān)鍵幀的判定條件。在滿足以下條件中任意一項(xiàng)時(shí)，定義當(dāng)前幀為關(guān)鍵幀，并觸發(fā)邊緣化：

算法通過遍歷當(dāng)前幀的特征點(diǎn)，計(jì)算當(dāng)前幀內(nèi)信息并判斷以下條件：

1）如果關(guān)鍵幀滑動(dòng)窗口內(nèi)關(guān)鍵幀數(shù)量小于兩幀或者共視點(diǎn)數(shù)n1（滑窗中的特征點(diǎn)在當(dāng)前幀中繼續(xù)被追蹤到的點(diǎn)）小于threshold1，則視當(dāng)前幀為關(guān)鍵幀，在實(shí)驗(yàn)測試中根據(jù)測試效果閾值threshold1 最優(yōu)為20。

2）如果當(dāng)前特征點(diǎn)在關(guān)鍵幀滑動(dòng)窗口最新幀和關(guān)鍵幀滑動(dòng)窗口最新幀之前任一幀內(nèi)存在，那么該特征點(diǎn)就定義為平行特征點(diǎn)。當(dāng)平行特征點(diǎn)數(shù)n2為0，則視當(dāng)前幀為關(guān)鍵幀。

3）定義非關(guān)鍵幀滑動(dòng)窗口內(nèi)兩幀之間匹配的特征點(diǎn)數(shù)為n3，當(dāng)前幀內(nèi)的所有特征點(diǎn)數(shù)nnum。當(dāng)則視當(dāng)前幀為關(guān)鍵幀。實(shí)驗(yàn)測試中根據(jù)測試效果閾值threshold2 最優(yōu)為0.001。

4）若當(dāng)前特征點(diǎn)為平行特征點(diǎn)，則通過計(jì)算當(dāng)前特征點(diǎn)在當(dāng)前幀和關(guān)鍵幀滑動(dòng)窗口最新幀內(nèi)的歸一化坐標(biāo)的歐式距離作為視差l。設(shè)當(dāng)前幀和滑動(dòng)窗口最新幀匹配的平行特征點(diǎn)在歸一化平面的坐標(biāo)分別為 （x1,y1） 和 （x2,y2），視差若≥threshold4，則視當(dāng)前幀為關(guān)鍵幀。

關(guān)鍵幀篩選策略的算法步驟可以由算法2 表示。

算法2：Key frame selection strategy Input：-當(dāng)前幀K 已完成特征點(diǎn)提取-滑動(dòng)窗口大小N-在關(guān)鍵幀滑動(dòng)窗口中的幀 （）iK≤i ＜N-在非關(guān)鍵幀滑動(dòng)窗口的幀T-當(dāng)前幀K 中特征點(diǎn)jP 的數(shù)量n（0≤ j ＜）n Output:-當(dāng)前幀K 是否為關(guān)鍵幀F(xiàn)or ;;0 jj n j=＜++0計(jì)算共視點(diǎn)的數(shù)量 n1計(jì)算平行特征點(diǎn)的數(shù)量 2n計(jì)算前一個(gè)非關(guān)鍵幀T 與當(dāng)前幀K 之間匹配特征點(diǎn)的數(shù)量 n3 ifjP 是平行特征點(diǎn)計(jì)算t 和K0 中jP 的歸一化坐標(biāo)的歐幾里得距離lj l+=l總距離 j end if end for if 1 2 Nn threshold1＜＜或return true elseif n2=0 return true else if 2 2 n / n threshold＜return true else if 2 4 l n threshold≥return true end if

3 實(shí)驗(yàn)與分析

為驗(yàn)證本文算法對(duì)應(yīng)的運(yùn)算速度和定位精度，采用面向視覺慣性SLAM 算法的EuRoC 數(shù)據(jù)集進(jìn)行驗(yàn)證[17]。實(shí)驗(yàn)硬件平臺(tái)參數(shù)如表1所示。為了評(píng)估改進(jìn)了關(guān)鍵幀篩選和邊緣化算法的系統(tǒng)，使用絕對(duì)軌跡誤差（ATE）度量。本文將輸出結(jié)果與SVO-MSF[18]、OKVIS、ROVIO 和VINS-Mono進(jìn)行了比較[19]。SVO-MSF 是使用半直接視覺里程計(jì)（SVO）和多傳感器融合（MSF）[20]結(jié)合的松耦合算法。OKVIS 是基于開放關(guān)鍵幀的視覺慣性SLAM，它在關(guān)鍵幀姿勢的滑動(dòng)窗口上利用了非線性優(yōu)化。ROVIO 是魯棒的視覺慣性里程計(jì)，是基于擴(kuò)展卡爾曼濾波器（EKF）的視覺慣性狀態(tài)估計(jì)器。VINSMono 是使用預(yù)積分IMU 因子的基于非線性優(yōu)化系統(tǒng)，是目前綜合性能較優(yōu)的SLAM/IMU 組合導(dǎo)航算法。如表2和表3所示，在測試的5 個(gè)EuRoC 數(shù)據(jù)集中，本文方法相較于VINS-Mono 在運(yùn)行速度上平均提升14.91%，在輸出軌跡精度上提升率有17.59%。

表1 硬件平臺(tái)參數(shù)表Tab.1 Hardware platform parameters

為了獲得更詳細(xì)的系統(tǒng)效果，本文將估計(jì)的軌跡與EuRoC 數(shù)據(jù)集的軌跡真值（GroundTruth）進(jìn)行比較，輸出結(jié)果如圖7所示，圖7(a)是VINS-Mono系統(tǒng)估計(jì)的軌跡結(jié)果，圖7(b)是本文的系統(tǒng)估計(jì)的軌跡結(jié)果。需要指出的是，在圖7中可以通過觀察輸出軌跡與數(shù)據(jù)集真值的誤差看到改進(jìn)后精度的提升,由于軌跡圖路徑較為復(fù)雜，在圖7第二列和第三列放大了局部軌跡以顯示本文算法的優(yōu)化效果。圖8比較了本文的方法與VINS-Mono 在V1_01.easy 數(shù)據(jù)集中的誤差，其中藍(lán)色線表示本文的方法，紅色線表示VINS-Mono。從圖8可以看出，本文的系統(tǒng)比VINSMono 性能更穩(wěn)定和準(zhǔn)確。

表2 EuRoC 測試中運(yùn)行時(shí)間的比較Tab.2 Comparison of running time in EuRoC test

圖7 VINS-Mono 與本文系統(tǒng)在V1&V2&MH 測試數(shù)據(jù)集的輸出軌跡圖Fig.7 The output trajectory graphs of VINS-Mono and our method in V1&V2&MH test data set

表3 EuRoC 測試中絕對(duì)軌跡誤差的RMSE 比較（單位：米）Tab.3 RMSE comparison of absolute trajectory error in EuRoC test (Unit：m)

圖8 數(shù)據(jù)集EuRoCV1_01.easy的各方向誤差對(duì)比圖Fig.8 Comparison chart of each direction error of thedata set EuRoCV1_01.easy

4 結(jié)論

針對(duì)目前主流視覺/慣導(dǎo)組合SLAM 系統(tǒng)后端優(yōu)化中計(jì)算代價(jià)較大的問題，本文提出了一種改進(jìn)邊緣化策略和關(guān)鍵幀篩選策略的SLAM 方法，支持以更高的效率輸出更高精度的位姿計(jì)算。該系統(tǒng)通過增加非關(guān)鍵幀滑動(dòng)窗口之間的圖像幀約束關(guān)系，使圖像數(shù)據(jù)更有效地被使用，基于Schur 邊緣化的原理，通過將因子圖中的因子分類，并根據(jù)因子類別對(duì)應(yīng)的矩陣進(jìn)行分步邊緣化的方法，將高維矩陣分步拆解進(jìn)行運(yùn)算，降低了運(yùn)算難度，并提高了運(yùn)算精度。

采用公開數(shù)據(jù)集EuRoC驗(yàn)證本文算法。實(shí)驗(yàn)表明，本文的方法相較于VINS-Mono系統(tǒng)在平均運(yùn)行速度上平均提高了14.91%，且定位精度也有一定改善。