一種R3LIVE++的LiDAR慣性視覺融合框架

摘要

本文提出了一種名為 R3LIVE++ 的 LiDAR 慣性視覺融合框架，以實現(xiàn)穩(wěn)健且準確的狀態(tài)估計，同時實時重建輻射圖。R3LIVE++ 由實時運行的 LiDAR 慣性里程計 (LIO) 和視覺慣性里程計 (VIO) 組成。LIO 子系統(tǒng)利用來自 LiDAR 的測量來重建幾何結(jié)構(gòu)，而 VIO 子系統(tǒng)同時從輸入圖像中恢復(fù)幾何結(jié)構(gòu)的輻射信息。

R3LIVE++基于R3LIVE，通過結(jié)合相機光度校準和相機曝光時間的在線估計，進一步提高了定位和建圖的準確性。論文在公開和私有數(shù)據(jù)集上展開了充分的實驗，并同其他SOTA的SLAM 系統(tǒng)進行比較。定量和定性的結(jié)果表明，R3LIVE++ 在準確性和魯棒性方面都有很大的優(yōu)勢。

此外，為了展示R3LIVE++ 的可擴展性，論文基于R3LIVE++得到的重建地圖開發(fā)了若干應(yīng)用程序，例如高動態(tài)范圍 (HDR) 成像、虛擬環(huán)境探索和 3D 視頻游戲。 在SLAM中融合LiDAR和相機測量，可以克服各個傳感器在定位中的退化問題，并生成滿足各種地圖應(yīng)用需求的精確、紋理化和高分辨率3D地圖。基于此，論文提出的R3LIVE++有以下特點：

R3LIVE++是一個激光雷達慣性視覺融合框架，緊密耦合了兩個子系統(tǒng)：LIO和VIO子系統(tǒng)。這兩個子系統(tǒng)聯(lián)合增量地實時構(gòu)建周圍環(huán)境的3D輻射圖。特別是，LIO通過將LiDAR中的新點云配準到地圖中來重建幾何結(jié)構(gòu)，而VIO則通過將圖像中的像素顏色渲染到地圖中的點來恢復(fù)輻射信息；

R3LIVE++提出一個新穎的VIO設(shè)計，通過最小化輻射圖中的點與當前圖像中稀疏像素集合間的輻射度差異來跟蹤相機姿態(tài)（并估計其他系統(tǒng)狀態(tài)）。frame-to-map的對齊有效地降低了里程計的漂移，而單個像素稀疏集合上的直接光度誤差有效地限制了計算負載。此外，基于光度誤差，VIO能夠在線估計相機曝光時間，從而能夠恢復(fù)環(huán)境的真實輻射信息；

在定位精度、魯棒性和輻射圖重建精度方面，論文的方法在真實世界的實驗中得到了廣泛驗證。來自開放數(shù)據(jù)集（NCLT數(shù)據(jù)集）的25個序列的基準測試結(jié)果表明，相比之下，R3LIVE++在所有其他最先進的SLAM系統(tǒng)（如LVI-SAM、LIO-SAM、FAST-LIO2等）中實現(xiàn)了新SOTA。

此外，論文在私有數(shù)據(jù)集的評估表明，R3LIVE++對于激光雷達或相機測量退化的極端挑戰(zhàn)場景（例如，當設(shè)備面對無紋理的墻時）具有更好的魯棒性。最后，與其他同類產(chǎn)品相比，R3LIVE++可以更準確地估計相機的曝光時間，并重建環(huán)境的真實輻射信息，與圖像中的測量像素相比，誤差要小得多；

據(jù)論文稱，這是第一個輻射圖重建框架，它可以在配備標準CPU的PC上實現(xiàn)實時性能，而無需任何硬件或GPU加速?；谶M一步開發(fā)的一套網(wǎng)格重建和紋理處理離線應(yīng)用程序，該系統(tǒng)在各種現(xiàn)實應(yīng)用中顯示出巨大潛力，例如3D HDR成像、物理模擬和視頻游戲。

方法

論文使用的符號含義如下表所示：

系統(tǒng)概覽

為了同時估計傳感器姿態(tài)并重建環(huán)境輻射圖，論文設(shè)計了一個緊密耦合的LiDAR慣性視覺傳感器融合框架，如圖2所示。該框架包含兩個子系統(tǒng)：LIO和VIO子系統(tǒng)。LIO子系統(tǒng)通過記錄每個輸入LiDAR的點云測量值，構(gòu)建輻射圖的幾何結(jié)構(gòu)。

