Blog
From Offline to Online SLAM
Batch Factor Graphs unlocked fabulous offline SLAM to produce highly accurate maps. You collect data on a long robot run, feed it to the algorithm, and watch it generate clean trajectories and environments. However, the sheer size of the resulting graph — and the time required to process it from scratch every iteration — historically made online SLAM a distant dream.
To enable real-time execution, new algorithmic approaches were necessary to avoid rebuilding the entire system at once. Variable elimination and elimination ordering techniques allowed us to accurately factorize the graph while maintaining the sparse structure of the information matrix. Grouping these sorted conditionals into cliques induces a “Bayes Tree”. When running a new step, only a few localized nodes (cliques) near the root of the tree need to be touched. In practice, regular odometry updates (BetweenFactors) require updating only 2 cliques, leaving the expensive global processing for heavy operations like loop closures.
By utilizing a fixed-lag smoother to marginalize out old variables, and applying local incremental updates that can handle out-of-order measurements fluidly, we can compress past history into a single prior and focus compute exactly where it’s needed. These exact sparse factorization techniques turn an otherwise prohibitively expensive process into an efficient online solver that runs comfortably on modern robotics hardware.
Let’s dive deeper into the mechanics of sparse solving through the notebook below:
factor-graphs-from-scratch/modules/05-incremental-sparse/lesson.ipynb at main ·…
Also published on Medium.