基于雅克比域零空間邊緣化的視覺SLAM*

蘆 濤，金 馨，廖毅霏，黃圣杰，楊依琳，謝國濤，，秦曉輝，3

（1.汽車車身先進設計制造國家重點實驗室，湖南大學機械與運載工程學院，長沙 410082；2.山東省科學技術情報研究院，濟南 250101；3.湖南大學無錫智能控制研究院，無錫 214115）

前言

自動駕駛技術包含環境感知、自主定位、決策規劃以及運動控制等多項關鍵技術［1］，其中自主定位技術是智能車輛在復雜環境下自主運動的基礎保障，是車輛決策與規控的重要前提［2］。同時定位與建圖（simultaneous localization and mapping，SLAM）是指智能車輛在沒有周圍環境的先驗信息前提下，僅利用自身傳感器完成位姿估計與地圖構建的過程，是自主定位技術中的重要組成部分［3］。近年來，隨著自動駕駛系統的推廣，SLAM 技術得到廣泛研究。根據傳感器方案不同，SLAM 技術主要分為視覺SLAM 與激光SLAM，相較于激光雷達，視覺傳感器具有成本低、易于安裝和信息豐富等特點，常用于智能車自動駕駛實際落地的部署方案中［4］。

視覺SLAM 算法基于PTAM 架構日漸成熟［5］，主要分為前端跟蹤模塊和后端優化模塊。目前主流視覺SLAM 研究工作主要集中在前端跟蹤及系統功能創新［6］，而對于自動駕駛落地任務，如何在保證精度及魯棒性的基礎上，提升高算力負擔下后端模塊的求解速度，保證其在搭載低算力平臺的設備上穩定運行成為了一個難題。

后端優化本質上是一個狀態估計模塊，待估計狀態量為相機曝光時刻位姿以及特征像素點空間位置［7］。由于待估計像素點數量一般較多，導致優化問題維度較大，需要利用一定技巧將優化問題降階。根據狀態估計方法不同，主要分為濾波框架以及基于非線性優化框架。基于濾波框架的代表作有MSCKF［8］，該系統在濾波更新階段將觀測方程的路標點雅克比投影至其左零空間使路標點邊緣化，形成僅含位姿狀態的新誤差觀測方程，從而在不擴充狀態向量前提下對系統進行觀測更新；基于優化框架的代表作有ORB-SLAM［9］等方法，此種優化框架在進行狀態估計時常常會利用海森矩陣獨有的稀疏性結合舒爾消元將待估計問題進行降階處理［10］。相較于非線性優化，基于濾波的后端估計存在著一些局限性，例如其需要將待估計狀態建模成具有高斯分布的隨機變量，在預測更新過程中實時維護其均值和協方差，導致系統擴展性較差且沒有異常檢測機制，在遇到離群數據時系統容易發散。此外，因為濾波器方法在一定程度上假設了馬爾科夫性，在觀測信息的使用上不像優化方法可以使用歷史數據；最重要的是，由于濾波方法無法在狀態更新后重新線性化，導致其精度會受到線性化誤差的影響，而基于優化的方法在迭代中可以基于新的狀態量多次線性化，在系統精度方面存在一定優勢，同時由于目前優化問題已有一些成熟的工具，如ceres［11］、g2o［12］等，使得基于非線性優化的視覺SLAM 在近些年受到廣泛關注。

基于非線性優化的后端模塊最主要功能就是實時執行光束法平差（bundle adjustment，BA），從一系列視覺圖像中提煉出最優像素點空間位置和相機外參［13］。由于像素路標點數量眾多，BA問題優化的算力要求很容易達到低算力平臺難以接受的程度。BA 優化中較為代表性的工作有ICE-BA［14］，其在迭代求解時的線性化階段使用增量式狀態線性化策略，避免反復線性化變化不大的狀態量，從而降低系統算力負擔，但帶來的結果是梯度不準確從而導致定位精度有一定下降。此外，也存在諸如MegBA 這種利用GPU 并行化求解BA 問題的工作［15］，但使用如此高成本設備求解自主定位模塊中一個小功能不利于工程化應用。在求解BA問題時，由于對增量方程（normal equation）直接求解會導致計算量較高，往往會利用海森矩陣稀疏性結合舒爾消元將優化問題降階，即邊緣化（marginalization）［16］。但基于舒爾消元的邊緣化方法不可避免地需要顯式構建海森矩陣（Hession matrix），即信息矩陣，為濾波框架中協方差矩陣的逆。對于一般視覺SLAM 系統，海森矩陣很容易達到成千上萬的維度，這為將BA 問題移植到低算力平臺實時求解留下了隱患。同時，由于目前基于非線性優化的視覺SLAM 框架一般使用通用的非線性優化器求解BA 問題，若針對具體系統設計優化器，在性能方面還存在一定的提升空間［17］。

