本文經(jīng)3D視覺之心公眾號授權(quán)轉(zhuǎn)載，轉(zhuǎn)載請聯(lián)系出處。

實(shí)時地圖更新

近年來，隨著同步定位與建圖（SLAM）技術(shù)的發(fā)展，出現(xiàn)了多種顯式地圖表示方法，包括稠密彩色點(diǎn)云、稀疏基于塊的結(jié)構(gòu)，以及基于網(wǎng)格或基于曲面元素（surfel）的重建。這些形式通常與基于特征的方法或直接方法相結(jié)合，在無人機(jī)和移動機(jī)器人等平臺上支持高效、實(shí)時操作。許多最先進(jìn)的SLAM系統(tǒng)利用這些傳統(tǒng)地圖結(jié)構(gòu)，得益于其成熟的技術(shù)流程和在位姿估計任務(wù)中的穩(wěn)健性能。

然而，這些人工設(shè)計的顯式表示方法盡管已高度成熟，但仍存在一些局限性：

• 它們通常依賴于豐富的幾何特征和高幀率的輸入以確保跟蹤的穩(wěn)定性。
• 此外，這些方法在提供逼真的重建和解釋場景中未觀測部分時往往力不從心。在需要預(yù)測或合成新視點(diǎn)的應(yīng)用中（例如沉浸式增強(qiáng)現(xiàn)實(shí)、高質(zhì)量的3D建模
• 需要推斷未見區(qū)域以實(shí)現(xiàn)穩(wěn)健決策的場景）

最近，在新視點(diǎn)合成領(lǐng)域的突破引入了神經(jīng)表示，可以從任意視點(diǎn)進(jìn)行真實(shí)感渲染。隱式模型（如Neural Radiance Fields, NeRF）和顯式結(jié)構(gòu)（如3D Gaussian Splatting, 3DGS）不僅豐富了重建環(huán)境的細(xì)節(jié)，還為更先進(jìn)的SLAM范式打開了大門。一些基于NeRF的SLAM方法和基于3DGS的方法試圖將這些高保真表示集成到SLAM流程中，旨在利用更豐富的光度和幾何信息以實(shí)現(xiàn)更精確的定位和建圖。

直觀地說，隨著地圖質(zhì)量的提升，提供更好的空間和外觀線索可以提高位姿估計的精度和穩(wěn)健性，從而強(qiáng)化SLAM中建圖與定位之間的相互關(guān)系。然而，當(dāng)前基于神經(jīng)網(wǎng)絡(luò)的SLAM系統(tǒng)面臨一個關(guān)鍵瓶頸：無法實(shí)現(xiàn)真正的實(shí)時地圖更新。盡管某些方法可以接近實(shí)時的里程計計算，其地圖優(yōu)化和細(xì)化過程往往滯后，依賴于單獨(dú)的、較慢的線程。這種位姿估計的速度與較慢的地圖更新之間的不匹配降低了系統(tǒng)的適應(yīng)性，尤其是在需要持續(xù)更新高保真地圖的動態(tài)或大規(guī)模環(huán)境中。因此，這些高質(zhì)量表示在機(jī)器人領(lǐng)域的實(shí)際部署仍然有限，而快速的環(huán)境解釋至關(guān)重要。

GS-LIVO[1]是一種激光雷達(dá)-慣性-視覺里程計系統(tǒng)，該系統(tǒng)通過高斯地圖的新穎表示形式，緊密結(jié)合了激光雷達(dá)與攝像頭的測量。本文的目標(biāo)包括：通過高斯地圖內(nèi)的多傳感器融合實(shí)現(xiàn)高精度定位，以及在大規(guī)模和復(fù)雜環(huán)境中顯著提高高斯地圖更新的效率。本方法解決了當(dāng)前高斯SLAM系統(tǒng)實(shí)際部署中的關(guān)鍵瓶頸。

項目主頁：https://github.com/HKUST-Aerial-Robotics/GS-LIVO

主要貢獻(xiàn)包括：

