R3LIVE：一個(gè)實(shí)時(shí)魯棒、帶有RGB顏色信息的激光雷達-慣性-視覺(jué)緊耦合系統（1）

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

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

論文、代碼地址：在公眾號「計算機視覺(jué)工坊」，后臺回復「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

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

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

I 引言

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

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

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

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

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

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

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

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

II 系統框架

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

III. 數學(xué)符號

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

A．state

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

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

B. Maps representation

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

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

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

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

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

V．視覺(jué)-慣性里程計子系統

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

A. Frame-to-frame Visual-Inertial odometry

殘差(4)中的測量噪聲有兩個(gè)來(lái)源：一是中的像素跟蹤誤差，二是地圖點(diǎn)位置誤差

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