為解決上述問題，受到基于濾波框架的MSCKF系統在觀測方程中使用左零空間邊緣化路標點的思想啟發［18］，加之濾波框架本身存在的局限性，本文提出一種利用雅克比域零空間在非線性優化視覺SLAM 系統中邊緣化路標點的方法，相較于傳統舒爾消元邊緣化方法，其能在不構建海森矩陣前提下將BA問題進行邊緣化降階，使得后端優化模塊在執行BA 過程中具備更好的數值穩定性。本文主要貢獻如下：

（1）提出一種利用雅克比域零空間在非線性優化視覺SLAM 系統中邊緣化路標點的方法，在代數上詳細推導了該邊緣化方法的過程，使得后端優化模塊可在不構建海森矩陣前提下將BA問題降階，提升了后端優化模塊的求解速度。

（2）在代數上證明了本文方法和基于舒爾消元的邊緣化方法的等價性，同時在數值分析的角度上證明本文方法具備更好的數值穩定性，從而支持求解器使用單精度浮點數求解，進一步提升運算效率，為后端優化模塊在一些精度和算力有限的低成本設備上運行提供了可能性。

（3）基于該邊緣化方法實現了非線性優化BA求解器，并將其應用在了主流的視覺SLAM框架［9］中。

（4）通過城市自動駕駛場景公開數據集［19］及實車試驗，證明本文方法具備更好的精度和處理速度。

1 系統架構

本文提出的基于雅克比域零空間邊緣化視覺SLAM 系統架構如圖1 所示，功能模塊主要分為3 個部分：前端跟蹤、后端優化和回環檢測。其中前端跟蹤部分接收圖像數據，經過與上一幀圖像數據關聯及位姿估計后［20］，將判定為關鍵幀的對象送到后端優化模塊進行優化，同時為優化問題提供初值；在后端模塊，首先利用關鍵幀與地圖點共視關系維護局部地圖，利用最小化重投影誤差對該局部地圖執行BA，獲得相機姿態以及路標點空間位置的最優解；最后再經過回環檢測模塊，進行回環矯正［21］，消除累積誤差。

圖1 基于雅克比域零空間邊緣化的視覺SLAM系統架構

該系統在ORB-SLAM2［22］基礎上，將基于第三方通用優化庫g2o 實現的舒爾消元邊緣化局部BA模塊替換成本文提出的基于雅克比域零空間邊緣化局部BA 模塊，如圖2 所示。該模塊在求解BA 時利用雅克比矩陣的左零空間投影將邊緣化操作從構建海森矩陣后提前到了線性化狀態量后，避免構建具有大維度海森矩陣同時降低了求解過程中的矩陣條件數，相較于基于舒爾消元邊緣化的局部BA模塊具備更好的精度和處理速度。

圖2 基于雅克比域零空間邊緣化的局部BA流程

2 基于零空間邊緣化的BA

本節介紹BA問題的數學形式，在代數上推導基于海森域舒爾消元邊緣化方法以及基于雅克比域零空間邊緣化方法，同時證明了兩種方法的等價性，并且從數值分析角度表明本文方法具備更好的運算效率和數值穩定性，最后介紹基于該邊緣化方法實現的求解器細節。

2.1 BA問題構建

本文的BA優化問題狀態量如下。

相機曝光時刻世界坐標系到相機坐標系的變換∈SE(3)，i∈[0，np]，李代數表示為ξi∈SE(3)，c表示相機坐標系，w表示世界坐標系，np表示相機狀態數量。

基于重投影誤差定義的代價函數為

式中：uij表示第i個相機對第j個地圖點的實際二維觀測；K為針孔相機的內參矩陣。

對于該最小二乘問題，一般利用高斯牛頓法迭代求解出最佳狀態量增量?x使得代價函數最小，即

