TRACE: Ergodic Trajectory Optimization for Active Scene Reconstruction

Background and Motivation

Active 3D reconstruction recovers the geometry of an unknown scene by moving a sensor along a sequence of informative viewpoints, incrementally building a high-fidelity model from streaming observations—a core problem in robotic perception. Recent advances in 3D Gaussian Splatting (3DGS) and its surface-aligned variant 2DGS have dramatically raised achievable reconstruction quality, shifting the bottleneck from map representation to the planner: under finite budgets of time, energy, and motion, the agent must decide how to spend each meter of motion across an unknown scene.

The dominant paradigm is view-by-view planning: at each step, the planner scores candidate viewpoints by an information-gain proxy, commits to the highest-scoring pose, and connects it to the current location via short-horizon path planning. This greedy decoupling has three fundamental problems.

First, viewpoint scoring is short-term: each decision commits to the locally most informative pose without anticipating where the sensor must subsequently travel. Second, motion between selected views does not contribute to information gathering: trajectories are jagged, redundant, and biased toward isolated high-information viewpoints rather than uniform spatial coverage. Third, the trajectory formed by planned views ignores real-world constraints—controllability, actuator limits, energy efficiency—often producing dynamically infeasible trajectories. Discrete search algorithms optimize over viewpoints rather than executable motions and cannot directly account for the robot's continuous-time dynamics.

TRACE proposes the first active 3D reconstruction method that plans continuous, control-feasible trajectories using ergodic search. The core idea: instead of modeling the problem as planning over a set of next poses, directly optimize a trajectory over the information distribution derived from the current map state—so the trajectory's time-averaged spatial statistics match a target distribution: time spent in any region is proportional to its information density.

Preliminaries

Understanding TRACE requires several key concepts. First, ergodic coverage: given a target distribution $\phi$, ergodic search makes the trajectory's time-averaged spatial statistics match $\phi$—dwelling longer and sampling more densely in high-information regions. Second, 2D Gaussian Splatting (2DGS): a surface-aligned variant of 3DGS that represents scene surfaces with 2D disk Gaussians, directly supporting mesh-quality evaluation. Third, kernel-ergodic metric: a kernel-function-based ergodic measure consisting of an information term (coverage of high-$\phi$ regions) and a self-correlation term (discouraging waypoint clustering, encouraging uniform coverage).

Another key concept is hybrid map representation: a 2DGS map for rendering and a voxel map for occupancy probability, working together. The voxel map is updated on-the-fly while the Gaussian map is trained at the end of each horizon.

Method: TRACE Framework

TRACE consists of three core modules: information map construction, kernel-ergodic trajectory optimization, and footprint-aware depletion. At each replanning step (horizon), the planner optimizes a K-step trajectory over the current information distribution, executes the full horizon, integrates new observations, and re-plans.

Framework Overview

flowchart TB
    RD[RGB-D Observations] --> VM[Voxel Map
Real-time update]
    RD --> GM[2DGS Map
Trained at horizon end]
    VM --> IM[Information Map phi_t]
    GM --> IM
    IM --> EP[Kernel-Ergodic Horizon Planner]
    EP --> E1[Info Term
MLE on phi]
    EP --> E2[Self-Correlation
Uniform coverage]
    EP --> E3[Footprint Depletion
Suppress re-coverage]
    EP --> E4[Gaze Reward
Aim at high-phi surfaces]
    E1 --> TR[K-step Trajectory tau_t]
    E2 --> TR
    E3 --> TR
    E4 --> TR
    TR --> EX[Execute Trajectory
Go2/FR3 controller]
    EX --> RD

The diagram above shows TRACE's complete framework. RGB-D observations update both voxel and Gaussian maps, from which the information distribution $\phi_t$ is derived online. The kernel-ergodic horizon planner optimizes a K-step trajectory combining information, self-correlation, footprint depletion, and gaze reward terms. After execution, new observations feed back to the maps, closing the loop between mapping and trajectory optimization.

Problem Formulation

