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.
Autonomous robots in unknown indoor environments require both reliable collision avoidance and object-level understanding. Classical representations such as TSDF support safe planning but lack semantics, while photorealistic methods like Gaussian Splatting (GS) provide rich appearance yet suffer from soft geometry, limiting precise obstacle avoidance. We present LiftNav, a hybrid navigation framework built on GSFusion's TSDF+GS dual map, augmented with a real-time pipeline of YOLO-based detection, TSDF-based 3D lifting, and B-spline trajectory optimization. This design enables flexible semantic navigation without dense 3D embeddings. We further introduce a hinge-loss-based collision penalty that improves trajectory smoothness and safety. We evaluate our approach in a simulation using the Replica dataset. Compared against a state-of-the-art radiance field baseline we show a 100% feasibility rate and shorter trajectories.
Hannah Schieber, Dominik Frischmann, Victor Schaack +2
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)
Image-goal navigation requires a global planner that connects visually specified endpoints while providing observations that a local policy can follow. Three-dimensional Gaussian Splatting (3DGS) provides map-registered RGB, opacity, and depth renderings, but does not explicitly encode navigable topology. We introduce GS-PRM, a Gaussian visual roadmap whose milestones, local connections, and visual references are constructed directly through 3DGS rendering without deriving an occupancy grid or signed distance field. We define render-based collision-free (RC-free) configurations and paths. Panoramic depth supplies conservative clearance for milestone validation and adaptive coverage, while edge-aligned orthographic rendering tests the robot's swept volume. We establish conditional soundness and sufficient conditions for deterministic query completeness and probabilistic construction completeness of GS-PRM. To validate the performance of GS-PRM, we conduct experiments in both simulated and real-world environments, demonstrating long-horizon navigation using goal-conditioned policies guided by GS-PRM. Compared with a state-of-the-art 3DGS-based navigation system, our GS-PRM-based navigation system achieves up to a twice higher average success rate and reduces travel time by up to 57.2% when both systems complete the navigation task.
Existing 3D Gaussian Splatting (3DGS) systems distribute representation capacity uniformly across a scene, ignoring the fact that many downstream robotic tasks engage only a fraction of the reconstructed geometry. This causes valuable onboard compute to be allocated towards optimizing irrelevant parts of the scene, either limiting online capacity or under-optimizing the most relevant parts of the scene. We introduce GaussLite, a task-driven 3DGS mapping system that conditions its representation density on a natural-language task specification. Given a posed RGB-D stream and a task such as "prepare to pick up the object on the desk," GaussLite uses a one-shot LLM parser to extract target and anchor objects, which are grounded per-frame by an open-vocabulary detector and segmented to produce per-pixel relevance masks in real time. The mapper allocates seeding density, gradient flow and scaling by task relevance. At matched Gaussian budget and real-time mapping at 4 Hz on resource-constrained hardware, GaussLite outperforms baselines on ROI PSNR on the Replica Dataset by an average +2.72 dB and on a real-hardware demonstration in indoor and outdoor settings by +2.23 dB. We further show that two task-specialized agents' maps can be fused into a single shared map via per-voxel voting on active-optimization counts in real time, outperforming concatenation by +3.42 dB while only sharing an average 7.08% of the map.