VIO子系統(tǒng)分兩步恢復(fù)地圖的輻射信息：幀到幀VIO更新通過最小化幀到幀PnP重投影誤差來估計系統(tǒng)狀態(tài)，而幀到地圖VIO更新則最小化地圖點云和當前圖像之間的輻射誤差。這兩個子系統(tǒng)在流形誤差狀態(tài)迭代卡爾曼濾波器框架（ESIKF）[43]內(nèi)緊密耦合，其中激光雷達和攝像機視覺測量在各自的數(shù)據(jù)接收時間融合到相同的系統(tǒng)狀態(tài)。

點

本文的輻射圖由全局框架中的地圖點組成，每個點P的結(jié)構(gòu)如下所示：

體素

為了有效地查詢輻射圖中的點，論文將地圖點設(shè)置為固定大?。?.1m×0.1m×0.1m）的體素。如果一個體素近期附加了點（例如最近1秒），則將該體素標記為激活。否則，此體素將標記為停用。

彩色相機光度模型

相機以圖像的形式觀察現(xiàn)實世界的輻射，圖像由像素強度的2D數(shù)組組成。在本文中基于[44]對相機的圖像形成過程進行建模，并將灰色相機模型進一步擴展到彩色相機。如圖3所示，對于現(xiàn)實世界中的點P，它反射來自光源（例如太陽）的入射光。反射光通過相機鏡頭，最終到達CMOS傳感器，傳感器記錄反射光的強度，并在輸出圖像中創(chuàng)建像素通道。記錄的強度由輻射度決定，輻射度是點P反射功率的測量值。



為了建模上述成像過程，使用表示點P的輻射度。由于彩色相機的CMOS傳感器中有三個通道：紅色、綠色和藍色，因此輻射度有三個分量：、、 。對于每個通道，穿過相機鏡頭的光線有如下能量：



拍攝圖像時，捕獲的輻射度隨時間（即曝光時間）進行積分。然后，通過相機響應(yīng)函數(shù)將累積輻射度轉(zhuǎn)換為像素強度輸出：



由于真實的相機傳感器具有有限的動態(tài)范圍，并且輻射的物理尺度無法恢復(fù)，因此可以將像素強度歸一化到0-1之間。 不同通道通常具有不同的非線性響應(yīng)函數(shù)，它們可以根據(jù)[45]中的方法與vignetting factor一起離線校準。本文的曝光時間是在線估計的。根據(jù)校準和估計結(jié)果，點P相對于觀測像素值的輻射度可計算為：

狀態(tài)

最終狀態(tài)x定義如下：

激光雷達慣性里程計（LIO）

論文的LIO子系統(tǒng)通過將新的LiDAR點云配準到全局地圖來重建環(huán)境的幾何結(jié)構(gòu)。即使用generalized-iterative closet point（GICP）方法[46]，通過最小化掃描中每個點云到地圖中相應(yīng)點擬合的平面的距離，迭代估計LiDAR姿態(tài)（和其他系統(tǒng)狀態(tài)）。然后使用估計的狀態(tài)估計值將新點附加到地圖。

LiDAR point-to-plane residual

如圖2所示，論文的LIO子系統(tǒng)構(gòu)建了全局地圖的幾何結(jié)構(gòu)。對于第k個輸入LiDAR掃描，首先使用在[22]中引入的IMU反向傳播來補償幀內(nèi)運動。假設(shè)是運動補償后m個LiDAR點的集合，計算的每個原始點（或下采樣子集）的殘差，其中s是點的索引，上標L表示點在LiDAR參考幀中的表示。 基于當前迭代的估計，將從LiDAR幀轉(zhuǎn)換到全局幀：



為了將點云配準到全局地圖，論文搜索地圖中最近的五個點。為了加速最近鄰搜索，地圖點被組織成incremental k-d tree（參見[23]）。使用找到的最近鄰點擬合一個具有法向和質(zhì)心的平面，然后使用激光雷達測量殘差：

LIO ESIKF update

理想情況下，上式中的殘差應(yīng)為零。然而，由于中的估計誤差和LiDAR測量噪聲，該殘差通常不是零，可以用于細化狀態(tài)估計。具體來說，假設(shè)是點的測量噪聲，則真值和測量值的關(guān)系如下所示：



該真實點位置和真實狀態(tài)可以推導(dǎo)出下式：



上式構(gòu)成的觀測分布，它與IMU傳播的先驗分布相結(jié)合得到：



求解上式得到的Maximum A-Posteriori（MAP）估計值，然后更新到中，如下所示：



