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、慣性和視覺傳感器的測量來實現(xiàn)魯棒和準確的狀態(tài)估計。R3LIVE 包含兩個子系統(tǒng)，即激光雷達-慣性里程計 (LIO) 和視覺-慣性里程計 (VIO)。LIO 子系統(tǒng) (FAST-LIO) 利用 LiDAR 和慣性傳感器的測量結果構建全局地圖（即 3D 點的位置）的幾何結構。VIO 子系統(tǒng)利用視覺-慣性傳感器的數據來渲染地圖的紋理（即 3D 點的顏色）。更具體地說，VIO 子系統(tǒng)通過最小化幀到地圖的光度誤差來直接有效地融合視覺數據。開發(fā)的系統(tǒng) R3LIVE 是在我們之前的工作 R2LIVE 的基礎上開發(fā)的，經過精心的架構設計和實現(xiàn)。實驗結果表明，所得到的系統(tǒng)在狀態(tài)估計方面比現(xiàn)有系統(tǒng)具有更強的魯棒性和更高的精度。

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

I 引言

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

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

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

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

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

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

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

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

II 系統(tǒng)框架

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

III. 數學符號

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

A．state

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

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

B. Maps representation

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

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

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

IV. 激光-慣性里程計子系統(tǒng)

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

V．視覺-慣性里程計子系統(tǒng)

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

A. Frame-to-frame Visual-Inertial odometry

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

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