式中：e(x)表示狀態量殘差；J(x)表示殘差對狀態量的雅克比矩陣；?x為狀態量增量。將式（2）展開后對增量?x求導并令其為零后生成增量方程：

式中H(x)=J(x)TJ(x)為海森矩陣（信息矩陣）。

令Pc=TcwPw=[xc yc zc]T，則Jp、Jl如下：

式中：fx、fy為相機模型參數；Rcw∈SO(3)為的旋轉矩陣。

2.2 基于海森域舒爾消元邊緣化

若要對BA問題進行求解，如何快速求解增量方程是關鍵。觀察式（4）可以發現，海森矩陣維度為(6np+3nl)，并且由于待優化的路標點數量遠遠大于相機數量（nl?np），很容易導致其維度達到成千上萬的級別，直接求逆解增量方程計算量太大，故一般會利用海森矩陣稀疏性結合邊緣化操作將優化問題降階，加速求解過程。

海森矩陣稀疏性是由BA 問題中相機與路標點觀測形式導致的，如圖3所示。

圖3 BA問題的矩陣稀疏性

基于信息矩陣結構特性，常在求解式（4）過程中結合舒爾消元進行邊緣化操作，即求出xp的邊緣分布，將求(?xp，?xl)的問題轉化為先固定?xl，求出?xp，再求出?xl的過程：

最后回代得到?xl：

2.3 基于雅克比域零空間邊緣化

如上節所述，基于舒爾消元的邊緣化依賴于超大維度海森矩陣的顯式構建，若想在避免構建海森矩陣前提下將優化問題進行降階，可直接在式（2）優化問題中將路標點進行邊緣化，即利用QR 分解將Jl進行左零空間投影：

式中：Q為正交矩陣；Q1和Q2分別構成了Jl列空間和零空間的基底；R1為上三角矩陣，表示將Jl旋轉到列空間Q1中的實際信息。

如此一來狀態量?xl在零空間Q2中變化不會影響整體殘差，利用此特性，可將式（2）優化問題經正交投影變換后拆成兩個子問題求解：

由于R1可逆，當給定?xp增量時，總能使得第一項為零，即

則式（13）優化項可簡化為

若要求解式（15），將其展開后對?xp求導即可得到邊緣化降階后僅含?xp的增量方程，對該增量方程求解后將?xp反代回式（14）即可得到式（2）優化問題在該次迭代中的結果。

相比基于舒爾消元的邊緣化方法，該方法在避免構建超大維度海森矩陣H(x)前提下就將優化問題進行邊緣化降階，轉化為先求出?xp、再求出?xl的過程。

2.4 等價性證明及數值穩定性分析

2.4.1 等價性證明

由于Q1和Q2為正交矩陣，即滿足：

將舒爾消元的式（9）及式（10）利用式（12）進行左零空間投影，結合式（16）進行化簡可得：

同時，將利用零空間邊緣化降階后的式（15）最小二乘問題展開：

對?xp求導并令其導數為零后可得基于零空間邊緣化降階后的增量方程：

可以發現，兩種邊緣化方式求得降階后的增量方程?xp=-完全等價，至此，證明了利用兩種邊緣化方法將式（2）優化問題降階后的等價性。隨后，僅須證明兩種邊緣化方式將?xp回代求得?xl完全等價即可。

同樣，將基于舒爾消元的回代式（11）方程進行左零空間投影：

該式和式（14）完全等價，證畢。

2.4.2 數值穩定性分析

如前所述，基于舒爾消元的邊緣化方法雖然可以對優化問題進行降階，但不可避免需要顯式構建海森矩陣，本文提出的基于雅克比域零空間邊緣化方法在避免構建海森矩陣的前提下就能將優化問題降階，從而提升求解效率。

同時注意到，邊緣化的本質是在信息矩陣的維度生成帶有xp邊緣分布的增量方程，從數值分析的角度上，在海森域與在雅克比域進行邊緣化操作的矩陣條件數是不同的，矩陣條件數含義即為求解過程中線性方程的抗擾程度。在實數域上推導上述公式無須考慮矩陣條件數，但對精度有限的計算機運算來說，數值誤差的出現不可避免。為降低數值誤差對增量影響程度，須降低迭代求解過程中的數值誤差，故現有求解器都是基于雙精度浮點數來實現的，從而保證求解過程中的數值穩定性。對于BA問題，由于H(x)=J(x)TJ(x)，可以得出基于舒爾消元邊緣化矩陣條件數是基于零空間邊緣化矩陣條件數的平方關系，這直接導致線性增量方程抗噪和抗擾能力產生了巨大差別。

