J-LAW: Joint Localization and Actionable World Modeling via Coupled Latent Factor Graphs

Abstract: Classical SLAM estimates metric poses and a geometric map but produces no actionable predictive model for planning. Action-conditioned world models learn compact latent dynamics for planning but ignore global metric consistency and accumulate drift under open-loop rollout. We argue these are two views of the same estimation problem and propose J-LAW (Joint Localization and Actionable World Modeling) in this letter: a coupled factor graph that jointly optimizes metric object poses, latent world states, and latent landmark embeddings. The bridge is a pose-conditioned latent encoder and a learned pose--latent coupling factor, so that better localization improves the world model and vice versa. We cast observation, action-conditioned prediction, metric odometry, pose--latent coupling, latent loop closure, and latent landmark observation as probabilistic factors in a single MAP objective. Real-data experiments on PushT and WildGS show that coupled graph correction substantially reduces latent prediction RMSE and endpoint drift relative to open-loop rollout, while latent loop closure improves global trajectory consistency. J-LAW yields a map that is simultaneously metric (poses) and actionable (latent landmarks for planning).

• The paper introduces a unified factor graph framework that jointly optimizes metric localization and latent world modeling.
• It employs alternating block coordinate descent to significantly reduce latent RMSE and endpoint drift in long-horizon scenarios.
• Adaptive confidence weighting and loop closure mechanisms enhance robustness compared to conventional SLAM or latent-only models.

Joint Localization and Actionable World Modeling with Coupled Latent Factor Graphs

Motivation and Problem Setting

Traditional SLAM systems provide global metric consistency and loop closure correction but lack predictive, actionable models for control and planning. In contrast, JEPA-style action-conditioned world models produce compact latent dynamics for planning but are ungrounded in global geometry, leading to drift and inconsistency, especially under open-loop rollout. These two paradigms—SLAM and learned world models—typically operate independently, missing opportunities for mutual constraint and improvement.

J-LAW addresses this gap by proposing a unified framework where metric localization and predictive latent world modeling are coupled via a novel factor graph architecture. This coupling enables bidirectional correction: improved localization sharpens predictive world models, and superior latent predictions correct accumulated localization drift.

Figure 1: J-LAW's factor graph architecture with alternating block coordinate descent for joint optimization of metric pose, latent state, and actionable latent landmarks.

Methodology: Coupled Latent-Metric Factor Graph

J-LAW formulates the joint estimation of metric poses ($X = \{x_t\}$), latent world states ($Z = \{z_t\}$), and actionable latent landmark embeddings ($M = \{m_k\}$) as a single MAP inference in a coupled factor graph. The key architectural innovations include:

• Pose-Conditioned Encoder: The representation $z_t = E_\theta(o_t, x_t)$ is explicitly grounded in metric pose, in contrast to pose-agnostic JEPAs.
• Learned Pose–Latent Coupling Decoders: A decoder $h_\theta(x_t, M)$ predicts latent states from pose and landmark embeddings, realizing a feedback bridge that constrains both localization and world modeling.
• Bidirectional Loop Closure: Revisiting locations triggers both metric and latent-space loop closures, enforcing consistency in both geometry and prediction.

Six major factor types are incorporated into the MAP objective, including observation (visual anchoring), JEPA-style latent prediction, metric odometry, pose–latent coupling, latent loop closure, and latent–landmark association. Each serves to either strictly or softly enforce consistency among the three key variables.

Figure 2: Latent and pose RMSE as a function of trajectory length $H$ across WildGS test scenes for various J-LAW ablations.

Block coordinate descent alternates between pose and latent/landmark updates. Direct joint optimization via L-BFGS is empirically shown to be ill-conditioned, often yielding degenerate solutions where neither pose nor latent states are properly regularized. Alternating optimization ensures each subproblem is well-constrained and that bidirectional information transfer is realized effectively.

Experimental Results

J-LAW's framework is evaluated on PushT (trained LeWM latents) and WildGS (dynamic SLAM sequences with ground-truth motion capture) datasets. The empirical investigation addresses several key axes:

• Correction of Open-Loop Drift: Factor-graph inference markedly reduces latent prediction RMSE and endpoint drift compared to open-loop rollouts, especially over long trajectories or with sparse/noisy loop closures.
• Ablation on Coupling Mechanisms: Alternating block coordinate descent surpasses direct joint L-BFGS optimization and the pose-only/latent-only baselines. Notably, pose-only optimization achieves lower latent RMSE for short trajectories due to its reduced solution space, but its error accumulates rapidly with trajectory length. J-LAW alternating maintains substantially lower drift over long horizons.
• Adaptive Coupling Confidence: Incorporation of IRLS-style adaptive confidence per time step enhances pose estimation robustness, reducing pose RMSE by 29–42% compared to fixed coupling. However, improvement in pose accuracy comes with a slight trade-off against latent RMSE, highlighting the limitations of coupling decoder quality.
• Loop Closure Robustness and Consistency–Accuracy Tradeoff: Empirical results demonstrate that true loop closures can reduce loop error by up to 98% while robust/cauchy weighting mitigates the negative impact of false positives.

Quantitative Highlights

• Latent RMSE reduction: On WildGS, factor-graph correction reduces mean latent RMSE from 0.3050 (open-loop) to 0.0873.
• Endpoint drift: Pose-only drift increases 38% from $H=48$ to $512$, while J-LAW alternating increases by only 11% and achieves lower drift at the longest horizon.
• Alternating superiority: Latent RMSE under alternating optimization (0.1383) is 16.9% lower than joint L-BFGS and 13.3% better than the latent-only baseline.

Implications and Future Directions

J-LAW's formulation validates that metric and latent representations can be coupled to yield maps that are both geometrically consistent and actionable for planning—without requiring dense geometric reconstruction. This has practical implications for robotics and embodied AI, where real-time adaptation, global trajectory consistency, and actionable internal modeling are critical.

Theoretically, J-LAW suggests new avenues for integrating geometric and learned representations in probabilistic inference frameworks, inviting further exploration into more expressive coupling factors, end-to-end differentiable training of the full system, and more principled confidence estimation mechanisms for loop closures and coupling weights.

Future work will likely focus on jointly optimizing coupling module parameters within the alternating factor-graph framework, extending the system to non-vision modalities, and adapting the method for online or incremental inference regimes suitable for real-world deployment.

Conclusion

J-LAW provides a principled, unified mechanism for simultaneous optimization of localization and actionable world modeling via a coupled factor graph. Its empirical results demonstrate improved drift stability, consistency, and actionable representations, especially under challenging open-loop and long-horizon conditions. The alternating block coordinate descent approach avoids degenerate solutions and maximizes the benefit of bidirectional coupling. These findings support the value of integrating metric and latent inference for robust, predictive, and actionable spatial intelligence systems ["J-LAW: Joint Localization and Actionable World Modeling via Coupled Latent Factor Graphs" (2606.28712)].