真正實(shí)時(shí)地圖更新！GS-LIVO：首個(gè)可部署在資源受限嵌入式系統(tǒng)上的實(shí)時(shí)高斯SLAM框架
本文經(jīng)3D視覺之心公眾號授權(quán)轉(zhuǎn)載，轉(zhuǎn)載請聯(lián)系出處。
實(shí)時(shí)地圖更新
近年來，隨著同步定位與建圖（SLAM）技術(shù)的發(fā)展，出現(xiàn)了多種顯式地圖表示方法，包括稠密彩色點(diǎn)云、稀疏基于塊的結(jié)構(gòu)，以及基于網(wǎng)格或基于曲面元素（surfel）的重建。這些形式通常與基于特征的方法或直接方法相結(jié)合，在無人機(jī)和移動機(jī)器人等平臺上支持高效、實(shí)時(shí)操作。許多最先進(jìn)的SLAM系統(tǒng)利用這些傳統(tǒng)地圖結(jié)構(gòu)，得益于其成熟的技術(shù)流程和在位姿估計(jì)任務(wù)中的穩(wěn)健性能。
然而，這些人工設(shè)計(jì)的顯式表示方法盡管已高度成熟，但仍存在一些局限性：
- 它們通常依賴于豐富的幾何特征和高幀率的輸入以確保跟蹤的穩(wěn)定性。
- 此外，這些方法在提供逼真的重建和解釋場景中未觀測部分時(shí)往往力不從心。在需要預(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ì)量的提升，提供更好的空間和外觀線索可以提高位姿估計(jì)的精度和穩(wěn)健性，從而強(qiáng)化SLAM中建圖與定位之間的相互關(guān)系。然而，當(dāng)前基于神經(jīng)網(wǎng)絡(luò)的SLAM系統(tǒng)面臨一個(gè)關(guān)鍵瓶頸：無法實(shí)現(xiàn)真正的實(shí)時(shí)地圖更新。盡管某些方法可以接近實(shí)時(shí)的里程計(jì)計(jì)算，其地圖優(yōu)化和細(xì)化過程往往滯后，依賴于單獨(dú)的、較慢的線程。這種位姿估計(jì)的速度與較慢的地圖更新之間的不匹配降低了系統(tǒng)的適應(yīng)性，尤其是在需要持續(xù)更新高保真地圖的動態(tài)或大規(guī)模環(huán)境中。因此，這些高質(zhì)量表示在機(jī)器人領(lǐng)域的實(shí)際部署仍然有限，而快速的環(huán)境解釋至關(guān)重要。
GS-LIVO[1]是一種激光雷達(dá)-慣性-視覺里程計(jì)系統(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)鍵瓶頸。
項(xiàng)目主頁：https://github.com/HKUST-Aerial-Robotics/GS-LIVO
主要貢獻(xiàn)包括：
- 引入了一種基于空間哈希索引八叉樹的全局高斯地圖表示結(jié)構(gòu)。該層次結(jié)構(gòu)支持高效的全局索引、共視性檢查，并本質(zhì)上支持可變的細(xì)節(jié)層次以適應(yīng)大規(guī)模場景。
- 提出了基于激光雷達(dá)與視覺數(shù)據(jù)的快速初始化策略，以快速生成結(jié)構(gòu)良好的全局高斯地圖，并實(shí)現(xiàn)高保真渲染。
- 提出了一種增量式滑動窗口高斯維護(hù)方法，優(yōu)化高斯地圖時(shí)能夠有效降低計(jì)算成本和GPU內(nèi)存負(fù)擔(dān)。
- 提出了一種利用高斯地圖的光度渲染能力的視覺測量模型，通過IESKF實(shí)現(xiàn)激光雷達(dá)-慣性測量的緊密集成。
具體方法
GS-LIVO系統(tǒng)的架構(gòu)如圖2所示。硬件配置整合了激光雷達(dá)（LiDAR）、慣性測量單元（IMU）和攝像頭數(shù)據(jù)，并通過模擬的每秒脈沖信號（PPS）實(shí)現(xiàn)了精確的時(shí)間同步。軟件框架由以下四個(gè)關(guān)鍵模塊組成：
- 全局高斯地圖：采用空間哈希索引的八叉樹組織形式，能夠有效覆蓋稀疏空間體積，同時(shí)適應(yīng)各種環(huán)境細(xì)節(jié)和規(guī)模。
- 基于光度梯度的高斯初始化與在線優(yōu)化：利用激光雷達(dá)與視覺信息實(shí)現(xiàn)高效的高斯初始化和優(yōu)化。
- 滑動窗口高斯增量維護(hù)：通過局部高斯滑動窗口進(jìn)行實(shí)時(shí)優(yōu)化，顯著減少圖形內(nèi)存使用。
- 基于IESKF的位姿優(yōu)化：采用序列更新的迭代誤差狀態(tài)卡爾曼濾波器（IESKF）實(shí)現(xiàn)位姿優(yōu)化。
本系統(tǒng)是一個(gè)無縫集成激光雷達(dá)、慣性測量單元和視覺傳感器的實(shí)時(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)主要由兩個(gè)部分組成：全局高斯地圖和局部高斯滑動窗口。全局高斯地圖采用哈希索引的八叉樹結(jié)構(gòu)，通過空間哈希技術(shù)高效地覆蓋場景中的稀疏空間體積。該結(jié)構(gòu)能夠根據(jù)環(huán)境復(fù)雜度遞歸劃分區(qū)域，從而實(shí)現(xiàn)更加細(xì)致的地圖表示。根體素（root voxel）的索引由以下公式計(jì)算：
其中，(v_s) 是根體素的長度。此方法允許對稀疏分布的數(shù)據(jù)點(diǎn)進(jìn)行高效管理和索引，尤其適用于大規(guī)模環(huán)境。
在激光雷達(dá)-慣性-視覺系統(tǒng)中，我們利用當(dāng)前幀的激光雷達(dá)數(shù)據(jù)計(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è)計(jì)了一種專門的高斯滑動窗口，用于維護(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í)，這些參數(shù)會從主存?zhèn)鬏數(shù)揭曨l存儲器，允許GPU高效地執(zhí)行并行優(yōu)化。優(yōu)化完成后，更新后的高斯參數(shù)會從視頻存儲器傳回主存，以保持全局地圖的一致性。
與傳統(tǒng)的有限且不可擴(kuò)展的圖形內(nèi)存不同，主存具有更大的容量，并可以通過交換空間進(jìn)一步擴(kuò)展，從而能夠處理更大、更復(fù)雜的場景。這種設(shè)計(jì)不僅提高了GPU處理的效率，還確保系統(tǒng)在處理大規(guī)模數(shù)據(jù)集時(shí)的穩(wěn)定性和可靠性，如圖3所示。
高斯的初始化與優(yōu)化
當(dāng)接收到新的激光雷達(dá)和攝像頭幀時(shí)，系統(tǒng)會對稠密的激光雷達(dá)點(diǎn)云進(jìn)行基于體素的地圖下采樣，以減少GPU內(nèi)存消耗。與使用邊界選擇的方法不同，本文采用八叉樹的葉節(jié)點(diǎn)體素對物體表面的3D空間進(jìn)行采樣。通過選擇每個(gè)體素內(nèi)的激光雷達(dá)點(diǎn)，場景得以更高效地表示。
如果葉體素未被填充，則會基于激光雷達(dá)和攝像頭粗略地初始化新的高斯參數(shù)，并將其插入葉體素中。
激光雷達(dá)-攝像頭聯(lián)合初始化高斯參數(shù)
在該步驟中，高斯的結(jié)構(gòu)參數(shù)通過激光雷達(dá)數(shù)據(jù)初始化。具體而言，高斯的縮放矩陣S根據(jù)哈希體素的層級進(jìn)行初始化，其表達(dá)式如下：
滑動窗口中高斯的實(shí)時(shí)優(yōu)化
高斯滑動窗口的維護(hù)
為了提高內(nèi)存效率并優(yōu)化計(jì)算速度，本系統(tǒng)將優(yōu)化范圍限制在滑動窗口內(nèi)的高斯。這種方法能夠顯著提高優(yōu)化速度并減少內(nèi)存消耗，同時(shí)避免傳統(tǒng)3D高斯方法中基于圖塊深度排序的混疊效應(yīng)（aliasing effects），確保前景和背景元素之間的清晰分離，從而提升深度表示的質(zhì)量。
滑動窗口增量更新
從全局高斯地圖中重新構(gòu)建每一幀的高斯滑動窗口需要大量的內(nèi)存拷貝，導(dǎo)致顯著的計(jì)算開銷。然而，連續(xù)幀之間通常存在較大的場景重疊，使得大部分工作是重復(fù)的。為了利用幀間的重疊，本系統(tǒng)提出了一種增量更新策略，用于高斯滑動窗口的維護(hù)。該方法顯著減少了冗余內(nèi)存?zhèn)鬏敚嵘藢?shí)時(shí)性能，并使系統(tǒng)能夠更有效地?cái)U(kuò)展至大規(guī)模和復(fù)雜環(huán)境。
滑動窗口維護(hù)步驟
高斯滑動窗口的維護(hù)包括以下關(guān)鍵步驟：
- 全局地圖更新
確定上一幀滑動窗口中的高斯體素是否仍在當(dāng)前視場（FoV）內(nèi)。如果體素超出視場，其優(yōu)化參數(shù)將被復(fù)制回全局高斯地圖并標(biāo)記為“刪除”。 - 刪除和壓縮
將標(biāo)記為“刪除”的體素參數(shù)與滑動窗口序列末尾的參數(shù)交換。重新定位后，從序列末尾移除這些體素，從而保持內(nèi)存的連續(xù)性。 - 重疊和添加
利用當(dāng)前激光雷達(dá)幀的空間哈希鍵，識別與上一幀滑動窗口重疊的體素，并確定需要添加到當(dāng)前視場滑動窗口的新區(qū)域。 - 附加新葉體素
將新增體素附加到CPU高斯緩沖區(qū)（CGB）的末尾，并更新空間哈希表（SHT）。隨后，將高斯數(shù)據(jù)從CGB（主存儲器）直接傳輸至GPU高斯緩沖區(qū)（GGB），確保滑動窗口中的高斯能夠被GPU立即優(yōu)化和渲染。 - 優(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)存中的一個(gè)連續(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)存操作
僅更新視場變化的體素，而非重新加載整個(gè)滑動窗口，減少了冗余操作。 - 降低計(jì)算消耗
將優(yōu)化范圍限制在當(dāng)前視場內(nèi)的高斯體素，從而降低計(jì)算成本并提升實(shí)時(shí)性能。 - 擴(kuò)展性
利用CPU內(nèi)存的可擴(kuò)展性和容量支持，更高效地處理大規(guī)模和復(fù)雜場景。
性能優(yōu)勢
通過滑動窗口維護(hù)策略，系統(tǒng)顯著減少了GPU內(nèi)存消耗，同時(shí)保持高映射質(zhì)量和優(yōu)化效率。結(jié)果顯示，總處理時(shí)間（包括滑動窗口維護(hù)和高斯優(yōu)化）在室內(nèi)和室外場景中始終低于100毫秒，支持每秒10幀的實(shí)時(shí)更新。這種高效設(shè)計(jì)使得本系統(tǒng)能夠適應(yīng)各種計(jì)算資源和場景復(fù)雜性。
狀態(tài)估計(jì)
基于高斯渲染的特點(diǎn)，本文重新設(shè)計(jì)了FAST-LIVO2的視覺更新流程。與將當(dāng)前幀的圖像塊映射到參考幀以計(jì)算光度誤差的方法不同，本文通過比較由高斯地圖渲染的圖像與實(shí)際圖像，統(tǒng)一地在當(dāng)前幀上計(jì)算光度誤差。這種優(yōu)化的收斂性由高斯渲染的平滑性和可微分性保證，類似于MonoGS中的實(shí)現(xiàn)。
如圖2所示，本文的里程計(jì)系統(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估計(jì)器中IMU位姿更新的雅可比矩陣推導(dǎo)如下：
從上述推導(dǎo)可知，優(yōu)化更新不僅包含相機(jī)位姿的變化，還系統(tǒng)性地?cái)U(kuò)展至IMU位姿，并集成到IESKF框架中：
緊耦合系統(tǒng)的優(yōu)勢
大多數(shù)基于高斯點(diǎn)的SLAM方法主要依賴優(yōu)化器計(jì)算相機(jī)位姿更新，通常不會評估更新位姿的協(xié)方差。然而，本文的方法通過將位姿和協(xié)方差進(jìn)一步傳播到下一個(gè)傳感器更新（例如IMU和激光雷達(dá)），形成了一個(gè)緊耦合的IESKF系統(tǒng)。這種方法增強(qiáng)了系統(tǒng)在動態(tài)和復(fù)雜環(huán)境中的一致性和穩(wěn)健性。
實(shí)驗(yàn)效果
總結(jié)一下
GS-LIVO是一種結(jié)合傳統(tǒng)激光雷達(dá)-慣性-視覺里程計(jì)與3D高斯點(diǎn)新型地圖表示的實(shí)時(shí)SLAM系統(tǒng)。通過用基于高斯的場景表示替代傳統(tǒng)的彩色點(diǎn)云和稀疏塊地圖，本系統(tǒng)實(shí)現(xiàn)了高精度定位與高保真地圖建模。
主要貢獻(xiàn)包括：
- 用于高效全局高斯地圖管理的空間哈希索引八叉樹結(jié)構(gòu)
- 通過激光雷達(dá)-視覺聯(lián)合初始化實(shí)現(xiàn)高保真建圖
- 用于實(shí)時(shí)地圖優(yōu)化的增量滑動窗口策略
- 基于IESKF的緊耦合多傳感器融合框架。
盡管現(xiàn)有的基于高斯的SLAM系統(tǒng)通常能夠?qū)崿F(xiàn)實(shí)時(shí)定位，但在實(shí)時(shí)地圖更新方面存在不足，我們的系統(tǒng)利用傳統(tǒng)多傳感器融合的優(yōu)勢，維持了高頻率的地圖更新，同時(shí)實(shí)現(xiàn)了緊耦合的里程計(jì)。
值得注意的是，GS-LIVO是首個(gè)成功部署在NVIDIA Jetson Orin NX嵌入式平臺上的基于高斯的SLAM系統(tǒng)，展現(xiàn)了其在實(shí)際機(jī)器人應(yīng)用中的潛力。大量實(shí)驗(yàn)表明，GS-LIVO在室內(nèi)和室外環(huán)境中均表現(xiàn)出優(yōu)異性能，相較現(xiàn)有方法減少了內(nèi)存消耗和優(yōu)化時(shí)間，同時(shí)保持了高渲染質(zhì)量。
未來研究:
- 可進(jìn)一步探索自適應(yīng)細(xì)節(jié)層次技術(shù)，這些技術(shù)可考慮視距、結(jié)構(gòu)復(fù)雜性及紋理豐富度。
- 此外，對于均質(zhì)區(qū)域，合并顏色相近的高斯點(diǎn)可以進(jìn)一步優(yōu)化內(nèi)存使用和計(jì)算效率。然而，這些高級體素管理機(jī)制會增加額外復(fù)雜性，需要仔細(xì)研究。