對于浮點數計算而言，雖然單精度浮點數比雙精度浮點數運算效率更高，但也會引入更多數值誤差，若想通過使用單精度浮點數加速運算效率，降低條件數是必不可少的前提。綜上所述，本文提出在非線性優化框架的視覺SLAM 系統中使用雅克比域零空間邊緣化方法，可使得SLAM 系統在執行后端優化時在數值穩定性上有顯著改善，從而支持后端模塊可使用單精度浮點數計算，進一步提升求解效率，甚至為其在一些算力有限、只支持32 位架構的計算平臺上運行提供了可能性，這也符合自動駕駛設備落地部署的低成本化目的。

2.5 優化求解器實現

常見SLAM 系統在后端優化時一般使用成熟第三方通用優化庫（如g2o、ceres）實現BA 問題的求解，為評估使用雅克比零空間邊緣化求解BA 的效率，本文基于上述零空間邊緣化方法實現了列文伯格-馬夸爾特（Levenberg-Marquardt）［23］算法求解器，該求解器在細節流程上與g2o 實現的LM 算法對齊，阻尼項更新方式采用Nielsen 策略，對于阻尼項初始值，g2o實現的LM算法阻尼項初始化策略如下：

式中max(J(x)TJ(x))ii為降階前海森矩陣H(x)對角線最大值。

由于零空間邊緣化方法直接生成降階后的增量方程?xp=-，無法計算J(x)TJ(x)，故阻尼項初始值設為1e-4。此外，由于ORB-SLAM 基于共視關系維護局部地圖，為盡可能高效存儲BA過程中的矩陣信息，本文基于BASALT 實現的稠密路標相機索引塊（dense landmark block）［24］實現求解器，進一步提高內存利用效率。該求解器算法流程如下：

For 每次迭代 do

（1）殘差e計算即式（1），Huber核函數映射

（2）狀態量線性化J(x)即式（6）和式（7）

（3）邊緣化降階即式（12）

（4）構建增量方程即式(20)

（5）增量求解?x即式（14），備份狀態量

（6）更新狀態量x⊕?x

（7）計算更新后的殘差e(x⊕?x)即式（1）

Ifρ>0 do

更新阻尼項（減小）

If ?x收斂do

跳出迭代，優化結束

Else

更新阻尼項（增大）

狀態量回滾

END

3 試驗與結果分析

為評估本文提出的基于雅克比域零空間邊緣化算法性能，在KITTI 數據集和實車場景進行試驗，并與基準系統進行對比，基準選擇ORB-SLAM2 的單目版本在通用優化庫g2o（基于舒爾消元的邊緣化）的統計結果，本文實現的后端流程與優化策略和基準進行了對齊，從而保證試驗的準確性。同時，由于系統后端優化算法的整體改變，導致許多因素耦合在一起，使得后端優化模塊對系統精度與效率的影響僅能從實際場景的最終結果進行衡量。為此，本文設計了仿真試驗，在保證優化器輸入數據完全相同前提下分析優化過程的細節，從而佐證其對實際場景的結果影響。最后，設計了本文系統與ORBSLAM3的對比試驗。

試驗環境為Ubuntu 20.04，CPU 為i7-12700H，內存為16 GB，由于基于零空間邊緣化方法條件數遠低于基準方法（數值穩定性較好），使得優化器支持單精度浮點數運算從而進一步提升求解速度，對比試驗分為單精度浮點數版本和雙精度浮點數版本，以下使用double 表示雙精度浮點數版本，float 表示單精度浮點數版本，SM（schur marginalization）表示基于舒爾消元的邊緣化方法，NM（nullspace marginalization）表示基于零空間投影的邊緣化方法。

3.1 試驗介紹

公開數據集選擇城市自動駕駛測試場景較為豐富的KITTI 數據集，KITTI 數據集是目前自動駕駛領域中應用范圍最廣的公開數據集之一，廣泛用于SLAM 算法驗證。本文選取KITTI odometry 數據集中多組序列進行試驗，包含城鎮、高速、郊區等多個場景，能夠有效驗證本文算法在不同場景中的處理速度和精度，由于KITTI 數據集01 和08 序列分別由于基準軌跡發散和真值不準確問題，不參與此次試驗。

