（a）傳感器設備（b）構建室外場景模型

VIL-SLAM系統圖。傳感器為灰色，模塊為綠色。箭頭指示消息在系統中的流動方式。深色粗箭頭表示系統實時輸出，淺色粗箭頭表示近實時后處理生成的輸出。

系統有四個模塊，如圖2所示。視覺前端從立體攝像機獲取立體圖像。它執行幀到幀的跟蹤和立體幀匹配，并輸出立體匹配結果作為視覺測量。立體VIO采用立體匹配和IMU測量，在位姿圖上執行IMU預積分和平滑緊耦合結果。該模塊根據IMU和攝像機輸出VIO姿態。LiDAR建圖模塊使用VIO的運動估計，并執行LiDAR點去噪和掃描以進行地圖配準。回環閉合模塊進行視覺環路檢測和初始環路約束估計，并通過稀疏點云ICP對準進一步精細配準。對約束所有LiDAR姿態的全局姿態圖進行增量式優化，得到全局修正軌跡和實時LiDAR姿態修正。它們被送回激光雷達建圖模塊進行地圖更新和重新定位。在后處理中，將激光雷達掃描幀與最佳估計的激光雷達姿態拼接，以獲得稠密的點云地圖結果

圖6 實驗裝置   使用兩個VPs校準相機機

視覺前端

對立體視覺進行立體匹配，本文使用Kanade Lucas Tomasi（KLT）特征跟蹤器來跟蹤先前立體匹配中的所有特征點，無論是在左圖像還是右圖像中。只有當他們都被跟蹤才會輸出。較大的立體基線有助于尺度估計，并減少由遠距離特征引起的退化問題。我們使用基于特征的方法，比KLT更適合處理長基線。如果跟蹤的立體匹配數低于閾值，使用Shi-Tomashi角點檢測器進行特征提取，然后進行特征消除過程，刪除與任何小于閾值的現有特征的像素距離的特征。ORB描述子后計算所有幸存特征，然后進行暴力立體匹配以獲得新的匹配結果。系統通過對第一幀立體視覺進行立體匹配來初始化系統。

視覺慣導里程計

立體VIO的目標是在相對較高的頻率上提供實時準確的狀態估計，作為LiDAR建圖算法的運動模型。在位姿圖上運行的緊耦合固定滯后平滑器在精度和效率之間是一個很好的權衡。基于優化的方法通常允許多次線性化以接近全局最小值。固定滯后位姿圖優化器進一步限定了變量的最大數目，因此計算成本是有界的。由于糟糕的視覺測量會導致收斂問題，對視覺測量強制執行嚴格的異常值拒絕機制。系統通過檢查平均重投影誤差來消除異常值。

該算法以IMU預積分因子和無結構視覺因子作為約束條件

激光雷達建圖

激光雷達建圖在進行激光雷達點提取和掃描幀點云到地圖配準之前，使用高頻IMU速率VIO姿態作為運動。將掃描c表示為從一個完整的激光雷達旋轉獲得的點云。從c中提取幾何特征，包括銳邊上的點和平面上的點。然后，基于當前掃描到地圖的特征點（所有先前的特征點），通過最小化特征點形成的歐幾里德距離殘差，將其作為一個優化問題來解決。

基于雷達增強的回環檢測

回環檢測對任何一個SLAM系統都是至關重要的，因為長期運行會引入漂移。環路閉合的目的是通過對全局位姿圖進行優化來消除漂移，該全局位姿圖結合了LiDAR建圖的回環約束和相對變換信息。為了更好地輔助激光雷達制圖，校正后的激光雷達姿態被實時發送回來，以便新掃描的特征點被配準到重新訪問的地圖上。本文提出在視覺詞袋回環檢測和PnP回環約束公式的基礎上增加ICP對齊。該系統采用增量求解器iSAM2對全局姿態圖進行優化，實現了實時性

這里提供了五個典型環境的結果，包括無特色的走廊、雜亂無章的高臺、隧道和室外環境。所有這些序列的數據收集在同一點開始和結束。基于最終漂移誤差（FDE）對里程計（LiDAR建圖姿態）進行評估。以Faro掃描為基本真實值，用平均配準誤差（MRE）對建圖結果進行評估。首先將地圖與模型對齊（Faro掃描），然后計算地圖點與模型中最近點之間的歐幾里德距離。里程表FDE和建圖結果如表一所示，較好的用粗體顯示。

地圖比較：VIL-SLAM（右）和LOAM（左）與模型相比的地圖配準誤差。0.3m以上的誤差（a-b）用紅色表示，0.5m以上的誤差（c）用紅色表示。藍色和綠色內部不連續的紅色區域是由于掃描設備的遮擋導致的模型缺失。