R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package

作者：Jiarong Lin and Fu Zhang（香港大學)

論文、代碼地址：在公眾號「計算機視覺工坊」，后臺回復「R3LIVE」，即可直接下載。

視頻：https://www.bilibili.com/video/BV1d341117d6?p=1&share_medium=android&share_plat=android&share_session_id=34022f35-58d8-49df-9aca-79b5cba2074d&share_source=WEIXIN&share_tag=s_i&timestamp=1631455968&unique_k=pmkmxs

https://www.bilibili.com/video/BV1e3411q7Di?p=1&share_medium=android&share_plat=android&share_session_id=f42a7bfc-8e53-488e-87ce-485d1cdffb40&share_source=WEIXIN&share_tag=s_i&timestamp=1631455997&unique_k=KIOPb0

摘要： 本文中，我們提出了一種稱為 R3LIVE 的新型 LiDAR-Inertial-Visual 傳感器融合框架，它利用 LiDAR、慣性和視覺傳感器的測量來實現魯棒和準確的狀態估計。R3LIVE 包含兩個子系統，即激光雷達-慣性里程計 (LIO) 和視覺-慣性里程計 (VIO)。LIO 子系統 (FAST-LIO) 利用 LiDAR 和慣性傳感器的測量結果構建全局地圖（即 3D 點的位置）的幾何結構。VIO 子系統利用視覺-慣性傳感器的數據來渲染地圖的紋理（即 3D 點的顏色）。更具體地說，VIO 子系統通過最小化幀到地圖的光度誤差來直接有效地融合視覺數據。開發的系統 R3LIVE 是在我們之前的工作 R2LIVE 的基礎上開發的，經過精心的架構設計和實現。實驗結果表明，所得到的系統在狀態估計方面比現有系統具有更強的魯棒性和更高的精度。

R3LIVE 是一個面向各種可能應用的多功能且精心設計的系統，它不僅可以作為實時機器人應用的 SLAM 系統，還可以為測繪等應用重建密集、精確的 RGB 彩色 3D 地圖 。此外，為了使 R3LIVE 更具可擴展性，我們開發了一系列用于重建和紋理化網格的離線實用程序，這進一步縮小了 R3LIVE 與各種 3D 應用程序（如模擬器、視頻游戲等）之間的差距。

I 引言

最近，激光雷達傳感器越來越多地用于各種機器人應用，例如自動駕駛汽車 [1]、無人機 [2]-[4] 等。尤其是隨著低成本固態激光雷達的出現（例如，[5] )，更多基于這些 LiDAR 的應用 [6]-[10] 推動了機器人領域的發展。然而，對于基于 LiDAR 的 SLAM 系統，它們很容易在沒有足夠幾何特征的情況下失敗，特別是對于通常具有有限視場 [11] 的固態 LiDAR。為了解決這個問題，將 LiDAR 與相機 [12]-[15] 和超寬帶 (UWB) [16, 17] 等其他傳感器融合可以提高系統的魯棒性和準確性。特別是，最近在機器人領域中提出了各種 LiDAR-Visual 融合框架 [18]。

Zhang and Singh提出的 V-LOAM [19] 是 LiDAR-Inertial-Visual 系統的早期作品之一，它利用松散耦合的視覺-慣性測距 (VIO) 作為初始化 LiDAR 映射子系統的運動模型。類似地，在 [20] 中，作者提出了一種立體視覺慣性 LiDAR SLAM，它結合了緊耦合的立體視覺-慣性里程計與 LiDAR 建圖和 LiDAR 增強的視覺閉環。最近，Wang 提出了 DV-LOAM [21]，這是一個直接的 Visual-LiDAR 融合框架。該系統首先利用兩階段直接視覺里程計模塊進行有效的粗略狀態估計，然后使用 LiDAR 建圖模塊細化粗略姿態，最后利用閉環模塊來校正累積漂移。上述系統在松耦合的水平上融合了 LiDAR 慣性視覺傳感器，其中 LiDAR 測量沒有與視覺或慣性測量一起聯合優化。

最近提出了緊耦合的 LiDAR-Inertial-Visual 融合框架。例如，Zuo 等提出的 LIC-fusion [14] 是一個緊耦合的 LiDARInertial-Visual 融合框架，它結合了 IMU 測量、稀疏視覺特征、LiDAR 特征以及多狀態約束卡爾曼濾波器內的在線空間和時間校準（ MSCKF) 框架。為了進一步增強 LiDAR 掃描匹配的魯棒性，他們的后續工作稱為 LIC-Fusion 2.0 [15] 提出了一種跨滑動窗口內多個 LiDAR 掃描的平面特征跟蹤算法，并細化窗口內的姿態軌跡。Shan 等在 [13] 中提出 LVI-SAM 通過緊密耦合的平滑和建圖框架融合 LiDAR-Visual-Inertial 傳感器，該框架構建在因子圖之上。LVI_SAM 的 LiDAR-Inertial 和 Visual-Inertial 子系統可以在其中之一檢測到故障時獨立運行，或者在檢測到足夠多的特征時聯合運行。我們之前的工作 R2LIVE [12] 將 LiDAR-Inertial-Visual 傳感器的數據緊密融合，提取 LiDAR 和稀疏視覺特征，通過在誤差狀態迭代卡爾曼濾波器框架內最小化特征重投影誤差來估計狀態，以實現實時性能，同時通過滑動窗口優化提高整體視覺映射精度。R2LIVE 能夠在具有劇烈運動、傳感器故障的各種具有挑戰性的場景中運行，甚至可以在具有大量移動物體和小型 LiDAR FoV 的狹窄隧道狀環境中運行。

