概述

分享一篇多傳感融合定位的工作：R3live++。這是繼R3live后的第二個版本。這項工作提出的激光雷達視覺慣性融合框架實現(xiàn)了魯棒和準確的狀態(tài)估計，同時實時重建光度地圖。

系統(tǒng)由激光雷達慣性里程計 (LIO) 和視覺慣性里程計 (VIO) 組成，兩者都是實時運行的。LIO子系統(tǒng)利用來自激光雷達的測量值來重建幾何結(jié)構(gòu)，而VIO子系統(tǒng)同時從輸入圖像中恢復(fù)幾何結(jié)構(gòu)的光度信息。通過考慮相機光度校準和相機曝光時間的在線估計，進一步提高了定位和映射的準確性。在公共和私有數(shù)據(jù)集上進行了更廣泛的實驗，將提出的系統(tǒng)與其他最先進的SLAM系統(tǒng)進行比較。定量和定性結(jié)果表明，R3LIVE++在準確性和魯棒性方面均具有顯著提高。

此外，為了演示R3LIVE的可擴展性，我們基于重建的地圖開發(fā)了幾種應(yīng)用程序，例如高動態(tài)范圍 (HDR) 成像，虛擬環(huán)境探索和3D視頻游戲。代碼是開源的，文末附源碼和文章鏈接。

圖1 R3live++系統(tǒng)結(jié)構(gòu)

為了同時估計傳感器姿態(tài)并重建環(huán)境輻射圖，設(shè)計了一個緊密耦合的LiDAR-慣性-視覺傳感器融合框架，如圖1所示。

所提出的框架包含兩個子系統(tǒng): LIO子系統(tǒng) (上部) 和VIO子系統(tǒng)。LIO子系統(tǒng)通過配準每個輸入激光雷達掃描的點云測量來構(gòu)造輻射圖的幾何結(jié)構(gòu)。VIO子系統(tǒng)通過兩個步驟恢復(fù)地圖的輻射信息: 幀到幀VIO更新通過最小化幀到幀PnP重投影誤差來估計系統(tǒng)狀態(tài)，而幀到地圖VIO更新最小化地圖點和當(dāng)前圖像之間的光度誤差。

這兩個子系統(tǒng)在流形錯誤狀態(tài)迭代卡爾曼濾波器框架 (ESIKF) 內(nèi)緊密耦合，其中激光雷達和攝像機視覺測量在其各自的數(shù)據(jù)接收時間融合到相同的系統(tǒng)狀態(tài) 。

我們的輻射圖由全局框架中的地圖點組成，每個點P是如下結(jié)構(gòu):

其中前三維表示點3D位置，并且后三維是由三個獨立通道 (即紅色，綠色和藍色通道)，用于相機光度校準。此外，我們還記錄該點的其他必要信息，包括點的估計誤差的協(xié)方差，以及創(chuàng)建和更新該點時的時間戳。

圖2 彩色圖像成像過程

相機以圖像的形式觀察現(xiàn)實世界的輻射，該圖像由像素強度的2D陣列組成。對相機的圖像形成過程進行建模，并進一步將灰色相機模型擴展到彩色相機。如圖2所示，對于世界上的點P，它反射從光源 (例如，太陽) 發(fā)出的入射光。然后，反射光穿過相機鏡頭，最終到達CMOS傳感器，該傳感器記錄反射光的強度，并在輸出圖像中創(chuàng)建一個像素通道。記錄的強度由輻射度確定，輻射度是在點P處反射的功率的度量。

為了模擬上述成像過程，P處的輻射度表示為 r。由于彩色相機的CMOS傳感器具有三個通道: 紅色，綠色和藍色，對于每個通道 i，穿過相機鏡頭的光線都有功率。?

其中V(p)被稱為解釋透鏡漸暈效應(yīng)的漸暈因子。由于漸暈效果在透鏡的不同區(qū)域處是不同的，因此漸暈因子V(p) 是像素位置 p可以接收的功率量，稱為輻照度。當(dāng)拍攝圖像時，捕獲的輻照度O(p) 隨時間 (即曝光時間r) 積分。累積輻照度通過相機響應(yīng)函數(shù) (CRF) ，轉(zhuǎn)換為像素強度Ij(p) 的輸出:

曝光時間 r 在我們的工作中是在線估計的。校準和估計結(jié)果,從觀察到的像素值 I (P) 得出的點 p 的輻射度可以計算為:

在我們的工作中，我們將完整狀態(tài)x定義為:

