LoGoPlanner: Localization Grounded Navigation Policy with Metric-aware Visual Geometry (2512.19629v2)

Abstract: Trajectory planning in unstructured environments is a fundamental and challenging capability for mobile robots. Traditional modular pipelines suffer from latency and cascading errors across perception, localization, mapping, and planning modules. Recent end-to-end learning methods map raw visual observations directly to control signals or trajectories, promising greater performance and efficiency in open-world settings. However, most prior end-to-end approaches still rely on separate localization modules that depend on accurate sensor extrinsic calibration for self-state estimation, thereby limiting generalization across embodiments and environments. We introduce LoGoPlanner, a localization-grounded, end-to-end navigation framework that addresses these limitations by: (1) finetuning a long-horizon visual-geometry backbone to ground predictions with absolute metric scale, thereby providing implicit state estimation for accurate localization; (2) reconstructing surrounding scene geometry from historical observations to supply dense, fine-grained environmental awareness for reliable obstacle avoidance; and (3) conditioning the policy on implicit geometry bootstrapped by the aforementioned auxiliary tasks, thereby reducing error propagation. We evaluate LoGoPlanner in both simulation and real-world settings, where its fully end-to-end design reduces cumulative error while metric-aware geometry memory enhances planning consistency and obstacle avoidance, leading to more than a 27.3\% improvement over oracle-localization baselines and strong generalization across embodiments and environments. The code and models have been made publicly available on the https://steinate.github.io/logoplanner.github.io.

• The paper introduces a unified end-to-end navigation policy that integrates implicit localization with metric-aware visual geometry, eliminating reliance on external localization modules.
• It utilizes a video geometry transformer augmented with depth-derived scale priors to reconstruct dense scene geometry and generate trajectories via a query-based diffusion policy.
• Empirical evaluations report over 27.3% improvement in Success Rate in simulations and robust real-world performance across diverse robotic platforms and environments.

LoGoPlanner: Localization-Grounded Metric-Aware Navigation Policy

Motivation and Limitations of Prior Art

Autonomous navigation in unstructured environments requires precise and robust trajectory planning while maintaining generalization across diverse robotic embodiments and environments. The established modular pipeline paradigm, which independently orchestrates perception, localization, mapping, and planning, introduces latency and error propagation across modules, impairing performance particularly on real robotic platforms where sensor noise and calibration mismatches are prevalent. Recent advances in end-to-end learning-based navigation policies mitigate inter-module cascading errors by mapping visual input directly to actions, but most state-of-the-art frameworks still depend on explicit localization systems such as SLAM or VO for self-state estimation. This continued reliance entails external calibration and suffers from scale drift and inconsistency, especially when models lack geometric memory or only process short observation windows (Figure 1).

Figure 1: Comparison of traditional modular planners, existing end-to-end frameworks, and LoGoPlanner’s integrated approach combing implicit state estimation with metric-aware visual geometry.

LoGoPlanner Framework Overview

LoGoPlanner addresses these limitations by unifying metric-scale scene understanding, implicit localization, and trajectory planning within a single end-to-end differentiable policy. At its core, the architecture leverages a video geometry transformer backbone augmented with depth-derived metric priors to reconstruct dense scene geometry and estimate egomotion over long visual horizons. Perception and control domains are rigorously decoupled to enforce embodiment-agnostic estimation even across varying camera positions and robot configurations. Unlike conventional approaches where point clouds or pose estimates are explicitly cascaded between modules—thereby compounding numerical error—LoGoPlanner conducts all intermediate processing at the feature level, exploiting cross-attention to generate unified planning contexts for downstream trajectory refinement (Figure 2).

Figure 2: High-level architecture, showing the integration of scale priors within ViT-based video geometry backbones and the query-based, diffusion-head-driven action policy.

Metric-Aware Visual Geometry and Implicit Localization

