Incremental smoothing

SLAM is a growing estimation problem: every new keyframe adds variables and factors to the factor graph. Solving the whole nonlinear least-squares problem from scratch at every step (batch optimization) gets more expensive as the trajectory grows — yet each new measurement usually affects only a small, recent part of the graph. Incremental smoothing exploits this: update the previous solution with just the new information, instead of recomputing everything.

The linear-algebra view

Solving the linearized least-squares system means factorizing either the measurement Jacobian ( $J = QR$ , then back-substituting $R\,\Delta\mathbf{x} = \mathbf{d}$ ) or the information matrix ( $H = J^TJ$ via Cholesky). A new measurement appends new rows to $J$ . The key observation of iSAM (incremental Smoothing and Mapping, Kaess et al.): the existing triangular factor $R$ can be updated for those new rows with a sequence of Givens rotations — a low-rank modification — rather than rebuilt from scratch. Because the update only fills in entries connected to the new measurement’s variables, most of $R$ is untouched. iSAM still needed periodic batch steps to re-linearize and reorder variables, since both linearization points and the variable ordering (which controls fill-in) go stale as the estimate evolves.

iSAM2 and the Bayes tree

iSAM2 removed the periodic-batch crutch by introducing the Bayes tree, a tree-structured data structure derived from variable elimination on the factor graph: running elimination to completion yields a Bayes net whose cliques organize into a directed tree. The Bayes tree encodes exactly which variables’ solutions depend on which others — each clique’s solution depends only on its ancestors toward the root.

When a new factor arrives:

Only the cliques on the paths from the affected variables to the root are re-eliminated — the rest of the tree, typically the vast majority, is untouched.
Fluid relinearization: a variable is re-linearized only when its estimate has moved beyond a threshold since its last linearization point, so nonlinearity is handled where (and only where) it matters.
Partial state updates: solution updates propagate down from the modified cliques and stop where the change falls below a threshold, so only estimates that actually moved are recomputed.

The result is near-constant-time updates for exploration-style trajectories, while remaining exact up to the linearization thresholds — this is not a fixed-lag approximation that throws information away; the full smoothing problem over all variables is maintained. Large loop closures still touch a large part of the tree (correctly so — they genuinely affect everything), which shows up as an occasional latency spike in profiling.

Choosing a back-end style

	Batch smoothing	Incremental (iSAM2)	Fixed-lag + marginalization
Variables kept	all	all	recent window only
Cost per update	grows with trajectory	~constant while exploring; spikes at loop closures	strictly bounded
Information	exact	full history, exact up to relinearization thresholds	old states summarized in a frozen linearized prior
Typical use	offline mapping, SfM	online SLAM with loop closures	embedded VIO

iSAM2 is implemented in GTSAM and is the back-end of choice when you want full-history smoothing at real-time rates: Kimera-VIO, for example, runs GTSAM’s iSAM2 with structureless “smart” projection factors for incremental visual-inertial estimation, and LIO-SAM feeds its LiDAR-inertial factor graph to the same solver. The main alternative back-end style — a fixed-lag sliding window with marginalization — bounds cost by discarding old variables instead; incremental smoothing bounds cost by updating cleverly while keeping them.

Practical notes

The relinearization threshold trades speed for exactness; a too-loose threshold quietly degrades accuracy in high-curvature (fast-rotation) segments.
Structureless/“smart” factors (which eliminate landmarks analytically inside the factor) keep the Bayes tree small for vision problems with many short-lived landmarks.
If your platform cannot tolerate the loop-closure latency spike, the common architecture is a fast fixed-lag odometry front-end plus an iSAM2 (or pose-graph) smoother running behind it.

Why it matters for SLAM

Incremental smoothing is what makes the “smoothing beats filtering” insight practical at real-time rates: you get the accuracy of full nonlinear optimization over the whole trajectory without paying batch cost at every frame. When you see a system described as “GTSAM/iSAM2-based,” you now know its computational story: Bayes-tree updates, fluid relinearization, and exact full-history MAP inference. It is the standard back-end for VIO and LiDAR-inertial systems that need loop closures folded in seamlessly (e.g., LIO-SAM, Kimera).