仿真數據可視化后的效果圖如圖4 所示，其通過系統在KITTI 數據集上運行時統計生成，記錄了某次局部BA時待優化的關鍵幀相機位姿、路標點位置以及觀測情況。在仿真試驗中，3 種優化器的配置參數完全相同，迭代次數為20 次，在輸入數據完全相同的前提下，統計每次優化迭代與殘差和耗時之間的關系，從而保證結果分析的有效性。

圖4 仿真數據可視化效果圖

實車場景利用如圖5（a）所示的試驗車采集，其上搭載了ZED-2i 視覺傳感器以及華測-410 組合慣導，錄制了一段包含長度為1 026 m、頻率為10 Hz、分辨率為540×960 的圖像數據，實車數據運行效果如圖5（b）所示，為隨機選取的雨天校園內，包含上坡、減速帶、匯車等場景。

圖5 試驗平臺車及實車數據運行效果圖

本文采用絕對位姿誤差（absolute pose error，APE）作為評價指標，計算每一相機曝光時刻算法輸出的相機位姿與真值之間對齊后的絕對誤差，以該指標的平均值（mean）和均方根誤差（root mean squared error，RMSE）綜合評估算法性能，同時統計后端模塊中每幀關鍵幀進行局部BA時的耗時，評估后端模塊在處理速度上的效率提升。

3.2 結果分析

3 種優化器在仿真數據上進行BA 優化的結果統計如圖6 所示，左側為迭代次數與殘差的關系統計圖，右側為迭代次數與耗時的關系統計圖。

圖6 仿真數據優化過程統計圖

從兩組仿真數據的迭代次數與殘差的統計圖可以看出，隨著迭代的不斷進行，整體殘差呈下降趨勢，由于零空間邊緣化算法與舒爾消元邊緣化算法在代數上的等價性，殘差的整體下降趨勢幾乎相同，但從圖6（a）可以看出，基準版本在優化時存在著經過一次迭代后殘差卻沒有下降的情況，因為此次迭代的狀態增量會導致整體殘差上升，根據2.5節所述的優化器策略，需要將狀態量回滾并重新調整阻尼項，此種不穩定迭代會導致多線程的SLAM在發生線程干預時系統誤差增大，而基于零空間邊緣化的優化器在兩種精度浮點數的優化結果上都呈穩定下降且高度一致的態勢，這說明了由于更小的條件數帶來優化穩定性的提升。同時，從迭代次數與耗時的關系圖可以發現單精度版本的優化器在每次迭代的處理速度上是最快的，而雙精度版本比基準版本略快。對于相同的輸入數據，單精度版本和雙精度版本在每次迭代的處理時間上幾乎相差一個定值，這說明改變浮點數精度所帶來的效率提升存在著一定上限，根據經驗，實際效率的提高與具體BA問題的形式有關，如問題規模、觀測情況等。

KITTI 數據集與實車數據集試驗APE 結果如表1 所示，提供了基準版本、兩種精度的浮點數版本以及ORB-SLAM3的對比統計。

表1 KITTI及實車數據集試驗APE對比結果

從表1 可以看出，在定位精度方面，本文提出的基于零空間邊緣化方法相較舒爾消元邊緣化方法，APE均在同一量級。同為雙精度浮點數求解的基準方法與本文方法（g2o-baseline 和Ours-double）對比可發現，本文方法由于矩陣條件數降低帶來數值穩定性提高的特性，可使基于本文方法實現的后端模塊在定位結果上取得更好的精度。在00 到10 序列中，基于零空間邊緣化的雙精度版本（Ours-double）定位誤差降低了10%～40%左右。單精度浮點版本相比于雙精度浮點版本，雖然定位精度有略微下降，但整體和基準版本持平，僅有02、07、09 及實車數據集的RMSE 比基準方法略低4%左右，精度下降的原因有可能是系統在轉換浮點數時發生數值截斷所導致。ORB-SLAM3 相比于ORB-SLAM2 在后端優化時處理外點的策略有所改變，從對比試驗可以發現，兩者整體精度相當，但存在部分序列誤差上升現象，說明僅改變后端優化策略不一定能在所有已經成功的場景中帶來精度提升，側面反映了對于一個完善的視覺SLAM 系統需要嘗試從各個模塊的本質進行改進從而提升系統性能。

