概述

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

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

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

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

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

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

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

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

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

圖2 彩色圖像成像過(guò)程

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

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

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

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

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

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

a.雷達(dá)慣性里程計(jì) (LIO)

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

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

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

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

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

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

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

圖3 輻射度的貝葉斯更新

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

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

c.實(shí)驗(yàn)

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

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

2) 為了驗(yàn)證我們框架的魯棒性，我們?cè)诎l(fā)生相機(jī)和激光雷達(dá)傳感器退化的各種具有挑戰(zhàn)性的場(chǎng)景下對(duì)其進(jìn)行了測(cè)試。

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

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

i.輻射圖重建的評(píng)估

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

審核編輯：郭婷