To enable metric-consistent local and world-centric geometric representations, LoGoPlanner fine-tunes a video geometry transformer (e.g., VGGT) by injecting depth map-derived scale priors into cross-patch embeddings. Scene reconstruction and camera pose estimation are supervised via auxiliary tasks, enforcing both point-cloud correspondence (relative to the robot's frame) and rigid-body coordinate consistency across time steps.

• Local Point Prediction: Given RGB-D sequences, the system infers robust local geometric features grounded by learned scale, supporting dense point-cloud reconstruction as an implicit intermediate prior (Figure 3).
• Camera Pose Estimation: Task-specific heads decode pose estimates, with egomotion expressed relative to a canonical chassis coordinate, thus decoupling perception viewpoint (camera frame) from actionable control (chassis frame) to enhance generalization.

By fusing local geometry and temporal pose features through a context fusion transformer, the reconstructed world-centric scene geometry is made directly available for planning, closing the loop without explicit reliance on external localization.

Figure 3: Qualitative comparison of ground-truth and predicted metric-scale point clouds reconstructed with implicit localization, referenced from the robot chassis.

Query-Based Diffusion Policy for Trajectory Generation

The planning policy head operates exclusively on fused implicit features. Task-specific queries—state and geometry—are extracted via cross-attention, concatenated with a learned goal embedding, and conditioned into the planning context. Trajectories are generated through a diffusion policy, iteratively refining action sequences towards feasible, collision-free navigation endpoints. This design contrasts with modular or RL-based planners by:

• eschewing brittle explicit pose or map representations,
• maintaining closed-loop feedback over long temporal windows via implicit geometry memory,
• optimizing planning error as the final objective via direct supervision of the diffusion policy.

Evaluation and Empirical Performance

Simulation Benchmarks

On the InternScenes simulation benchmark the proposed approach was rigorously evaluated across diverse home and commercial environments characterized by cluttered layouts and narrow passages (Figure 4). LoGoPlanner outperformed all competitive baselines, including oracularly-localized ViPlanner and iPlanner, achieving over 27.3% superiority in Success Rate (SR) and significant gains in Success weighted by Path Length (SPL). These numerical results provide strong evidence for the efficacy of implicit metric-aware localization: even in the absence of external SLAM input, LoGoPlanner consistently outstripped alternative pipelines that depended on simulator or SLAM-based egomotion.

Figure 4: Diversity of test environments, with representative home and commercial scene layouts.

Real-World Generalization

Transferring to real robot platforms (TurtleBot, Unitree Go2, Unitree G1) and challenging unseen scenes, LoGoPlanner continued to exhibit robust generalization. Unlike prior approaches, which suffered from embodiment-specific failures and failed to scale to industrial scenes, the metric-aware, implicit localization policy showed strong robustness to sensor jitter, environmental ambiguity, and sim-to-real gaps, attaining SRs of 85, 70, and 50 across office, home, and industrial domains, respectively. The qualitative results (Figure 5) show smooth, collision-free planned paths and resilience across platforms.

Figure 5: Real-world deployment, showing planned LoGoPlanner trajectories on various robotic platforms and diverse environments.

Ablation and Model Analysis

Ablation studies confirmed the essentiality of each auxiliary task (odometry, subgoal, point cloud prediction). Notably, removing geometric supervision or substituting single-frame geometric backbones resulted in measurable degradation in both planning accuracy and navigational robustness. Only the combination of all auxiliary tasks with depth-injected metric priors attained both high-level planning success and accurate geometric correspondence.

Practical and Theoretical Implications

LoGoPlanner’s tightly coupled, end-to-end mapping from perception to action obviates the need for fragile external localization modules while ensuring robust generalization across embodiments and complex, real-world environments. By grounding policy optimization in metric-scale, temporally-consistent geometry, the system advances practical deployment of autonomous navigation in scenarios with limited or unreliable sensor calibration. Theoretically, this architecture exemplifies the future direction for robotic planning—implicit, cross-modal feature grounding and direct optimization on downstream task losses—offering a scalable path for sim-to-real transfer and robust execution in complex open-world conditions.

Conclusion

LoGoPlanner establishes a new benchmark for fully end-to-end, localization-grounded navigation policies by fusing metric-aware visual geometry with implicit state estimation and query-driven action planning. Significant empirical improvements in both simulation and real-world experiments validate the paradigm of tightly integrating geometry priors and feature-centric policy reasoning. As future large-scale datasets and more powerful geometric backbones emerge, continued refinement along these lines will likely further close the gap between vision and robust goal-directed robot navigation in dynamic, cluttered, and unstructured settings.