迭代直至收斂（即更新小于給定閾值）。然后將收斂狀態(tài)估計值用作IMU傳播的起點，直到接收到下一個LiDAR掃描或相機圖像。此外，收斂估計值用于將當前LiDAR掃描中的點添加到全局地圖中，如下所示。對于第s個點，其在全局幀中的位置首先通過（6）獲得。如果在地圖中有距離為1cm的附近點，將被丟棄，以保持1cm的空間分辨率。否則，將在地圖中創(chuàng)建一個新的點結(jié)構(gòu)：

視覺慣性里程計(VIO)

當LIO子系統(tǒng)重建環(huán)境的幾何結(jié)構(gòu)時，VIO子系統(tǒng)從輸入的彩色圖像中恢復(fù)輻射信息。更具體地說，VIO子系統(tǒng)從全局地圖向當前圖像投影一定數(shù)量的點（即跟蹤點），然后通過最小化這些點的輻射誤差來迭代估計相機姿態(tài)（和其他系統(tǒng)狀態(tài)）。為了提高計算效率，只使用一組稀疏的跟蹤地圖點。

論文提出的框架不同于以前基于光度的方法[33]、[47]，這些方法通過考慮所有相鄰像素（即patch）的光度誤差來構(gòu)成點的殘差。這些基于patch的方法具有更強的魯棒性和更快的收斂速度，然而對平移或旋轉(zhuǎn)都不是不變的，即需要在兩個patch對齊時估計相對變換。

此外，這些方法假設(shè)patch中所有像素的深度與中心點相同，所以殘差的計算并不精確。另一方面，論文的VIO在逐像素上操作，利用單個地圖點的輻射來計算殘差。在VIO中同時更新的輻射度是世界上某個點的固有屬性，對攝影機平移和旋轉(zhuǎn)都是不變的。

為了確保穩(wěn)健和快速收斂，論文設(shè)計了圖2所示的兩步流水線，其中在第一步（即 frame-to-frame VIO）中，利用frame-to-frame光流跟蹤最后一幀中觀察到的地圖點，并通過最小化跟蹤點的PnP重投影誤差來獲得系統(tǒng)狀態(tài)的粗略估計。然后，在第二步（即frame-to-map VIO）中，通過最小化地圖點的亮度和當前圖像中其投影位置的像素強度之間的差異，進一步細化狀態(tài)估計。

通過收斂狀態(tài)估計和原始輸入圖像，論文最終根據(jù)當前圖像測量更新地圖點的輻射度。

光度矯正

對于輸入圖像，首先校正圖像non-linear CRF 和vignetting factor ，其已經(jīng)預(yù)先校準，以獲得測光校正圖像，其像素位置的第i通道為：



測光校正圖像 之后在VIO管道中使用，包括frame-to-frame VIO、frame-to-map VIO和輻射恢復(fù)。

Frame-to-frame VIO

Perspective-n-Point reprojection error

假設(shè)跟蹤了m個在最后一幀圖像中的地圖點及其在圖像中的投影位置。論文使用Lucas Kanade光流來定位這些點在當前圖像中的位置。然后迭代地最小化點P的重投影誤差，以獲得狀態(tài)的粗略估計（見圖4）。



投影誤差表示如下：

Frame-to-frame VIO Update

與LIO更新類似，中的狀態(tài)估計誤差和相機測量噪聲將導(dǎo)致（14）中的某個殘差，從中可以如下更新狀態(tài)估計。首先（14）中的測量噪聲由兩個來源組成：一個是的像素跟蹤誤差，另一個是地圖點定位誤差：



類似的，校正此類噪聲并使用真實的系統(tǒng)狀態(tài)應(yīng)導(dǎo)致零殘差：



上式構(gòu)成了的觀測分布，將其與與IMU傳播相結(jié)合，以獲得狀態(tài)的MAP估計值，方法與LIO更新相同。然后收斂的狀態(tài)估計值用于細化frame-to-map VIO。

Frame-to-map VIO

Frame-to-map radiance error

frame-to-frame VIO更新可以提供良好的狀態(tài)估計，通過最小化跟蹤的地圖點P的輻射誤差，frame-to-map VIO更新會進一步細化。利用當前迭代的狀態(tài)估計（包含估計的相機姿態(tài)、內(nèi)外參和曝光時間），進一步將跟蹤的地圖點投影到圖像平面，以獲得其像素位置。然后，由表示的觀測輻射度可以由下式獲得：



最后，frame-to-map輻射誤差由下式獲得：

Frame-to-map VIO update

上式中的測量噪聲來自輻射分量和：






結(jié)合（19）、（20）和（21）進一步得到了真實零殘差的一階泰勒展開式：



與之前類似，（22）構(gòu)成了狀態(tài)的觀測分布，該分布與IMU傳播相結(jié)合，以獲得該狀態(tài)的MAP估計值。