其中符號 Itc是IMU和相機之間的時間偏移，而LiDAR被假定已經(jīng)與IMU同步。相機外參和時間偏移 tc 通常具有可用的粗略值 。外參或時間延遲包括在狀態(tài)x中，以便在線估計它們。此外，我們還在線估算相機曝光時間，以恢復(fù)每個地圖點的真實輻射值。

a.雷達慣性里程計 (LIO)

LIO 子系統(tǒng)通過將每個新的 LiDAR 掃描注冊到全局地圖來重建環(huán)境的幾何結(jié)構(gòu)。使用廣義迭代最近點 (GICP) 方法,通過最小化掃描中每一點到從地圖中相應(yīng)點擬合的平面的距離,迭代估計激光雷達姿態(tài) (和其他系統(tǒng)狀態(tài))。然后使用估計的狀態(tài)估計將新點附加到地圖上。激光雷達點對平面殘差可以寫為：

想情況下，殘差應(yīng)該為零。但是，由于X中的估計誤差和激光雷達測量噪聲，該殘差通常不為零，我們使用ESIKF細化狀態(tài)估計X。

b.視覺慣性子系統(tǒng)VIO

LIO 子系統(tǒng)重建環(huán)境的幾何結(jié)構(gòu)時,我們的 VIO 子系統(tǒng)從輸入的彩色圖像中恢復(fù)輻射信息。更具體地說,我們的 VIO子系統(tǒng)將一定數(shù)量的點 (即,跟蹤點) 從全局地圖投影到當(dāng)前圖像,然后通過最小化這些點的輻射誤差來迭代估計相機姿勢 (和其他系統(tǒng)狀態(tài))。

為了提高計算效率,僅使用一組稀疏的跟蹤地圖點。我們提出的框架不同于以前的基于光度的方法 ,后者通過考慮所有鄰域像素 (即補丁) 的光度誤差來構(gòu)成點的殘差。這些基于補丁的方法比沒有補丁的方法具有更強的魯棒性和更快的收斂速度。但是,基于補丁的方法對于平移或旋轉(zhuǎn)都不是不變的,這需要在將一個補丁與另一個補丁對齊時估計相對變換。另外,通過假設(shè)補丁中所有像素的深度與中點相同,殘差的計算并不完全精確。

另一方面,我們的 VIO 在單個像素上運行,該像素利用單個地圖點的輻射來計算殘差。在 VIO 中同時更新的輪胎輻射度是世界上一個點的固有屬性,并且對于相機的平移和旋轉(zhuǎn)都是不變的。為了確保穩(wěn)健和快速的收斂,我們設(shè)計了一個兩步流水線,如圖所示,其中在第一步 (即,幀到幀 VIO) 我們利用幀到幀光流來跟蹤最后一幀中觀察到的地圖點,并通過最小化被跟蹤點的透視 n 點 (PnP) 重投影誤差來獲得系統(tǒng)狀態(tài)的粗略估計 。

然后,在第二步驟 (即幀到地圖 VIO) 中,通過最小化地圖點的輻射度與當(dāng)前圖像中它們的投影位置處的像素強度之間的差來進一步細化狀態(tài)估計 。利用收斂狀態(tài)估計和原始輸入圖像,我們最終根據(jù)當(dāng)前圖像測量更新地圖點的輻射度 。

圖3 輻射度的貝葉斯更新

i.輻射信息的恢復(fù)

點到地圖VIO更新后，我們將獲得當(dāng)前圖像的精確姿勢。然后，我們執(zhí)行貝葉斯更新以確定所有地圖點的最佳輻射度，以使每個點與其查看圖像之間的平均輻射度誤差最小。見圖3。

c.實驗

在本章中，我們進行了廣泛的實驗，以驗證我們提出的系統(tǒng)相對于其他同行的優(yōu)勢有三個方面:

1) 為了驗證本地化的準確性，我們將系統(tǒng)與公共數(shù)據(jù)集 (NCLT) 上現(xiàn)有的最先進的SLAM系統(tǒng)進行了定量比較。

2) 為了驗證我們框架的魯棒性，我們在發(fā)生相機和激光雷達傳感器退化的各種具有挑戰(zhàn)性的場景下對其進行了測試。

3) 為了評估我們的系統(tǒng)在重建輻射圖方面的準確性，我們將其與現(xiàn)有的基線進行比較，以估算相機曝光時間并計算平均光度誤差。

圖4 激光雷達退化環(huán)境中的測試

i.輻射圖重建的評估

在這個實驗中，我們評估了我們提出的算法在重建輻射圖方面的準確性。由于無法測量環(huán)境的地面真實輻照度圖，因此我們根據(jù)兩個指標評估精度: 一是相機曝光時間的估計質(zhì)量，二是重建的輻照度圖與測量圖像之間的平均光度誤差。

審核編輯：郭婷