1. 引入了一種基于空間哈希索引八叉樹的全局高斯地圖表示結(jié)構(gòu)。該層次結(jié)構(gòu)支持高效的全局索引、共視性檢查，并本質(zhì)上支持可變的細(xì)節(jié)層次以適應(yīng)大規(guī)模場景。
2. 提出了基于激光雷達(dá)與視覺數(shù)據(jù)的快速初始化策略，以快速生成結(jié)構(gòu)良好的全局高斯地圖，并實(shí)現(xiàn)高保真渲染。
3. 提出了一種增量式滑動窗口高斯維護(hù)方法，優(yōu)化高斯地圖時能夠有效降低計算成本和GPU內(nèi)存負(fù)擔(dān)。
4. 提出了一種利用高斯地圖的光度渲染能力的視覺測量模型，通過IESKF實(shí)現(xiàn)激光雷達(dá)-慣性測量的緊密集成。

具體方法

GS-LIVO系統(tǒng)的架構(gòu)如圖2所示。硬件配置整合了激光雷達(dá)（LiDAR）、慣性測量單元（IMU）和攝像頭數(shù)據(jù)，并通過模擬的每秒脈沖信號（PPS）實(shí)現(xiàn)了精確的時間同步。軟件框架由以下四個關(guān)鍵模塊組成：

1. 全局高斯地圖：采用空間哈希索引的八叉樹組織形式，能夠有效覆蓋稀疏空間體積，同時適應(yīng)各種環(huán)境細(xì)節(jié)和規(guī)模。
2. 基于光度梯度的高斯初始化與在線優(yōu)化：利用激光雷達(dá)與視覺信息實(shí)現(xiàn)高效的高斯初始化和優(yōu)化。
3. 滑動窗口高斯增量維護(hù)：通過局部高斯滑動窗口進(jìn)行實(shí)時優(yōu)化，顯著減少圖形內(nèi)存使用。
4. 基于IESKF的位姿優(yōu)化：采用序列更新的迭代誤差狀態(tài)卡爾曼濾波器（IESKF）實(shí)現(xiàn)位姿優(yōu)化。

本系統(tǒng)是一個無縫集成激光雷達(dá)、慣性測量單元和視覺傳感器的實(shí)時SLAM框架，在競爭性的定位精度下實(shí)現(xiàn)了高效運(yùn)行。為方便描述，本節(jié)首先定義了相關(guān)符號及其解釋（如表I），包括坐標(biāo)變換、高斯屬性、空間結(jié)構(gòu)及優(yōu)化參數(shù)。

全局高斯地圖：哈希索引八叉樹

圖3展示了本文系統(tǒng)中全局地圖和高斯滑動窗口的結(jié)構(gòu)。映射系統(tǒng)主要由兩個部分組成：全局高斯地圖和局部高斯滑動窗口。全局高斯地圖采用哈希索引的八叉樹結(jié)構(gòu)，通過空間哈希技術(shù)高效地覆蓋場景中的稀疏空間體積。該結(jié)構(gòu)能夠根據(jù)環(huán)境復(fù)雜度遞歸劃分區(qū)域，從而實(shí)現(xiàn)更加細(xì)致的地圖表示。根體素（root voxel）的索引由以下公式計算：

其中，(v_s) 是根體素的長度。此方法允許對稀疏分布的數(shù)據(jù)點(diǎn)進(jìn)行高效管理和索引，尤其適用于大規(guī)模環(huán)境。

在激光雷達(dá)-慣性-視覺系統(tǒng)中，我們利用當(dāng)前幀的激光雷達(dá)數(shù)據(jù)計算空間鍵，從而高效地識別全局高斯地圖中與當(dāng)前視場（FoV）對應(yīng)的根體素。這種方法能夠在空間環(huán)境中識別共視區(qū)域，確定哪些區(qū)域在當(dāng)前視點(diǎn)下可見，從而為后續(xù)處理提供準(zhǔn)確的地圖信息。然而，由于哈希索引存儲的不連續(xù)性，直接在GPU上對高斯參數(shù)進(jìn)行并行優(yōu)化存在局限性。不連續(xù)的內(nèi)存布局會導(dǎo)致GPU訪問和處理效率的降低，引入不必要的延遲。

