TL;DR
This paper introduces a method for end-to-end learning of factor graph-based smoothers by making the nonlinear optimization process differentiable. By "unrolling" the optimizer, the authors allow gradients to flow from estimation errors back to the parameters of probabilistic system models, resulting in state estimators that significantly outperform traditional filters and LSTMs in complex tasks like visual tracking and odometry.
What Problem Does This Paper Address?
In robotics, "state estimation" is the art of guessing a robot's position or velocity based on noisy sensor data. Traditionally, researchers use Bayesian filters (like the Kalman Filter), which update a "belief" step-by-step. While efficient, filters struggle with complex, nonlinear sensors (like cameras) and often 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 look at the "big picture," but they are harder to integrate with deep learning. The core difficulty is that smoothers involve an internal optimization loop; to train a neural network inside that loop, you need to be able to backpropagate through the optimization steps themselves.
Methods
The authors propose Differentiable Factor Graph Optimization. Their approach treats Maximum A Posteriori (MAP) inference—the process of finding the most likely state—as a sequence of differentiable operations.
Key technical highlights include:
- Optimizer Unrolling: They implement a nonlinear optimizer (like Gauss-Newton) and "unroll" it for a fixed number of iterations ($K$). This turns the optimization process into a standard computation graph that deep learning frameworks can handle.
- Ground-Truth Initialization: To ensure stable training and avoid local minima, they initialize the optimizer at the ground-truth trajectory during training and unroll steps away from it. This guarantees that if the model is perfect, the gradients are zero.
- Lie Theory Integration: Since robotics often involves rotations and 3D poses (which don't live in standard Euclidean space), the authors use "generalized" addition and subtraction operators to ensure the math remains consistent on manifolds like $SE(2)$.
- JAX Implementation: They released a library built on JAX that utilizes sparse matrix computations and automatic differentiation to make this process computationally feasible.
Datasets
The approach was evaluated on two primary tasks:
- Synthetic Visual Tracking: A benchmark where a red disk moves according to linear-Gaussian dynamics but is frequently occluded by other "distractor" disks. This tests the model's ability to handle "heteroscedastic" noise (noise that changes depending on the input).
- KITTI-10 Visual Odometry: A real-world dataset involving a car driving through various environments. The goal is to estimate the car's 5-degree-of-freedom state using only camera images. The authors used 10-fold cross-validation, ensuring the model generalized to "never-before-seen" roads.
Key Insights and Ideas
- Context is King: A noise model learned in isolation (e.g., via maximum likelihood) performs worse than one learned "in the loop" of the smoother. The smoother learns to "trust" or "distrust" specific sensor inputs based on how they affect the final trajectory.
- Handling Occlusions: In the visual tracking task, the end-to-end smoother learned to assign massive uncertainty to the vision sensor when the disk was occluded, effectively telling the system to ignore the camera and rely on the physics (dynamics) model instead.
- Smoothers > Filters: The results confirm that smoothers provide a distinct accuracy advantage over recursive filters, especially when dealing with the complex, non-Gaussian noise profiles typical of deep-learning-based "virtual sensors."
- No "One Size Fits All" Model: A model optimized for a filter does not perform optimally when plugged into a smoother. This underscores the importance of training models in the exact architectural context in which they will be deployed.
Key Results
- Visual Tracking: The proposed differentiable smoother achieved an RMSE of 2.34 pixels, significantly outperforming Bidirectional LSTMs (2.89) and differentiable Kalman Filters (3.08).
- KITTI Odometry: The smoother with a learned heteroscedastic noise model achieved a translational error of 0.1457 m/m, outperforming the EKF baseline (0.1683 m/m) and significantly beating LSTM-based approaches.
- Qualitative Success: The authors demonstrated that their smoother could correctly reason about erroneous measurements when an object exited the camera frame, a scenario where constant-noise models typically fail.
Limitations and Future Work
- Supervision Requirements: The current method requires ground-truth state labels for training. While common in research, this can be difficult to obtain for some real-world robotic applications.
- Computational Overhead: While unrolling $K$ steps is faster than waiting for full convergence, it is still more computationally expensive than a simple recursive filter update.
- Future Directions: The authors suggest applying this framework to other "hard-to-model" modalities, such as tactile sensing or audio-based localization, where factor graphs have historically been limited by the difficulty of hand-designing factors.
Summary updated