Differentiable Factor Graph Optimization for Learning Smoothers

Brent Yi, Michelle A. Lee, Alina Kloss, Roberto Martín-Martín, Jeannette Bohg

Abstract

A recent line of work has shown that end-to-end optimization of Bayesian filters can be used to learn state estimators for systems whose underlying models are difficult to hand-design or tune, while retaining the core advantages of probabilistic state estimation. As an alternative approach for state estimation in these settings, we present an end-to-end approach for learning state estimators modeled as factor graph-based smoothers. By unrolling the optimizer we use for maximum a posteriori inference in these probabilistic graphical models, we can learn probabilistic system models in the full context of an overall state estimator, while also taking advantage of the distinct accuracy and runtime advantages that smoothers offer over recursive filters. We study this approach using two fundamental state estimation problems, object tracking and visual odometry, where we demonstrate a significant improvement over existing baselines.

Introduction

State estimation—the process of inferring a robot's position or velocity from noisy sensor data—is a cornerstone of robotics. Traditionally, researchers have relied on Bayesian filters, such as the Kalman Filter, which update a "belief" state recursively. While efficient, these filters often struggle with complex, nonlinear sensor modalities like cameras and typically require tedious manual tuning of noise models.

An alternative is smoothing, which optimizes the entire trajectory at once using a factor graph. Smoothers are generally more accurate than filters because they consider the global context of the trajectory. However, integrating them with deep learning has been challenging because smoothers involve an internal optimization loop. To train a neural network within this loop, one must be able to backpropagate gradients through the optimization steps themselves. This paper introduces a method for end-to-end learning of factor graph-based smoothers by making the nonlinear optimization process differentiable.

Methods

The proposed approach treats Maximum A Posteriori (MAP) inference—the process of finding the most likely state sequence—as a sequence of differentiable operations. By "unrolling" the optimizer, the authors allow gradients to flow from estimation errors back to the parameters of probabilistic system models.

The end-to-end differentiable pipeline for factor graph optimization, showing the flow from system models to error calculation and the backpropagation of gradients.

Key Technical Highlights

  • Optimizer Unrolling: The authors implement a nonlinear optimizer (e.g., Gauss-Newton) and "unroll" it for a fixed number of iterations ($K$). This transforms the optimization process into a standard computation graph suitable for deep learning frameworks.
  • Ground-Truth Initialization: To ensure stable training and avoid local minima, the optimizer is initialized at the ground-truth trajectory during training, with unrolling steps moving away from it. This ensures that a perfect model yields zero gradients.
  • Lie Theory Integration: To handle 3D poses and rotations correctly, the framework uses generalized addition and subtraction operators, ensuring mathematical consistency on manifolds like $SE(2)$.
  • JAX Implementation: The system is built on JAX, leveraging sparse matrix computations and automatic differentiation for computational efficiency.

Evaluation and Results

The approach was evaluated on two primary tasks: synthetic visual tracking and real-world visual odometry using the KITTI-10 dataset.

  • Visual Tracking: In a benchmark involving a moving disk frequently occluded by distractors, the differentiable smoother achieved an RMSE of 2.34 pixels, significantly outperforming Bidirectional LSTMs (2.89) and differentiable Kalman Filters (3.08).
  • KITTI Odometry: For estimating a car's 5-degree-of-freedom state from camera images, the smoother with a learned heteroscedastic noise model achieved a translational error of 0.1457 m/m, surpassing the EKF baseline (0.1683 m/m).

Discussion and Insights

A key finding is that "context is king": noise models learned "in the loop" of the smoother perform better than those learned in isolation. The smoother learns to dynamically weight sensor inputs based on their impact on the final trajectory. For instance, in tracking tasks, the model learned to assign high uncertainty to the vision sensor during occlusions, effectively relying on the dynamics model instead.

While the method requires ground-truth state labels and introduces some computational overhead compared to recursive filters, it proves that smoothers provide a distinct accuracy advantage, especially when dealing with the complex, non-Gaussian noise typical of deep-learning-based sensors. This work bridges the gap between classical probabilistic robotics and modern deep learning, offering a robust framework for complex robotic tasks.

Summary updated

Abstract

A recent line of work has shown that end-to-end optimization of Bayesian filters can be used to learn state estimators for systems whose underlying models are difficult to hand-design or tune, while retaining the core advantages of probabilistic state estimation. As an alternative approach for state estimation in these settings, we present an end-to-end approach for learning state estimators modeled as factor graph-based smoothers. By unrolling the optimizer we use for maximum a posteriori inference in these probabilistic graphical models, we can learn probabilistic system models in the full context of an overall state estimator, while also taking advantage of the distinct accuracy and runtime advantages that smoothers offer over recursive filters. We study this approach using two fundamental state estimation problems, object tracking and visual odometry, where we demonstrate a significant improvement over existing baselines. Our work comes with an extensive code release, which includes training and evaluation scripts, as well as Python libraries for Lie theory and factor graph optimization: https://sites.google.com/view/diffsmoothing/
BibTeX
@inproceedings{yi2021differentiable,
  author = {Brent Yi and Michelle A. Lee and Alina Kloss and Roberto Martín-Martín and Jeannette Bohg},
  title = {{Differentiable Factor Graph Optimization for Learning Smoothers}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2021},
  doi = {10.1109/iros51168.2021.9636300},
}
Figure