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

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

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

論文、代碼地址：在公眾號(hào)「計(jì)算機(jī)視覺工坊」，后臺(tái)回復(fù)「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、慣性和視覺傳感器的測量來實(shí)現(xiàn)魯棒和準(zhǔn)確的狀態(tài)估計(jì)。R3LIVE 包含兩個(gè)子系統(tǒng)，即激光雷達(dá)-慣性里程計(jì) (LIO) 和視覺-慣性里程計(jì) (VIO)。LIO 子系統(tǒng) (FAST-LIO) 利用 LiDAR 和慣性傳感器的測量結(jié)果構(gòu)建全局地圖（即 3D 點(diǎn)的位置）的幾何結(jié)構(gòu)。VIO 子系統(tǒng)利用視覺-慣性傳感器的數(shù)據(jù)來渲染地圖的紋理（即 3D 點(diǎn)的顏色）。更具體地說，VIO 子系統(tǒng)通過最小化幀到地圖的光度誤差來直接有效地融合視覺數(shù)據(jù)。開發(fā)的系統(tǒng) R3LIVE 是在我們之前的工作 R2LIVE 的基礎(chǔ)上開發(fā)的，經(jīng)過精心的架構(gòu)設(shè)計(jì)和實(shí)現(xiàn)。實(shí)驗(yàn)結(jié)果表明，所得到的系統(tǒng)在狀態(tài)估計(jì)方面比現(xiàn)有系統(tǒng)具有更強(qiáng)的魯棒性和更高的精度。

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

I 引言

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

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

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

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

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

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

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

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

II 系統(tǒng)框架

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

III. 數(shù)學(xué)符號(hào)

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

A．state

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

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

B. Maps representation

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

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

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

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

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

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

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

A. Frame-to-frame Visual-Inertial odometry

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

(12)中第一項(xiàng)的詳細(xì)推導(dǎo)可以在R2LIVE [12]的E節(jié)中找到。