為了解決這一問題，我們設(shè)計了一種專門的高斯滑動窗口，用于維護(hù)視場范圍內(nèi)的高斯數(shù)據(jù)，并采用連續(xù)的內(nèi)存布局以確保GPU對高斯參數(shù)的高效處理。具體而言，大規(guī)模環(huán)境中的所有高斯數(shù)據(jù)以非連續(xù)的哈希八叉樹結(jié)構(gòu)存儲在主存（RAM）中，而僅視場范圍內(nèi)的高斯數(shù)據(jù)被存儲在連續(xù)的主存區(qū)域中，并在視頻隨機(jī)存儲器（VRAM）中保留副本。當(dāng)需要優(yōu)化高斯參數(shù)時，這些參數(shù)會從主存?zhèn)鬏數(shù)揭曨l存儲器，允許GPU高效地執(zhí)行并行優(yōu)化。優(yōu)化完成后，更新后的高斯參數(shù)會從視頻存儲器傳回主存，以保持全局地圖的一致性。

與傳統(tǒng)的有限且不可擴(kuò)展的圖形內(nèi)存不同，主存具有更大的容量，并可以通過交換空間進(jìn)一步擴(kuò)展，從而能夠處理更大、更復(fù)雜的場景。這種設(shè)計不僅提高了GPU處理的效率，還確保系統(tǒng)在處理大規(guī)模數(shù)據(jù)集時的穩(wěn)定性和可靠性，如圖3所示。

高斯的初始化與優(yōu)化

當(dāng)接收到新的激光雷達(dá)和攝像頭幀時，系統(tǒng)會對稠密的激光雷達(dá)點(diǎn)云進(jìn)行基于體素的地圖下采樣，以減少GPU內(nèi)存消耗。與使用邊界選擇的方法不同，本文采用八叉樹的葉節(jié)點(diǎn)體素對物體表面的3D空間進(jìn)行采樣。通過選擇每個體素內(nèi)的激光雷達(dá)點(diǎn)，場景得以更高效地表示。

如果葉體素未被填充，則會基于激光雷達(dá)和攝像頭粗略地初始化新的高斯參數(shù)，并將其插入葉體素中。

激光雷達(dá)-攝像頭聯(lián)合初始化高斯參數(shù)

在該步驟中，高斯的結(jié)構(gòu)參數(shù)通過激光雷達(dá)數(shù)據(jù)初始化。具體而言，高斯的縮放矩陣S根據(jù)哈希體素的層級進(jìn)行初始化，其表達(dá)式如下：

滑動窗口中高斯的實(shí)時優(yōu)化

高斯滑動窗口的維護(hù)

為了提高內(nèi)存效率并優(yōu)化計算速度，本系統(tǒng)將優(yōu)化范圍限制在滑動窗口內(nèi)的高斯。這種方法能夠顯著提高優(yōu)化速度并減少內(nèi)存消耗，同時避免傳統(tǒng)3D高斯方法中基于圖塊深度排序的混疊效應(yīng)（aliasing effects），確保前景和背景元素之間的清晰分離，從而提升深度表示的質(zhì)量。

滑動窗口增量更新

從全局高斯地圖中重新構(gòu)建每一幀的高斯滑動窗口需要大量的內(nèi)存拷貝，導(dǎo)致顯著的計算開銷。然而，連續(xù)幀之間通常存在較大的場景重疊，使得大部分工作是重復(fù)的。為了利用幀間的重疊，本系統(tǒng)提出了一種增量更新策略，用于高斯滑動窗口的維護(hù)。該方法顯著減少了冗余內(nèi)存?zhèn)鬏?，提升了?shí)時性能，并使系統(tǒng)能夠更有效地擴(kuò)展至大規(guī)模和復(fù)雜環(huán)境。

滑動窗口維護(hù)步驟

高斯滑動窗口的維護(hù)包括以下關(guān)鍵步驟：

