Incremental smoothing (iSAM2) and GTSAM

"You keep handing me one more measurement and asking me to re-solve the universe. I have started keeping my old work. Only the parts you disturbed get erased."

A Thrifty AI Agent

This section assumes you are fluent in the MAP-as-least-squares formulation, the Gauss-Newton normal equations, and sparse factorization from Section 11.2, and comfortable with the information-matrix view of a Gaussian from Chapter 4. We build toward the loop-closure and marginalization mechanics of Section 11.4, which lean directly on the data structure introduced here.

Why re-solving from scratch does not scale

Recall the linearized normal equations from batch smoothing. At each Gauss-Newton iteration we solve \(A^\top A \, \delta = A^\top b\), where \(A\) is the stacked, whitened Jacobian of every factor and \(\delta\) is the update to all state variables at once. The matrix \(\Lambda = A^\top A\) is the information matrix: sparse, symmetric, positive-definite. Solving means factorizing it, typically \(\Lambda = R^\top R\) by Cholesky or, working directly on \(A\), a QR factorization that yields the same upper-triangular square-root factor \(R\). Back-substitution on \(R\) then delivers \(\delta\).

Here is the waste. When you append one new pose and one new odometry factor, almost every entry of \(R\) is identical to what it was a moment ago. The new measurement is local: it constrains the newest variable and a handful of its neighbors, nothing on the far side of the trajectory. Yet a naive batch solve rebuilds and refactorizes the whole matrix, an \(O(n^3)\) operation in the dense case and still superlinear for the sparse trajectories we care about. The question that defines this section is: can we reuse the old \(R\) and repair only the disturbed corner? The answer is yes, and the machinery that makes the "disturbed corner" precise is a data structure called the Bayes tree.

The Bayes tree: elimination you can edit

Factorizing \(\Lambda\) is variable elimination. You pick an ordering of the state variables, and eliminating each one in turn produces a row of \(R\) plus fill-in edges among that variable's remaining neighbors. The result of eliminating everything is a Bayes net: a directed, chordal graph whose conditional densities are exactly the rows of \(R\). The Bayes tree, introduced by Kaess and colleagues in 2011, is that Bayes net reorganized into a tree of cliques, where each clique groups variables eliminated together and each edge carries the separator they share with their parent. It is the square-root information matrix drawn as a junction tree.

Why bother? Because a tree makes locality explicit. When a new factor arrives involving variable \(x_k\), only the cliques on the path from \(x_k\)'s clique up to the root are affected. Everything hanging off other branches is provably untouched: its rows of \(R\) are still valid, its stored densities still correct. Re-solving means detaching that top sub-tree, converting its cliques back into factors, adding the new factor, and re-eliminating just that small piece. The deeper, older branches are never reopened. This is the structural insight that converts "refactorize \(n\) variables" into "refactorize the few that the measurement could plausibly influence."

iSAM2: fluid relinearization and partial back-substitution

The Bayes tree handles the linear reuse. iSAM2 adds two more economies that matter for nonlinear sensor problems. The first is fluid relinearization. A batch Gauss-Newton solve relinearizes every factor at every iteration, because a factor's Jacobian is only valid near the operating point it was computed at. But most variables barely move once the trajectory has settled. iSAM2 tracks, per variable, how far the current estimate has drifted from the point where its factors were last linearized, and relinearizes a factor only when that drift crosses a threshold (a few centimeters, a fraction of a degree). Variables that are not moving keep their old Jacobians for free. The second economy is partial back-substitution. After editing the tree, you must back-substitute to get the updated \(\delta\), but a local measurement typically nudges only nearby variables. iSAM2 propagates the update outward from the changed cliques and stops descending a branch as soon as the delta falls below a threshold, so distant, unaffected poses are never even read.

Two more details make it robust in the long run. Elimination ordering controls fill-in, and a bad ordering can turn a sparse tree dense; iSAM2 periodically re-orders variables with a heuristic such as COLAMD, and constrains the newest variables to the end so they stay cheap to update. And because the whole thing is a repeated linearize-edit-solve loop, it inherits the trust-region option of Section 11.2: iSAM2 can run its steps in a dogleg region to stay stable when a loop closure snaps the graph hard.

GTSAM in practice

GTSAM (Georgia Tech Smoothing And Mapping) is the reference implementation. You describe your problem as a NonlinearFactorGraph of typed factors over variables that live on manifolds (Pose2 on \(SE(2)\), Pose3 on \(SE(3)\), so updates compose correctly rather than adding blindly), hand batches of new factors to an ISAM2 object, and read back a live estimate. The loop below runs a small incremental pose graph: a prior anchors the first pose, then each step adds one odometry BetweenFactor and calls update, which does exactly the Bayes-tree edit described above.

import gtsam
from gtsam import Pose2, ISAM2, NonlinearFactorGraph, Values
from gtsam import BetweenFactorPose2, PriorFactorPose2
from gtsam.symbol_shorthand import X          # X(k) names pose k

prior_noise = gtsam.noiseModel.Diagonal.Sigmas([0.10, 0.10, 0.05])  # x, y, theta
odom_noise  = gtsam.noiseModel.Diagonal.Sigmas([0.20, 0.20, 0.10])

isam = ISAM2()                 # holds the Bayes tree across calls
odom = Pose2(1.0, 0.0, 0.0)    # command: drive 1 m forward each step

for k in range(5):
    graph   = NonlinearFactorGraph()   # ONLY the new factors this step
    initial = Values()                 # ONLY the new variable this step
    if k == 0:
        graph.add(PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise))
        initial.insert(X(0), Pose2(0, 0, 0))
    else:
        graph.add(BetweenFactorPose2(X(k - 1), X(k), odom, odom_noise))
        prev = isam.calculateEstimate().atPose2(X(k - 1))
        initial.insert(X(k), prev.compose(odom))   # warm start on the manifold
    isam.update(graph, initial)        # incremental solve: edits, does not rebuild
    est = isam.calculateEstimate()
    print(k, est.atPose2(X(k)))

Notice what the loop never does: it never rebuilds the full graph and never re-solves the whole trajectory. It appends and calls update. That is incremental smoothing in three lines of body code.

What's Next

In Section 11.4, we confront the two operations that most stress an incremental smoother: loop closure, where recognizing a previously seen place adds a factor spanning far across the graph and forces the Bayes tree to re-eliminate a long branch, and marginalization, the disciplined way to drop old variables when even a smoother must bound its memory, turning the fixed-lag estimator that sits between a filter and a full smoother.