"I spent a decade forgetting my past one frame at a time, and it made me fast but wrong. Then someone taught me to keep the graph, and I was slower for a millisecond and correct for the rest of my life."
A Reformed Filtering AI Agent
Prerequisites
This section is the synthesis of Chapter 11, so it assumes the pose-graph view of Section 11.1, the MAP and nonlinear-least-squares formulation of Section 11.2, the incremental smoother of Section 11.3, loop closure and marginalization from Section 11.4, robust kernels from Section 11.5, and the information-matrix covariances of Section 11.6. It contrasts throughout with the recursive filters of Chapter 9 and Chapter 10, and it uses the inertial-drift facts developed in Chapter 23.
The Big Picture
For twenty years, real-time simultaneous localization and mapping (SLAM) and visual-inertial odometry (VIO) ran on filters, because filters were the only thing fast enough. Then the field pivoted, almost completely, to factor-graph smoothing, and today essentially every serious open-source and production system, ORB-SLAM3, VINS-Fusion, OpenVINS, Kimera, the tracker in a modern headset, is a smoother at its core. This was not fashion. It was the convergence of three facts: a well-designed graph is sparse, so smoothing over a whole trajectory can be as cheap per step as filtering; smoothing relinearizes, so it does not permanently bake in the linearization errors that make filters silently overconfident and eventually diverge; and a graph keeps the past alive, so a loop closure or a delayed measurement can reach back and repair history that a filter has already thrown away. This section explains those three facts as a single argument, and shows where the tradeoff still favors a filter.
The filter's original sin: marginalize-as-you-go
A Kalman filter, and every extended or unscented variant in Chapter 9, maintains only the current state and its covariance. Each new measurement is fused and the old state is discarded, which is exactly the marginalization operation of Section 11.4 applied relentlessly, once per step, forever. That is what makes a filter cheap and constant-time, and it is also its original sin. Marginalizing a variable is only exact for a linear-Gaussian model. In SLAM the measurement models are nonlinear (a camera projecting a 3D landmark, an inertial measurement unit integrating rotation), so every marginalization freezes a linearization made at a state estimate that later turns out to be wrong. The frozen Jacobians can never be corrected, because the states they were computed at no longer exist as variables. Errors of linearization therefore accumulate as spurious information: the filter's covariance shrinks faster than the true uncertainty, the estimate grows confidently wrong, and over a long run the estimator diverges even though every individual update looked healthy.
A factor-graph smoother refuses to commit. It stores measurements as factors and re-solves the whole trajectory, so at each iteration it can relinearize every factor at the current best estimate. A landmark first seen from a bad initial guess and later triangulated well contributes a Jacobian computed at the good estimate, not the bad one. This single difference, deferring linearization instead of freezing it, is the deepest reason smoothing is more accurate than filtering on the same data, and it is why benchmark after benchmark shows batch and incremental smoothers beating their filter counterparts in absolute trajectory error.
Key Insight
The filter-versus-smoother debate is really a debate about when you are allowed to change your mind about the past. A filter linearizes each measurement exactly once, at the state estimate it happened to hold at that instant, and lives with the consequences forever. A smoother keeps the raw measurements and relinearizes them every time the estimate improves. Nonlinear SLAM and VIO punish anyone who cannot change their mind, so the estimator that keeps its options open wins, provided it can be made fast enough. That last proviso is what sparsity delivers.
Why smoothing is not slow: sparsity and incrementality
The historical objection to smoothing was cost. Re-solving a trajectory of thousands of poses sounds hopelessly more expensive than a filter's constant-time update. Two facts dissolve the objection. First, the SLAM information matrix is sparse: a pose is connected only to its immediate odometry neighbors, to the handful of landmarks it directly observed, and to the rare loop closure. Sparse linear algebra solves such systems in time closer to linear than cubic in the number of variables, so a full nonlinear least-squares step over the graph is far cheaper than the dense \(O(n^3)\) intuition suggests. Second, and decisively, iSAM2 from Section 11.3 made the solve incremental: adding one new pose and its factors updates only the small subtree of the Bayes tree that actually changed, so the amortized per-step cost is bounded and real-time, right up until a large loop closure forces a wider re-solve, which is exactly the moment you want the extra work done. The upshot is that a modern smoother runs at camera frame rate on the same hardware that once could only afford a filter, while producing the more accurate, relinearized estimate.
In Practice: a mixed-reality headset that never loses the room
A standalone augmented-reality headset must anchor virtual objects to the physical room to within a few millimeters, using a pair of grayscale cameras and a consumer-grade inertial measurement unit, all on a battery-powered mobile chip. Early consumer AR shipped filter-based VIO and users learned its tells: turn your head quickly and the virtual sofa slides off the floor, because the extended Kalman filter's frozen linearization could not absorb the fast rotation and its covariance had already collapsed. The current generation runs a fixed-lag factor-graph smoother. Inertial pre-integration bundles the hundreds of IMU samples between two camera frames into a single relative-motion factor (see Chapter 23 for why raw IMU integration drifts so fast), visual factors tie poses to map points, and the smoother relinearizes the recent window every frame. When the wearer walks out of the room and back, a loop closure snaps the map into global consistency and the sofa stays welded to the floor. The graph is what lets a millimeter anchor survive a fast head turn.
Fusing many sensors is just adding factors
The other reason the field standardized on graphs is representational. A sensor-fusion architecture built on filters needs a bespoke measurement update for each modality, and adding a delayed or out-of-order measurement (GNSS that arrives half a second late, a wheel-odometry tick on its own clock) means surgery on the recursion. In a factor graph, every sensor is simply a factor type: an IMU pre-integration factor between consecutive poses, a reprojection factor from a camera, a range factor from a GNSS or ultra-wideband anchor, a scan-match factor from a lidar, a prior factor from a magnetometer heading. A late measurement is inserted at the pose whose timestamp it matches and the graph re-solves; there is no notion of "too late," because time is just graph topology, not the front of a recursion. This is why the same GTSAM or Ceres back end serves visual-inertial, lidar-inertial, and GNSS-fused systems with only the front-end factors swapped, and it is the concrete mechanism behind the probabilistic-fusion principles of Chapter 49. Listing 11.7 shows how little code it takes to fuse two modalities once the graph abstraction is in place.
import gtsam
from gtsam import symbol_shorthand
X = symbol_shorthand.X # pose keys X(0), X(1), ...
graph = gtsam.NonlinearFactorGraph()
noise_odom = gtsam.noiseModel.Diagonal.Sigmas([0.05, 0.05, 0.02]) # x, y, theta
noise_loop = gtsam.noiseModel.Diagonal.Sigmas([0.10, 0.10, 0.05])
# A chain of odometry factors: one factor per sensor reading, any modality.
for i in range(4):
graph.add(gtsam.BetweenFactorPose2(
X(i), X(i + 1), gtsam.Pose2(1.0, 0.0, 0.0), noise_odom))
# A late-arriving loop closure between pose 0 and pose 4 (just another factor).
graph.add(gtsam.BetweenFactorPose2(
X(0), X(4), gtsam.Pose2(0.0, 0.0, 0.0), noise_loop))
graph.add(gtsam.PriorFactorPose2(
X(0), gtsam.Pose2(0, 0, 0), gtsam.noiseModel.Diagonal.Sigmas([1e-3]*3)))
values = gtsam.Values()
for i in range(5):
values.insert(X(i), gtsam.Pose2(i * 0.9, 0.0, 0.0)) # drifted initial guess
result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()
print("optimized pose 4:", result.atPose2(X(4))) # pulled back toward the startAs Listing 11.7 shows, the drifted pose 4 is pulled back toward the origin once the loop factor is added, and every pose in between shifts to absorb the correction: the relinearization-plus-reach behavior that a filter cannot reproduce, expressed in a dozen lines.
The Right Tool
Rolling your own visual-inertial back end, the sparse Cholesky factorization, the Bayes-tree bookkeeping, IMU pre-integration with correct covariance propagation, and marginalization, is a multi-thousand-line effort that research groups have spent years hardening. GTSAM's gtsam.ImuFactor plus PreintegratedImuMeasurements collapses the entire inertial front end into roughly 20 lines, and iSAM2 gives incremental real-time solving through ISAM2.update. A from-scratch VIO smoother is commonly 3,000 to 5,000 lines; the library-backed equivalent is a few hundred, and the library handles the numerically delicate parts (manifold retraction, on-manifold covariance, ordering for minimal fill-in) that are the usual source of silent bugs.
Research Frontier
The current state of the art is uniformly graph-based. ORB-SLAM3 (Campos et al., IEEE T-RO 2021) fuses visual and inertial data in a full smoother with a multi-map "Atlas" back end; VINS-Fusion and OpenVINS represent the sliding-window and MSCKF-style real-time end; Kimera adds metric-semantic mapping on the same factor-graph substrate. The live frontier is the seam between classical smoothing and learning: learned front ends (SuperPoint and SuperGlue features, NetVLAD and transformer place recognition) feed factors into a classical back end, and differentiable optimizers such as DROID-SLAM and Theseus (Facebook AI, NeurIPS 2022) embed the nonlinear least-squares solve inside a network so gradients flow through the smoother itself. The deeper reasons this chapter gives, relinearization, sparsity, and keeping the past alive, are exactly why even end-to-end learned SLAM keeps a factor-graph optimizer in the loop rather than replacing it. The full spatial-AI picture is developed in Chapter 52.
When a filter still wins
The honest engineer keeps a filter in the toolbox. On a microcontroller with kilobytes of RAM, a graph that grows without bound is a non-starter and a tightly tuned MSCKF or complementary filter is the right answer. When latency is measured in tens of microseconds (a flight-control inner loop reacting to gyro rates), a constant-time recursive update beats a solver that occasionally re-factorizes on loop closure. And when the model is genuinely close to linear-Gaussian over the horizon that matters, the smoother's relinearization advantage vanishes and you pay its overhead for nothing. The reason smoothing dominates SLAM and VIO specifically is that those problems combine three things that break filters and reward graphs: strong nonlinearity (rotations and projections), long time horizons where drift compounds (Chapter 23), and loop closures that must reach into the deep past. Change any of those and the balance can tip back. Smoothing is not universally superior; it is the correct answer to the particular shape of the SLAM problem.
Exercise
Take the five-pose graph of Listing 11.7 and run two experiments. (a) Solve it once, record the estimate of pose 4, then double the loop-closure noise sigmas and re-solve; explain why pose 4 moves less toward the origin when the loop factor is trusted less, connecting your answer to the information-weighting of Section 11.6. (b) Now start the optimizer from a far worse initial guess (multiply the initial pose offsets by five) and observe whether Levenberg-Marquardt still converges to the same solution. Write one sentence on why a filter, which linearizes only once at the arriving estimate, cannot recover from an equally bad starting point.
Self-Check
1. State the single most important reason a factor-graph smoother is more accurate than an extended Kalman filter on the same nonlinear SLAM data, in terms of what each does with linearization.
2. Smoothing over a whole trajectory sounds far more expensive than filtering. Name the two properties of the SLAM problem that make it real-time anyway.
3. Give one concrete deployment scenario in which you would still choose a filter over a factor-graph smoother, and say which property of that scenario drives the choice.
Lab 11
build a factor-graph pose estimator fusing odometry and landmark measurements with GTSAM.
Bibliography
Foundations of smoothing and factor graphs
The canonical monograph tying MAP inference, sparse linear algebra, and the Bayes tree together; the intellectual backbone of this entire chapter.
The algorithm that made full smoothing real-time by updating only the affected subtree, the reason smoothing displaced filtering in practice.
The influential empirical argument that bundle-adjustment-style optimization dominates filtering per unit of compute; the paper that framed the debate this section resolves.
Visual-inertial and modern SLAM systems
A production-grade smoother fusing vision and IMU with a multi-map back end; the reference system for graph-based SLAM today.
A widely deployed sliding-window graph optimizer with IMU pre-integration; the practical template for real-time VIO on constrained hardware.
Derives the IMU pre-integration factor that lets hundreds of inertial samples become one graph edge; the enabling trick for inertial factor graphs.
Differentiable and learned optimization
Embeds a differentiable bundle-adjustment layer inside a recurrent network, showing that even learned SLAM keeps a factor-graph solver in the loop.
Pineda, L. et al. (2022). Theseus: A Library for Differentiable Nonlinear Optimization. NeurIPS.
A differentiable factor-graph optimizer that lets gradients flow through the smoother, bridging classical estimation and end-to-end learning.
What's Next
In Chapter 12, we leave the world of estimating a state and turn to detecting when a signal has changed: point, contextual, and collective anomalies, statistical process control, and change-point detection. Where this chapter asked "what is the best trajectory given all the measurements," the next asks "which measurements no longer belong," and the residuals produced by the estimators you just built become the raw material for spotting a fault before it becomes a failure.