GOFusion: Real-Time Gaussian-Occupancy Submap Fusion for Fisher-Informed Autonomous Robot Navigation
Authors: Boxin Bu, Simon Boche, Haoming Zhang, Jaehyung Jung, Jiaxin Wei, Hannah Schieber, Daniel Roth, Angela P. Schoellig, Stefan Leutenegger
Organizations: Mobile Robotics Lab, Technical University of Munich and ETH Zurich. · Learning Systems & Robotics Lab, Technical University of Munich and Institute of Robotics and Machine Intelligence (MIRMI). · Human-Centered Computing and Extended Reality Lab, TUM University Hospital, Clinic for Orthopedics and Sports Orthopedics, Munich Institute of Robotics and Machine Intelligence (MIRMI). · University of Toronto Institute for Aerospace Studies, University of Toronto Robotics Institute, and Vector Institute for Artificial Intelligence, and Robotics Institute Germany.
In this letter, we present GOFusion, a real-time visual mapping system for autonomous robot navigation that anchors Gaussian primitives to local occupancy maps, providing both spatial awareness for path planning and high-fidelity appearance that enables information-driven robot navigation. GOFusion builds 3D Gaussian maps from pose estimates and local occupancy maps obtained via visual-inertial SLAM and enables pose corrections to maintain both geometric and visual consistency. Furthermore, it restricts the 3D Gaussian Splatting optimization to active submaps to bound GPU memory usage. Building on the appearance-rich Gaussian map, we introduce a rasterizer-integrated Fisher approximation that quantifies per-primitive sensitivity online, serving as an information-driven heuristic for Gaussian pruning and next-best-view planning. We evaluate GOFusion on public datasets and in real-world drone flights. It not only outperforms several baselines in mapping quality, but also achieves real-time, high-fidelity and uncertainty-aware mapping at scale. Finally, we demonstrate fully autonomous drone exploration for Gaussian map refinement using the proposed Fisher-informed heuristic. Code is available at https://github.com/Bennnoo/GOFusion. We provide a supplementary video demonstration at https://youtu.be/pLQKbxQvb6I.
Figures & tables
Fig. 1: Benchmark result on the uHumans2 dataset [ 8 ] . Left: finalized map of GOFusion , presenting different submaps. Right: performance summary of GOFusion on rendering quality (PSNR/SSIM/LPIPS), trajectory accuracy ( ATE−1 ), speed (Full-FPS), and GPU-memory efficiency (VRAM eff.).
Fig. 2: System overview of GOFusion . OKVIS2-X [ 11 ] processes stereo RGB, IMU, and depth to estimate poses and build local-occupancy maps. Each occupancy map guides image-adaptive, voxel-weighted, and normal-aligned Gaussian initialization. Online Gaussian optimization then refines the active submap and provides gradient buffers to compute the accumulated Fisher approximation, which can be used for redundant Gaussian pruning or next-best-view selection. Our system can run entirely onboard or split between onboard and offboard processes, depending on the available compute.
Fig. 3: Comparison of quadtree-based image decomposition on [ 29 ] . Left: decomposition in [ 9 ] . Right: our dual-threshold decomposition. Red regions highlight additional subdivisions that capture the image details effectively.
Method
PSNR ↑
SSIM ↑
LPIPS ↓
Mapp. FPS ↑
#Gauss. ( 106 ) ↓
VRAM (GB) ↓
GSFusion [ 9 ]
20.23
0.708
0.357
18.17
0.10
2.1
RTG-SLAM [ 17 ]
18.47
0.708
0.470
10.64
0.31
2.4
GPS-SLAM [ 10 ]
18.78
0.652
0.393
103.7
0.51
8.8
PhotoSLAM [ 18 ]
21.30
0.748
0.205
34.57
0.11
1.2
GS-ICP SLAM [ 19 ]
18.92
0.688
0.304
30.02
1.47
2.2
SplaTAM [ 15 ]
19.45
0.801
0.336
2.40
1.91
3.9
Table I: Benchmark of rendering quality and map compactness on TUM-RGBD [ 31 ] with ground-truth poses, averaged over 10 runs. All methods re-evaluated on full images, not cited from the original papers. (+D/N) denotes explicit depth and normal supervision in the 3DGS objective. Best and second-best in bold and underlined.
Seq.
Metric
RTG- SLAM [ 17 ]
GPS- SLAM [ 10 ]
Photo- SLAM [ 18 ]
GS-ICP SLAM [ 19 ]
LoopSplat [ 22 ]
DiskChun -GS [ 23 ]
Ours
Ours (+D/N)
Apart. 49.4 m
PSNR ↑
26.04
14.79
20.27
23.67
25.24
29.15
33.09
31.96
SSIM ↑
0.894
0.284
0.730
0.816
0.828
0.866
0.942
0.928
LPIPS ↓
0.330
0.383
0.392
0.335
0.324
0.110
0.062
0.076
Full-FPS ↑
9.66
42.35
57.13
10.62
0.59
10.50
35.23
30.54
ATE ↓
0.007
0.140
0.139
1.581
0.046
0.016
0.015
0.014
#Gauss. ( 106 ) ↓
0.14
0.35
0.31
2.61
4.50
0.18
0.69
0.69
Table II: Benchmark of rendering quality and SLAM performance on uHumans2 [ 8 ] , averaged over 10 runs (median for ATE). (+D/N) denotes explicit depth/normal supervision in the 3DGS optimization objective. Best and second-best results are bold and underlined. OOM: out-of-memory.
Method
PSNR ↑
SSIM ↑
LPIPS ↓
#Gauss. ( 106 ) ↓
VRAM (GB) ↓
Offline
GSFusion [ 9 ]
24.23
0.701
0.443
0.40
7.7
RTG-SLAM [ 17 ]
15.85
0.478
0.693
0.46
8.6
GPS-SLAM [ 10 ]
7.19
0.125
0.970
0.31
14.3
PhotoSLAM [ 18 ]
23.35
0.642
0.524
0.36
6.1
GS-ICP SLAM [ 19 ]
22.36
0.758
0.267
2.92
17.5
LoopSplat [ 22 ]
CPU OOM
Table III: Benchmark of rendering quality and map compactness on the real-world flight sequences using estimated poses, averaged over 16 sequences. (+D/N) denotes explicit depth and normal supervision in the 3DGS objective. Best and second-best in bold and underlined.
Metric
Single Map
Multiple Submaps
w/o. Joint Opt.
w. Joint Opt.
w/o. Joint Opt.
w. Joint Opt.
psnr ↑
25.01
25.06
29.68
29.92
ssim ↑
0.774
0.775
0.898
0.903
lpips ↓
0.274
0.269
0.096
0.091
ate ↓
0.150
0.124
0.134
0.134
Full-FPS ↑
17.95
17.97
19.60
19.72
Table IV: Ablation of map format and joint Gaussian-pose optimization on uHumans2 [ 8 ] , averaged over 10 independent runs across all sequences. Best and second-best results are bold and underlined.
Fig. 4: Qualitative novel-view comparison of 3DGS optimization without (left) and with (right) depth and normal supervision on uHumans2 [ 8 ] .
Fig. 5: Comparison of three Gaussian pruning strategies evaluated on the live pre-global map with online SLAM poses on uHumans2 [ 8 ] .
Fig. 6: Real-world autonomous drone flight with two next-best-view candidates. Camera frustums (light blue and green) indicate the planned next-best-views, directed toward regions with low Fisher entries (warm color). The yellow frame marks the planner’s geofence, keeping the drone out of cluttered areas during autonomous flights.
Fig. 7: Map before (left) and after (right) visiting the next-best-view during the flight. The rendered RGBs are from the same candidate pose. Warmer colors in the Fisher map indicate higher relative uncertainty.
Technical University of Munich, Human-Centered Computing and Extended Reality Lab, TUM University Hospital, Clinic for Orthopedics and Sports Orthopedics, Munich Institute of Robotics and Machine Intelligence (MIRMI) · Technical University of Munich, Learning Systems and Robotics Lab, Munich Institute of Robotics and Machine Intelligence (MIRMI)