1. 全局地圖更新
   確定上一幀滑動窗口中的高斯體素是否仍在當(dāng)前視場（FoV）內(nèi)。如果體素超出視場，其優(yōu)化參數(shù)將被復(fù)制回全局高斯地圖并標(biāo)記為“刪除”。
2. 刪除和壓縮
   將標(biāo)記為“刪除”的體素參數(shù)與滑動窗口序列末尾的參數(shù)交換。重新定位后，從序列末尾移除這些體素，從而保持內(nèi)存的連續(xù)性。
3. 重疊和添加
   利用當(dāng)前激光雷達(dá)幀的空間哈希鍵，識別與上一幀滑動窗口重疊的體素，并確定需要添加到當(dāng)前視場滑動窗口的新區(qū)域。
4. 附加新葉體素
   將新增體素附加到CPU高斯緩沖區(qū)（CGB）的末尾，并更新空間哈希表（SHT）。隨后，將高斯數(shù)據(jù)從CGB（主存儲器）直接傳輸至GPU高斯緩沖區(qū)（GGB），確?；瑒哟翱谥械母咚鼓軌虮籊PU立即優(yōu)化和渲染。
5. 優(yōu)化與渲染
   在滑動窗口內(nèi)的高斯體素會通過GPU高效優(yōu)化，并在優(yōu)化完成后將更新的參數(shù)從GGB傳回CGB，以保持全局地圖的一致性。

系統(tǒng)架構(gòu)和內(nèi)存布局

高斯滑動窗口的架構(gòu)包括以下主要組件：

• 空間哈希表（SHT）
  基于哈希的索引結(jié)構(gòu)，用于將空間坐標(biāo)映射到CPU內(nèi)存中的高斯參數(shù)指針。
• CPU高斯緩沖區(qū)（CGB）
  CPU內(nèi)存中的一個連續(xù)存儲區(qū)域，用于存儲當(dāng)前活動體素的高斯參數(shù)。這種緊湊的布局使數(shù)據(jù)能夠快速傳輸至GPU。
• GPU高斯緩沖區(qū)（GGB）
  GPU上分配的內(nèi)存塊，用于并行處理和快速渲染，可直接訪問高斯數(shù)據(jù)。

優(yōu)化性能與內(nèi)存使用

增量維護(hù)高斯滑動窗口的策略通過以下方式提升了系統(tǒng)性能：

• 減少內(nèi)存操作
  僅更新視場變化的體素，而非重新加載整個滑動窗口，減少了冗余操作。
• 降低計算消耗
  將優(yōu)化范圍限制在當(dāng)前視場內(nèi)的高斯體素，從而降低計算成本并提升實(shí)時性能。
• 擴(kuò)展性
  利用CPU內(nèi)存的可擴(kuò)展性和容量支持，更高效地處理大規(guī)模和復(fù)雜場景。

性能優(yōu)勢

通過滑動窗口維護(hù)策略，系統(tǒng)顯著減少了GPU內(nèi)存消耗，同時保持高映射質(zhì)量和優(yōu)化效率。結(jié)果顯示，總處理時間（包括滑動窗口維護(hù)和高斯優(yōu)化）在室內(nèi)和室外場景中始終低于100毫秒，支持每秒10幀的實(shí)時更新。這種高效設(shè)計使得本系統(tǒng)能夠適應(yīng)各種計算資源和場景復(fù)雜性。

狀態(tài)估計

基于高斯渲染的特點(diǎn)，本文重新設(shè)計了FAST-LIVO2的視覺更新流程。與將當(dāng)前幀的圖像塊映射到參考幀以計算光度誤差的方法不同，本文通過比較由高斯地圖渲染的圖像與實(shí)際圖像，統(tǒng)一地在當(dāng)前幀上計算光度誤差。這種優(yōu)化的收斂性由高斯渲染的平滑性和可微分性保證，類似于MonoGS中的實(shí)現(xiàn)。