Recovery of radiance information

在frame-to-map VIO更新后，便得到了當前圖像的精確姿態(tài)。然后進行貝葉斯更新以確定所有地圖點的最佳輻射度，從而使每個點與其對應(yīng)圖像之間的平均輻射度誤差最小。 進一步檢索所有激活體素中的點。假設(shè)檢索到的點集為Q。對于落在當前圖像FoV中的第s個點，首先通過（19）獲取觀測到的輻射向量，通過（21）獲得其協(xié)方差。然后，如果是LIO子系統(tǒng)附加的一個新點且，則我們設(shè)置：



否則，通過貝葉斯更新（見圖6），將保存在地圖中的輻射矢量與新觀測到的輻射矢量進行融合：

Update of the tracking points

在恢復(fù)輻射信息后，論文更新跟蹤點集P以供下一幀圖像使用。首先，如果（14）中的投影誤差或（19）中的輻射誤差過大，就從當前P中刪除點，并刪除不屬于當前圖像FoV的點。其次，論文將Q中的每個點投影到當前圖像中，如果在50像素的鄰域中沒有其他跟蹤點，則將其添加到P中。

實驗

論文進行了大量實驗，從三個方面驗證了提出的系統(tǒng)相對于其他同類系統(tǒng)的優(yōu)勢：

1）為了驗證定位的準確性，在公開數(shù)據(jù)集（NCLT數(shù)據(jù)集）上定量比較了R3LIVE++與其他SOTA的SLAM系統(tǒng)；

2） 為了驗證框架的魯棒性，在攝像機和激光雷達傳感器退化的各種具有挑戰(zhàn)性的場景下進行測試；

3） 為了評估R3LIVE++在重建輻射圖時的精度，論文將其與現(xiàn)有基線進行比較，以估算相機曝光時間并計算每個圖像的平均光度誤差。在實驗中，使用了兩個數(shù)據(jù)集進行評估：NCLT數(shù)據(jù)集和私有的R3LIVE數(shù)據(jù)集。

R3LIVE數(shù)據(jù)集收集于香港大學(xué)（HKU）和香港科技大學(xué)（HKUST）的校園內(nèi)。如表2所示，數(shù)據(jù)集包含13個序列，通過探索室內(nèi)和室外環(huán)境，在一天中不同時間（即上午、中午和晚上）的不同場景（如人行道、公園、森林等）中收集這些序列。這使得數(shù)據(jù)集能夠捕獲具有不同照明條件下的結(jié)構(gòu)化城市建筑和雜亂現(xiàn)場環(huán)境。

數(shù)據(jù)集還包括三個退化序列，其中激光雷達或相機（或兩者）偶爾面對設(shè)備，使其退化為單個視覺或無紋理平面（例如，墻、地面）?？傂谐?.4km，持續(xù)時間達到2.4h。

定位準確性評估

論文將R3LIVE++與其他SLAM系統(tǒng)進行對比，包括LIO-SAM[19]、LVI-SAM[40]、FAST-LIO2[23]和R2LIVE[41]。 對比結(jié)果如下表所示：


在圖10中，論文對比疊加了所有25條地面真實軌跡（左圖）和R3LIVE的結(jié)果（右圖）。可以看出，R3LIVE++系統(tǒng)估計的疊加軌跡與GT吻合得很好，并且每個軌跡仍然可以清楚地區(qū)分，沒有明顯的誤差。請注意，這25條軌跡是在同一校園區(qū)域內(nèi)一天中的不同時間和一年中的不同季節(jié)采集的，但R3LIVE++仍然可以生成一致且可靠的軌跡估計，證明了系統(tǒng)的魯棒性。

魯棒性評估

LiDAR退化場景下的魯棒性評估：可以看出，由于面對單個平面時激光雷達退化，所有三個LIO系統(tǒng)都失敗了，沒有返回起點。相比之下，通過利用視覺圖像中的線索，R3LIVE++在這兩個序列中都能很好地工作，并成功地回到起點。

LiDAR退化和視覺紋理較少環(huán)境下的魯棒性評估：在這個實驗中，R3LIVE++挑戰(zhàn)了SLAM中最困難的場景之一，激光雷達和相機都會退化。傳感器裝置穿過一個狹窄的“T”形通道（見圖11）



利用原始像素顏色信息，并將其與LiDAR點云測量緊密融合，R3LIVE++可以在這種極其困難的場景中“生存”。圖12是姿態(tài)估計的結(jié)果，穿過“墻-1”和“墻-2”的階段分別用紫色和黃色陰影表示。估計協(xié)方差也如圖12所示，它在整個估計軌跡上有界，表明R3LIVE++的估計質(zhì)量在整個過程中是穩(wěn)定的。