在本文中，我們解決了基于 LiDAR、慣性和視覺測量的緊耦合融合的實時同步定位、3D 建圖和地圖渲染問題。我們的貢獻是：

我們提出了一個實時同步定位、建圖和著色框架。所提出的框架包括用于重建幾何結構的 LiDAR 慣性里程計 (LIO) 和用于紋理渲染的視覺慣性里程計 (VIO)。整個系統能夠實時重建環境的稠密 3D RGB 色點云（圖 1（a）），

我們提出了一種基于 RGB_colored 點云圖的新型 VIO 系統。VIO 通過最小化觀察到的地圖點的 RGB 顏色與其在當前圖像中的測量顏色之間的光度誤差來估計當前狀態。這樣的過程不需要環境中的顯著視覺特征并節省相應的處理時間（例如特征檢測和提取），這使得我們提出的系統更加健壯，尤其是在無紋理環境中。

我們將所提出的方法實施到一個完整的系統 R3LIVE 中，該系統能夠實時且低漂移地構建環境的稠密、精確、3D、RGB 彩色點云圖。整個系統已在各種室內和室外環境中得到驗證。結果表明，我們的系統在行駛 1.5 公里后，平移僅漂移 0.16 米，旋轉漂移僅 3.9 度。

我們在 Github 上開源我們的系統。我們還開發了幾種離線工具，用于從彩色點云重建和紋理化網格（見圖 1（b）和（c））。我們設備的這些軟件實用程序和機械設計也是開源的，以使可能的應用程序受益。

II 系統框架

我們系統的概述如圖 2 所示，我們提出的框架包含兩個子系統：LIO 子系統（上部）和 VIO 子系統（下部）。LIO 子系統構建了全局地圖的幾何結構，它記錄了輸入的 LiDAR 掃描，并通過最小化點到平面的殘差來估計系統的狀態。VIO 子系統構建貼圖的紋理，用輸入圖像渲染每個點的 RGB 顏色，通過最小化幀到幀 PnP 重投影誤差和幀到貼圖光度誤差來更新系統狀態。

III. 數學符號

在整篇論文中，我們使用表 I 中所示的符號，這些符號已在之前的工作 R2LIVE [12] 中引入。

A．state

在我們的工作中，我們將完整狀態向量 x? ?29 定義為：

其中 Gg ? ?3是在全局幀（即第一個 LiDAR 幀）中表示的重力矢量，ItC是 IMU 和相機之間的時間偏移，同時假設 LiDAR 已經與 IMU 同步，φ 是相機內參矩陣。

B. Maps representation

我們的地圖由體素和點組成，其中點包含在體素中并且是地圖的最小元素。

1) 體素：為了在我們的 VIO 子系統中快速找到地圖中的點以進行渲染和跟蹤（參見 Section.V-C 和 Section.V-D），我們設計了一個固定大小（例如 0.1m *0.1m *0.1m） 名為體素的容器。如果一個體素最近附加了點（例如最近 1 秒），我們將這個體素標記為已激活。否則，該體素被標記為停用。

2）point：在我們的工作中，點P是一個大小為6的向量（坐標和顏色RGB）

IV. 激光-慣性里程計子系統

如圖 2 所示，R3LIVE 的 LIO 子系統構建了全局地圖的幾何結構。對于傳入的 LiDAR 掃描，由于幀內連續移動而導致的運動失真由 IMU 反向傳播補償，如 [6] 所示。然后，我們利用誤差狀態迭代卡爾曼濾波器 (ESIKF) 最小化點對平面殘差來估計系統的狀態。最后，在收斂狀態下，該掃描的點被附加到全局地圖上，并將相應的體素標記為激活或停用。全局地圖中累積的 3D 點形成幾何結構，也用于為我們的 VIO 子系統提供深度。R3LIVE中LIO子系統的詳細實現，請讀者參考我們之前的相關工作[12, 22]。

V．視覺-慣性里程計子系統

我們的 VIO 子系統渲染全局貼圖的紋理，通過最小化光度誤差來估計系統狀態。更具體地說，我們將全局地圖中的一定數量的點（即跟蹤點）投影到當前圖像，然后通過最小化這些點的光度誤差來迭代估計 ESIKF 框架內的系統狀態。為了提高效率，跟蹤的地圖點是稀疏的，這通常需要構建輸入圖像的金字塔。然而，金字塔對于也需要估計的平移或旋轉不是不變的。在我們提出的框架中，我們利用單個地圖點的顏色來計算光度誤差。在 VIO 中同時渲染的顏色是地圖點的固有屬性，并且不受相機平移和旋轉的影響。為了確保穩健且快速的收斂，我們設計了如圖 2 所示的兩步框架，我們首先利用幀到幀光流來跟蹤地圖點并通過最小化 Perspective-n-Point (PnP)來優化系統狀態跟蹤地圖點的投影誤差（第 VA 部分）。然后，我們通過最小化跟蹤點之間的幀到地圖光度誤差來進一步細化系統的狀態估計（第 V-B 部分）。使用收斂狀態估計和原始輸入圖像，我們執行紋理渲染以更新全局地圖中點的顏色（第 V-C 部分）。

A. Frame-to-frame Visual-Inertial odometry

殘差(4)中的測量噪聲有兩個來源：一是中的像素跟蹤誤差，二是地圖點位置誤差

(12)中第一項的詳細推導可以在R2LIVE [12]的E節中找到。