FOCI: Trajectory Optimization on Gaussian Splats

Abstract

3D Gaussian Splatting (3DGS) has recently gained popularity as a faster alternative to Neural Radiance Fields (NeRFs) in 3D reconstruction and view synthesis methods. Leveraging the spatial information encoded in 3DGS, this work proposes FOCI (Field Overlap Collision Integral), an algorithm that is able to optimize trajectories directly on the Gaussians themselves. FOCI leverages a novel and interpretable collision formulation for 3DGS using the notion of the overlap integral between Gaussians. Contrary to other approaches, which represent the robot with conservative bounding boxes that underestimate the traversability of the environment, we propose to represent the environment and the robot as Gaussian Splats. This not only has desirable computational properties, but also allows for orientation-aware planning, allowing the robot to pass through very tight and narrow spaces. We extensively test our algorithm in both synthetic and real Gaussian Splats, showcasing that collision-free trajectories for the ANYmal legged robot that can be computed in a few seconds, even with hundreds of thousands of Gaussians making up the environment.

We present three main contributions to the state-of-the-art:

A novel collision measure between Gaussian Splats based on the overlap integral between Gaussians.
A trajectory planning algorithm that uses this formulation to optimize fully differentiable trajectories.
A GPU implementation of the trajectory optimization.

Comparisons

We compare the performance of our method with similar Splat-based planners:

	Splat-Nav	GS-Planner	GSM	GaussNav	FOCI (ours)
Planning pipeline included
Orientation-aware collision
Covariance information leveraged

Planned Trajectories

We evaluate the method on a variety of environments including both synthetic and real-world scenes. For these comparisons we model the robot as 3 Gaussians with the green Gaussian representing the head, the red the center, and the blue being the tail of the robot.

Synthetic Environments

Realistic Environments

BibTeX

@article{andreuwildersmith2025foci,
        author        = {Mario Gomez Andreu and Maximum Wilder-Smith and Victor Klemm and Vaishakh Patil and Jesus Tordesillas and Marco Hutter},
        title         = {FOCI: Trajectory Optimization on Gaussian Splats},
        year          = {2025},
        eprint        = {2505.08510},
        archivePrefix = {arXiv},
        primaryClass  = {cs.RO},
        url           = {https://arxiv.org/abs/2505.08510}
}