輻射圖重建評估

由于無法測量環(huán)境的真實輻射圖，論文使用兩個指標來評估精度：一個是相機曝光時間的估計質(zhì)量，另一個是重建的輻射圖與測量圖像之間的平均光度誤差。

曝光時間估計：在這個實驗中，論文通過將估計的相機曝光時間與從相機API讀取的GT進行比較來評估其準確性。結(jié)果如圖13和表4所示。



輻射圖評估：為了評估輻射重建誤差，在地圖重建后使用估計的相機姿態(tài)和校準的光度參數(shù)，將地圖中所有具有輻射信息的點重新投影到每個圖像幀。然后計算地圖點顏色和圖像在投影像素位置的RGB值之間的光度誤差。計算每個圖像幀的平均光度誤差，以評估重建的輻射圖精度。實驗結(jié)果如圖14和表5所示：


可以看出，R3LIVE++在所有序列中始終實現(xiàn)了最低的光度誤差，其次是R3LIVE。圖15顯示了重建輻射圖的一些特寫鏡頭，從中可以清楚地分辨物體上的單詞。此外，圖16展示了R3LIVE數(shù)據(jù)集中“香港大學(xué)主樓”序列的重建輻射圖，室內(nèi)和室外細節(jié)（例如道路上的標記）都非常清晰，表明R3LIVE++具有較高的精度。

運行時間分析

本節(jié)分析SLAM系統(tǒng)在純CPU PC（配備Intel i7-9700K CPU和64GB RAM）上的平均耗時。論文計算了兩個數(shù)據(jù)集所有序列的平均耗時，結(jié)果如表6和表7所示。在NCLT數(shù)據(jù)集上，每次LiDAR掃描需要34.3 ms，相機需要16.6 ms的處理時間。

R3LIVE++的應(yīng)用

High dynamic range (HDR) imaging

在重建輻射圖后，可以進一步渲染圖像，可視化結(jié)果如圖17所示：

Mesh reconstruction and texturing

當R3LIVE++實時重建彩色3D地圖時，論文還開發(fā)了軟件程序來離線對重建的地圖進行網(wǎng)格劃分和紋理處理（見圖18）。

Toward various of 3D applications

通過開發(fā)的軟件，論文將將重建的3D地圖導(dǎo)出到Unreal Engine[60]，以支持一系列3D應(yīng)用程序。如圖19所示的汽車模擬器、視頻游戲等

結(jié)論和未來工作

結(jié)論

論文提出了一種新的激光雷達慣性視覺融合框架R3LIVE++，以實現(xiàn)魯棒和精確的狀態(tài)估計，同時動態(tài)重建輻射圖。該框架由兩個子系統(tǒng)（即LIO和VIO）組成，這兩個子系統(tǒng)共同增量地實時構(gòu)建環(huán)境的3D輻射圖。通過將三種不同類型傳感器的測量值緊密融合，R3LIVE++可以實現(xiàn)更高的定位精度，同時對傳感器退化的場景具有足夠的魯棒性。

論文通過真實世界的實驗，在定位精度、魯棒性和輻射圖重建精度方面驗證了R3LIVE++的有效性。在NCLT數(shù)據(jù)集上的實驗結(jié)果表明，R3LIVE++超越了以往SOTA的SLAM系統(tǒng)。對私有數(shù)據(jù)集（R3LIVE數(shù)據(jù)集）的評估表明，R3LIVE++對LiDAR或相機測量退化的極端挑戰(zhàn)性場景具有魯棒性。最后與其他算法相比，R3LIVE++可以更準確地估計相機的曝光時間，并重建環(huán)境的真實輻射信息，與圖像中的測量值相比，誤差要小得多。

為了證明R3LIVE++的可擴展性，論文基于重建的輻射圖開發(fā)了幾個應(yīng)用程序，例如高動態(tài)范圍（HDR）成像、虛擬環(huán)境探索和3D視頻游戲。

未來工作

在R3LIVE++中，輻射圖是用包含輻射信息的3D點云重建的，因此無法從輻射圖中渲染高分辨率圖像，因為輻射圖的點云密度有限（論文設(shè)置為1cm）。雖然可以進一步增加點云密度，但也會進一步增加耗時。此外點云密度還受到激光雷達傳感器測量的原始點云密度的限制。我們注意到圖像通常具有更高的分辨率，未來可以探索如何在融合框架中充分利用此類高分辨率圖像。

審核編輯：劉清