An autonomous agent equipped with an RGB-D sensor incrementally builds a 2DGS map $\mathcal{M}$ of an unknown bounded scene $\Omega \subset \mathbb{R}^3$. The sensor is modeled as a view cone rotationally symmetric about its optical axis, so roll is immaterial and held fixed. At each replanning step (horizon) $t$, the planner selects a K-step trajectory $\tau_t = (\mathbf{p}_t^1, \ldots, \mathbf{p}_t^K) \in SE(3)^K$ with positions $\mathbf{x}_t^k \in \mathbb{R}^3$ and orientations determined by yaw and pitch. The trajectory is parameterized by control $\mathbf{u}_t \in \mathbb{R}^{K \times 5}$ (position velocity, yaw rate, pitch rate) cumulatively integrated under single-integrator dynamics (matching Go2's interface). Each replanning step solves:

$$\tau_t^\star = \arg\min_{\tau_t \in \mathcal{T}_t} J_t(\tau_t;\, \mathcal{M}_t, \mathcal{V}_t)$$

where $\mathcal{T}_t$ is the feasible-trajectory set in horizon $t$ (collision-free, bounded step size) and $J_t$ is the ergodic optimal-control objective defined over a target information distribution $\phi_t$ derived online from the current map.

Information Map Construction

Ergodic search requires a target information distribution. In active reconstruction, $\phi_t$ evolves as new observations are integrated. TRACE uses a hybrid map representation: 2DGS map $\mathcal{M}_t$ for rendering and voxel map $\mathcal{V}_t$ for occupancy probability. For each voxel $v$ in $\mathcal{V}_t$, the raw information value is:

$$\phi_t^{\text{raw}}(v) = \alpha_u \mathbf{1}_{\text{unexp}}(v) + \alpha_f \mathbf{1}_{\text{front}}(v) + \alpha_b \mathbf{1}_{\text{unbuilt}}(v) + \beta(1 - c(v))\mathbf{1}_{\text{low}}(v)$$

where $\mathbf{1}_{\text{unexp}}$ marks unexplored voxels no depth ray has traversed, $\mathbf{1}_{\text{front}}$ marks free voxels bordering unexplored space (frontiers), $\mathbf{1}_{\text{unbuilt}}$ marks voxels deemed occupied by $\mathcal{V}_t$ but lacking a corresponding Gaussian in $\mathcal{M}_t$, $c(v)$ is Gaussian confidence, and $\mathbf{1}_{\text{low}}$ marks low-confidence voxels. These four terms capture exploration, frontier, unmodeled, and uncertainty information respectively.

The raw information is box-filtered to diffuse into free space, masked to the observed collision-free region, and normalized to a probability distribution yielding $\phi_t$. Masking is what makes the ergodic target realizable: it projects information mass from occupied surfaces onto the reachable free space the sensor can actually occupy. The gaze reward uses the unmasked $\phi_t^{\text{raw}}$ to preserve absolute scale. All fields are sampled at continuous locations by trilinear interpolation, ensuring all terms are differentiable in the trajectory.

Footprint-Aware Depletion

In active sensing, the marginal value of observing a region diminishes—$\phi$ should decay at already-covered locations as the trajectory progresses. TRACE replaces the static evaluation with a coverage-discounted value:

$$\phi_t^k \equiv \phi_t(\mathbf{x}_t^k) \prod_{j < k}(1 - \eta \cdot v_{jk})$$

where $\eta \in (0,1)$ is the per-visit discount rate and $v_{jk} \in [0,1]$ is a soft footprint-overlap kernel between waypoints $j$ and $k$. The sensor footprint at waypoint $k$ is approximated by $D$ points sampled along the optical axis $\mathbf{z}_t^k$ at depths $\{d_1, \ldots, d_D\}$, with the overlap kernel:

$$v_{jk} = \max_{d_k, d_j} \exp\left(-\frac{\|\mathbf{f}_{t,d_k}^k - \mathbf{f}_{t,d_j}^j\|^2}{2\sigma_{\text{fp}}^2}\right)$$

This serves as a differentiable proxy for surface co-visibility: two waypoints whose central rays pass through nearby observable points are penalized for re-observing the same region, with $\sigma_{\text{fp}}$ softening each ray into a cylinder. The depletion product runs over the current horizon's waypoints; cross-horizon re-coverage is suppressed by re-deriving $\phi_t$ from the updated map at every horizon boundary.

Footprint Gaze Reward

The depletion-aware ergodic metric still evaluates $\phi$ at waypoint $\mathbf{x}_t^k$, whose gradient attracts the robot toward high-$\phi$ positions—which in reconstruction lie on or behind surfaces the robot cannot occupy. TRACE complements this with a footprint reward that attracts the camera's footprint to high-$\phi$ surfaces while the robot position remains governed by the ergodic metric and safety penalties:

$$L_{\text{gaze}}(\mathbf{u}_t) = -\frac{1}{KD}\sum_{k=1}^{K}\sum_{d=1}^{D} w_{k,d}\,\phi_t^{\text{raw}}(\mathbf{f}_{t,d}^k)$$

where $w_{k,d} = \exp(-\kappa\,d_{\text{unsafe}}(\mathbf{f}_{t,d}^k))$ attenuates footprint samples in unobservable space. $d_{\text{unsafe}}(\mathbf{f})$ is the distance from $\mathbf{f}$ to the observed collision-free region—zero in free space, growing inside obstacles and in the unobserved region behind them, ensuring the reward never credits pointing at high-$\phi$ voxels visible only through a wall.

Full Cost and Trajectory Optimization

The depletion-aware kernel-ergodic metric is:

$$E_{\text{kernel}}^{\text{dep}}(\tau_t) = -\frac{2}{K}\sum_{k=1}^{K}\phi_t^k + \frac{1}{K^2}\sum_{i,j=1}^{K}\exp\left(-\frac{\|\mathbf{x}_t^i - \mathbf{x}_t^j\|^2}{2\sigma^2}\right)$$

where $\phi_t^k$ is the depletion-discounted target value (information term) and the pairwise term repels nearby waypoints toward uniform coverage (self-correlation term). The box-filter diffusion plays the mollifying role of the metric's Gaussian kernel, realizing the information term on a target the sensor can reach. The full cost combines the ergodic metric, gaze reward, safety penalties, and regularization:

$$J_t(\mathbf{u}_t) = E_{\text{kernel}}^{\text{dep}}(\tau_t(\mathbf{u}_t)) + \lambda_g L_{\text{gaze}}(\mathbf{u}_t) + \lambda_s L_{\text{safe}}(\mathbf{u}_t) + \lambda_r \|\mathbf{u}_t\|^2$$

All terms are differentiable in $\mathbf{u}_t$; optimized with Adam, warm-started from the previous-horizon solution. The agent executes all K waypoints before re-planning.

Code Correspondence

TRACE's code is open-sourced with core modules directly mapping to the paper's method. `planning/kernel_ergodic.py` implements the kernel-ergodic metric $E_{\text{kernel}}^{\text{dep}}$, where `kernel_ergodic_metric_pos` computes the two-term metric (information + self-correlation), and `trilinear_sample_field` provides differentiable trilinear interpolation via PyTorch's `grid_sample`. `planning/info_dynamics.py` implements footprint depletion, with `causal_depletion_factors` computing the causal discount $\prod_{j<k}(1-\eta \cdot v_{jk})$. `planning/ergodic_horizon.py` is the `ErgodicHorizon` planner class that builds the cost closure with info-map, gaze, and collision terms and passes it to `optimize_trajectory`, with the decision variable being a $(K, 5)$ tensor of per-step deltas [dx, dy, dz, dyaw, dpitch].

# planning/ergodic_horizon.py: Decision variable
# Decision variable is a (K, 5) tensor of per-step deltas
# [dx, dy, dz, dyaw, dpitch]; trajectory is cumulative sum
# starting from the current pose.

Experimental Results

Quantitative Analysis

TRACE is evaluated on 8 indoor scenes from the Replica dataset, achieving higher PSNR than the strongest NBV baseline ActiveGS on every scene, with a mean gain of 1.5dB. SSIM and LPIPS show the same pattern. The lead arises from two mechanisms: continuous-pose optimization places the camera at any high-$\phi$ pose along the trajectory rather than the nearest discrete candidate; depletion suppresses re-coverage, spreading subsequent waypoints to low-confidence surfaces.

Table 1: Average rendering quality over 8 Replica scenes (5 independent runs).

The PSNR gain is positive on every scene, but its composition differs by scene type. On office scenes, SSIM is comparable while LPIPS improves by 6-19%, indicating gains concentrated in fine-scale appearance—attributed to continuous-pose optimization reaching poses unavailable to discrete candidates. On room and hotel scenes, the SSIM gap widens to 0.012-0.017, indicating structural differences—these scenes contain heavier occlusion from furniture, where surfaces missed by discrete viewpoint selection require deliberate coverage via the ergodic trajectory's continuous heading variation.

Ablation Studies

Ablations verify whether TRACE's gain simply comes from denser sampling along the trajectory. ActiveGS-Random scatters 10 random observations per horizon (no planner); ActiveGS-Uniform captures 10 uniformly-interpolated poses along its original path. The former matches ActiveGS within noise (+0.08dB), the latter drops (-1.7dB), neither closing the gap—densifying sampling performs worse, as redundant views dilute the training weight of informative observations.

Removing the footprint depletion mechanism loses 2.1dB, falling below ActiveGS on 6 of 8 scenes. NBV replans after every view, with confidence updates implicitly depleting visited regions. Ergodic search optimizes the whole horizon against a frozen information map, yielding redundant views—depletion is therefore essential to the framework.

Real-World Deployment

TRACE is deployed on two physical platforms: a Unitree Go2 quadruped for room-scale scene reconstruction and a Franka FR3 arm for object-centric reconstruction. Since the planned trajectory is itself the optimization variable, its waypoints are handed directly to each platform's controller with no intermediate path planner—a practical payoff of trajectory-level planning. An NBV planner commits to discrete viewpoints and delegates connecting motion to a separate path planner, which can issue dynamically infeasible commands. TRACE optimizes the trajectory under the platform's own dynamics, so every planned waypoint is dynamically consistent by construction. In real-robot experiments, TRACE achieved a 100% success rate, while the NBV baseline made minor contact with the environment in every trial.

In 5-minute missions across a 42m² scene, the Go2's front-mounted RGB-D camera (Intel D435) sweeps the room as a single continuous ergodic path. The heading traces show TRACE's heading varying continuously, while NBV snaps between discrete targets with rapid reversals—exactly the motion a viewpoint-level planner cannot penalize.

Limitations

The paper does not include a dedicated limitations section, but several can be inferred from the method design and experiments. First, information map construction relies on voxel discretization (20cm grid), whose resolution affects the granularity of the information distribution—too coarse may miss small high-information regions, too fine increases computational cost. Second, the single-integrator dynamics model matches the Go2 interface but simplifies real dynamics; the more complex joint-space search on FR3 is relegated to the appendix, with the main text not fully discussing it. Third, evaluation is only on Replica indoor scenes; generalization to large-scale outdoor or unstructured environments is unverified.

From an independent assessment, the trade-off between time budget and information decay deserves attention. The paper fixes a 300-second budget, but the optimal budget likely varies with scene complexity—how to adaptively adjust the budget under dynamically changing information density is undiscussed. Additionally, interaction with active learning is a potential direction: the current information distribution is derived purely from reconstruction uncertainty; if combined with semantic uncertainty (which object categories are unrecognized), ergodic coverage could simultaneously serve geometric and semantic goals.

Conclusion and Outlook

TRACE reformulates active 2DGS reconstruction from greedy viewpoint selection to ergodic coverage. A target distribution is derived online from voxel-level information and per-Gaussian uncertainty, and a continuous, control-feasible trajectory is optimized with a kernel-ergodic horizon planner and footprint-overlap depletion. Across 8 Replica scenes, TRACE improves +1.5dB PSNR on average, and its trajectories execute directly on real robots without an intermediate path planner. By recasting active reconstruction as ergodic coverage, TRACE plans information gathering and feasible motion jointly within a single objective.

This work provides a paradigm shift for robotic active perception from discrete decisions to continuous optimization: no longer "where to look next," but "how to distribute sensing resources throughout the entire motion." As ergodic search theory and Gaussian-splatting representations continue to advance, this trajectory-level planning approach could extend to more robotic tasks—from exploration mapping to inspection monitoring.