上述對比版本在對應數據集上的后端優化耗時統計如表2 所示。由于ORB-SLAM3 與ORBSLAM2 在本文的BA 問題中耗時幾乎相同，故不在表中進行統計。可以發現，基準版本、雙精度浮點版本與單精度浮點版本在耗時統計上符合仿真試驗的結果，整體耗時呈依次遞減趨勢。其中雙精度浮點數版本由于不需要顯式構造維度較大的海森矩陣，在處理速度上有7%左右的提升。而單精度版本由于浮點精度的降低帶來運算效率的進一步提高，將求解BA 問題的耗時縮短了20%左右，為后端模塊釋放了一定的算力壓力，相較于基于通用優化庫實現的視覺SLAM，本文的工作展現出了更好的性能。

表2 KITTI及實車數據集后端優化耗時統計 ms

圖7 展示了在KITTI 的 00 序列上詳細試驗對比結果。對比圖7（a）與圖7（b）可以看出，本文提出的算法在定位精度上要優于基準算法，如同仿真試驗所展現的結果，雙精度版本和單精度版本在軌跡的一致性上表現較好，并沒有出現因引入單精度浮點數的數值計算誤差從而導致優化結果發散的情況。圖8為該00序列的統計后端優化中路標數量和優化耗時關系圖，由于待優化路標點數量遠遠大于待優化相機數量，優化問題維度基本取決于路標點數量，路標點數量可能在車輛發生轉向、地圖共用的情況出現時，導致局部關鍵幀數量增多，從而導致優化問題規模增大，耗時增加。對比圖8（a）～圖8（c）可以發現，雖然本文提出的邊緣化算法和基于舒爾消元通用優化庫算法（g2o）的優化耗時都和路標點數量成正比，但整體效率占優，尤其是單精度浮點版本的整體優化效率提升較大。此外，注意到使用本文算法生成的關鍵幀數量會比基準算法稠密一些，在平均每關鍵幀優化的路標點數量上也會略多，但雙精度版本的整體性能（精度、耗時）略好于基準算法，并且單精度版本在規模較大BA 問題（高峰值處）上由于數值位數降低引入的效率提升更大，降低了自動駕駛車輛在實際行駛過程中需要進行大規模BA 的算力負擔。

圖7 KITTI數據集00序列定位結果

圖8 KITTI數據集00序列后端優化模塊耗時統計

在實車試驗中，如圖9 所示，由于陰雨天氣導致相機部分模糊及校園內動態物體的存在，關鍵幀插入較為密集，從而導致局部地圖待優化路標點數量有一定上升，如圖10 所示，平均優化路標點數量達到了2 300 左右。在定位結果方面，如圖11 所示，雙精度浮點版本APE 的RMSE 比基準版本提升了28%，單精度浮點版本APE 基本和基準版本持平，但處理速度提升了19%。通過上述試驗分析，可以有效證明本文工作的雙精度浮點版本系統比基準版本處理速度略快的同時精度更高，單精度浮點版本的系統可在精度沒有顯著下降的情況下提升處理速度。

圖9 實車場景展示

圖10 實車數據集定位結果

圖11 實車數據集后端優化模塊耗時統計

4 結論

為提升基于非線性優化的視覺SLAM 在后端優化模塊處理速度，本文提出一種基于雅克比域零空間邊緣化的視覺SLAM 方法，該方法通過在后端優化模塊執行BA時將雅克比進行左零空間投影，從而達到無須顯式構建大維度海森矩陣就能將優化問題邊緣化降階效果。在代數上推導了基于雅克比域零空間邊緣化公式，及其與基于舒爾消元邊緣化方法的等價性，同時在數值分析角度上表明其相較基于舒爾消元的邊緣化方法具有更好的數值穩定性，這為自動駕駛車輛在實際行駛過程中需要進行大規模BA 提供支持，甚至為其在一些算力有限、只支持32位架構的計算平臺上運行提供了可能性。最后，基于該邊緣化方法實現了LM 算法優化求解器并與ORB-SLAM 系統進行融合，并在自動駕駛公開數據集和實車上進行試驗，結果表明本文工作相較于基于成熟第三方優化庫的后端優化模塊具有更好的精度和處理速度。

未來將嘗試在嵌入式平臺上基于此邊緣化方法實現多傳感器融合版本的求解器工作，提升系統魯棒性，并嘗試實現增量式BA，進一步提升后端效率。