如圖2所示，本文的里程計系統(tǒng)通過序列更新的迭代誤差狀態(tài)卡爾曼濾波器（IESKF），緊密地集成了激光雷達(dá)和圖像測量。與傳統(tǒng)的稀疏視覺地圖不同，本文使用稠密高斯地圖作為視覺模塊的基礎(chǔ)。

系統(tǒng)工作流程

首先，本文利用當(dāng)前視場內(nèi)優(yōu)化后的高斯地圖，通過激光雷達(dá)-慣性更新后的位姿進(jìn)行新視點(diǎn)的渲染。與FAST-LIVO2方法不同，本文的高斯地圖不僅能夠無縫渲染，還能以真實(shí)感質(zhì)量渲染非朗伯表面，體現(xiàn)了基于高斯方法的優(yōu)勢。

雅可比矩陣推導(dǎo)

高斯渲染的光度損失對IESKF估計器中IMU位姿更新的雅可比矩陣推導(dǎo)如下：

從上述推導(dǎo)可知，優(yōu)化更新不僅包含相機(jī)位姿的變化，還系統(tǒng)性地擴(kuò)展至IMU位姿，并集成到IESKF框架中：

緊耦合系統(tǒng)的優(yōu)勢

大多數(shù)基于高斯點(diǎn)的SLAM方法主要依賴優(yōu)化器計算相機(jī)位姿更新，通常不會評估更新位姿的協(xié)方差。然而，本文的方法通過將位姿和協(xié)方差進(jìn)一步傳播到下一個傳感器更新（例如IMU和激光雷達(dá)），形成了一個緊耦合的IESKF系統(tǒng)。這種方法增強(qiáng)了系統(tǒng)在動態(tài)和復(fù)雜環(huán)境中的一致性和穩(wěn)健性。

實(shí)驗效果

總結(jié)一下

GS-LIVO是一種結(jié)合傳統(tǒng)激光雷達(dá)-慣性-視覺里程計與3D高斯點(diǎn)新型地圖表示的實(shí)時SLAM系統(tǒng)。通過用基于高斯的場景表示替代傳統(tǒng)的彩色點(diǎn)云和稀疏塊地圖，本系統(tǒng)實(shí)現(xiàn)了高精度定位與高保真地圖建模。

主要貢獻(xiàn)包括：

1. 用于高效全局高斯地圖管理的空間哈希索引八叉樹結(jié)構(gòu)
2. 通過激光雷達(dá)-視覺聯(lián)合初始化實(shí)現(xiàn)高保真建圖
3. 用于實(shí)時地圖優(yōu)化的增量滑動窗口策略
4. 基于IESKF的緊耦合多傳感器融合框架。

盡管現(xiàn)有的基于高斯的SLAM系統(tǒng)通常能夠?qū)崿F(xiàn)實(shí)時定位，但在實(shí)時地圖更新方面存在不足，我們的系統(tǒng)利用傳統(tǒng)多傳感器融合的優(yōu)勢，維持了高頻率的地圖更新，同時實(shí)現(xiàn)了緊耦合的里程計。

值得注意的是，GS-LIVO是首個成功部署在NVIDIA Jetson Orin NX嵌入式平臺上的基于高斯的SLAM系統(tǒng)，展現(xiàn)了其在實(shí)際機(jī)器人應(yīng)用中的潛力。大量實(shí)驗表明，GS-LIVO在室內(nèi)和室外環(huán)境中均表現(xiàn)出優(yōu)異性能，相較現(xiàn)有方法減少了內(nèi)存消耗和優(yōu)化時間，同時保持了高渲染質(zhì)量。

未來研究:

• 可進(jìn)一步探索自適應(yīng)細(xì)節(jié)層次技術(shù)，這些技術(shù)可考慮視距、結(jié)構(gòu)復(fù)雜性及紋理豐富度。
• 此外，對于均質(zhì)區(qū)域，合并顏色相近的高斯點(diǎn)可以進(jìn)一步優(yōu)化內(nèi)存使用和計算效率。然而，這些高級體素管理機(jī)制會增加額外復(fù)雜性，需要仔細(xì)研究。