Math 5 · Factor graphs & sparse inference

Why this chapter exists. Chapters 3 and 4 reduced estimation to repeatedly solving a sparse linear least-squares problem \(\min_\delta\lVert A\delta - b\rVert^2\). This chapter is about solving it fast and incrementally. The central idea: solving a sparse linear system is the same as variable elimination on a graph, so the graph tells you everything about cost, parallelism, and what changes when new data arrives. This is the theory of GTSAM's inference/ and linear/ directories and of iSAM2.

1. Factor graphs, formally

A factor graph is a bipartite graph with variable nodes \(X = \{x_1,\dots,x_n\}\), factor nodes \(\{\phi_1,\dots,\phi_m\}\), and an edge between \(\phi_k\) and \(x_j\) iff \(\phi_k\) depends on \(x_j\). It represents the function

For Gaussian MAP estimation, \(\phi_k(X_k) = \exp\big(-\tfrac12\lVert h_k(X_k)-z_k\rVert^2_{\Sigma_k}\big)\), so maximizing \(\phi\) is minimizing \(\sum_k\tfrac12\lVert h_k-z_k\rVert^2_{\Sigma_k}\) (chapter 3 §6). In GTSAM: NonlinearFactorGraph is the list of \(\phi_k\), each NonlinearFactor knows its keys() (\(X_k\)), and Values holds an assignment to \(X\).

After linearization at the current estimate, each factor becomes a Gaussian factor on tangent-space increments, whitened (chapter 1 §5):

a JacobianFactor with one \(A\) block per key. The collection is a GaussianFactorGraph. Stacking all of them gives the global \(A\) (rows = factors, block columns = variables) and \(b\).

2. Three views of the same sparsity

For the PlanarSLAM example (chapter 3 §9: poses \(x_1,x_2,x_3\), landmarks \(l_1,l_2\)):

The MRF is what elimination acts on: eliminating a variable adds edges among its MRF neighbours. The factor graph is finer-grained. Two different factor graphs, e.g. one 3-way factor versus three pairwise factors, can have the same MRF.

3. Variable elimination: the algorithm

Given an ordering \(x_{\pi(1)},\dots,x_{\pi(n)}\):

for j in ordering:
    F_j  ← remove all factors that involve x_j from the graph
    S_j  ← (all variables of F_j) \ {x_j}                  # the separator
    ψ    ← product of factors in F_j                         # a joint over x_j ∪ S_j
    p(x_j | S_j), τ(S_j) ← factorize ψ = p(x_j | S_j) · τ(S_j)
    add τ(S_j) back into the graph                           # the "new factor" / message
record conditionals  →  Bayes net  ∏_j p(x_j | S_j)

The factorization \(\psi = p(x_j\mid S_j)\,\tau(S_j)\) is the chain rule. For Gaussians it is the conditional/Schur-complement pair of chapter 3 §4, computed by partial QR or partial Cholesky (chapter 1 §8–9). This loop is EliminateableFactorGraph::eliminateSequential + EliminationTree. The dense step is the Eliminate function you pass (EliminateQR, EliminateCholesky, EliminatePreferCholesky, or the discrete and hybrid equivalents).

The result is a Bayes net: a DAG where each \(x_j\) points from its separator. For Gaussians each conditional is \(R_{jj}x_j + S_j x_{S_j} = d_j\), one block row of an upper-triangular matrix \(R\). The Bayes net is the sparse square-root information matrix \(R\), with \(R^\top R = A^\top A\) (up to the column permutation given by the ordering).

Solving = back-substitution in reverse elimination order: the last variable has no parents and is solved directly; each earlier one plugs in its already-solved parents (chapter 1 §7). GaussianBayesNet::optimize.

4. Elimination by hand

Three scalar variables on a line: a prior \(x_1 = 0\), odometry \(x_2-x_1=1\) and \(x_3-x_2=1\), and a "GPS" reading \(x_3=2.5\). All σ = 1, so the problem is already whitened:

We eliminate in information (Hessian) form. A factor \(\tfrac12\lVert a^\top x - \beta\rVert^2\) contributes \(aa^\top\) to \(\Lambda\) and \(a\beta\) to \(\eta\).

Eliminate \(x_1\). The factors touching \(x_1\) are the prior and the first odometry, with separator \(\{x_2\}\):

• Conditional: \(x_1 = (\eta_1 - \Lambda_{12}x_2)/\Lambda_{11} = (-1 + x_2)/2\), with variance \(1/2\).
• New factor on \(x_2\) (Schur complement): \(\tilde\Lambda = 1 - \frac{(-1)(-1)}{2} = \frac12\), \(\tilde\eta = 1 - \frac{(-1)(-1)}{2} = \frac12\). So \(x_2\sim\mathcal N(1, 2)\), which is right: \(x_1\sim\mathcal N(0,1)\) plus a unit step with unit variance.

Eliminate \(x_2\). The factors are \(\tau(x_2)\) and the second odometry, with separator \(\{x_3\}\):

• Conditional: \(x_2 = (-\tfrac12 + x_3)/\tfrac32\).
• New factor on \(x_3\): \(\tilde\Lambda = 1-\tfrac{1}{3/2} = \tfrac13\) and \(\tilde\eta = 1 - \tfrac{(-1)(-1/2)}{3/2} = \tfrac23\), so the odometry chain alone predicts \(x_3\sim\mathcal N(2, 3)\).

Eliminate \(x_3\). The factors are \(\tau(x_3)\) and the GPS, giving \(\Lambda = \tfrac13+1 = \tfrac43\) and \(\eta = \tfrac23+2.5 = \tfrac{19}{6}\). So \(x_3 = \frac{19/6}{4/3} = 2.375\), with variance \(\tfrac34\).

Back-substitute: \(x_2 = (-0.5+2.375)/1.5 = 1.25\) and \(x_1 = (-1+1.25)/2 = 0.125\).

Check against the normal equations: \(A^\top A = \begin{bmatrix}2&-1&0\\-1&2&-1\\0&-1&2\end{bmatrix}\), \(A^\top b = (-1, 0, 3.5)\). Plugging in \((0.125, 1.25, 2.375)\) gives \((-1, 0, 3.5)\). ✓ The odometry says \(x_3=2\) and the GPS says \(2.5\); they are fused according to their relative information, and the correction is spread back along the chain. That is "smoothing".

5. Ordering and fill-in: the elimination game

Play elimination on the MRF:

Eliminate \(v\): connect all current neighbours of \(v\) into a clique, then delete \(v\).

The added edges are the fill-in, i.e. new nonzeros in \(R\). The final graph (original + fill edges) is chordal: every cycle of length ≥4 has a chord. Each eliminated variable plus its neighbours at that moment forms a clique.

Cost. Eliminating \(x_j\) with separator \(S_j\) is a dense operation on a block of size \(\lvert\{x_j\}\cup S_j\rvert\) (times the variable dimension). For Cholesky it costs \(O(\lvert S_j\rvert^2)\) per eliminated scalar, summed over all variables. The largest clique over an optimal ordering, minus one, is the graph's treewidth. Elimination is cheap exactly when treewidth is small. Chains and trees have treewidth 1; a \(k\times k\) grid has treewidth \(k\); a complete graph has \(n-1\).

What creates fill in SLAM?

• Odometry chains eliminated in time order: each pose's only later neighbour is the next pose, so there is zero fill (\(R\) is block-bidiagonal).
• A landmark seen from many poses: eliminating the landmark first connects all those poses (the arrow matrix, chapter 1 §13). Eliminating it last costs nothing until the end.
• Loop closures: a factor \(x_1\)–\(x_{100}\). Eliminating \(x_1,\dots,x_{99}\) in order carries \(x_{100}\) along in every separator: each conditional gets one extra parent, and the fill is \(O(\text{loop length})\).

Ordering heuristics (finding the optimal one is NP-complete):

Rule of thumb: COLAMD is excellent for typical SLAM; METIS can win on large, mesh-like problems (big SfM, dense loop closures).

6. The elimination tree

Record, for each eliminated variable \(j\), its parent: the first variable of its separator \(S_j\) in elimination order. This gives a forest, the elimination tree (EliminationTree). Its key properties:

• Dependencies: a variable's elimination needs only the factors (messages \(\tau\)) from its children. Disjoint subtrees are independent, so they can be eliminated in parallel. GTSAM's treeTraversal runs them as TBB tasks when GTSAM_WITH_TBB is on.
• Structure of \(R\): the nonzero pattern of row \(j\) of \(R\) is \(j\) plus a subset of its ancestors.

7. Cliques, junction trees, multifrontal elimination

Eliminating one scalar variable at a time is inefficient: many tiny dense operations. If consecutive variables in a chain of the elimination tree have nested separators (\(S_{j} = \{x_{j+1}\}\cup S_{j+1}\)), merge them into one clique (supernode) and eliminate them together as one dense block. The resulting tree of cliques is a junction tree (JunctionTree, a ClusterTree). Each node holds the factors to combine plus the list of frontal variables to eliminate.

Multifrontal elimination processes the junction tree bottom-up: each clique assembles its own factors plus its children's messages, does one dense partial QR/Cholesky on the frontal block (BLAS-3 efficient), emits a conditional \(p(F_k\mid S_k)\), and sends the Schur complement \(\tau(S_k)\) to its parent. GTSAM's eliminateMultifrontal does this; GTSAM 4.3's MultifrontalSolver precomputes the symbolic structure once and refactorizes numerically with packed storage, for LM's repeated solves.

8. The Bayes tree

The output of multifrontal elimination is a tree of cliques, each holding a conditional density

and the joint is \(p(X) = \prod_kp(F_k\mid S_k)\). This is the Bayes tree (Kaess et al. 2010): a directed version of the junction tree whose nodes are the cliques of the chordal Bayes net.

9. Incremental inference: why the Bayes tree makes iSAM2 possible

Claim. Adding a new factor on variables \(K\) only changes the cliques that contain variables in \(K\), plus all their ancestors up to the root.

Why. Elimination runs leaves → root. A clique's conditional is computed from the factors on its frontal variables plus the messages from its children. A new factor on \(x\in K\) first enters at the clique where \(x\) is frontal, changes that clique's message, which changes its parent's computation, and so on up to the root. Subtrees not on that path receive no new information, so their conditionals, and the messages they sent upward, stay valid.

The iSAM2 update (ISAM2::update → recalculate in nonlinear/ISAM2.cpp):

1. Find the cliques containing the affected variables (new factors, relinearized variables).
2. Detach the "top": those cliques and all their ancestors. The subtrees hanging below become orphans.
3. Re-create a factor graph for the top: the original (re)linearized factors of the top's variables, plus each orphan's cached message \(\tau(S_{orphan})\) (ISAM2Clique::cachedFactor(), gathered in GetCachedBoundaryFactors), plus the new factors.
4. Re-order the top with constrained COLAMD, putting the most recently affected variables last, so they end up near the root and the next update touches a small top.
5. Eliminate into a new top Bayes tree and re-attach the orphans at the clique containing their separator's first variable.
6. If too much of the tree is affected (GTSAM: about 65% of variables, or when fill has grown past adaptiveReorderThreshold), do a full batch re-elimination instead.

The cost is roughly proportional to the size of the top. During exploration that is a few cliques, nearly constant time. A loop closure touches everything along the loop, so updates are occasionally expensive.

Fluid relinearization. The Bayes tree encodes a linearization at points \(\Theta\), and the current estimate is \(\Theta\oplus\Delta\). When some variable's \(\lVert\Delta_j\rVert\) exceeds relinearizeThreshold (default 0.1, per-dimension thresholds possible), iSAM2 moves its linearization point (\(\Theta_j\leftarrow\Theta_j\oplus\Delta_j\)) and marks the cliques containing it, plus their ancestors, for re-elimination with the factors relinearized. The check runs every relinearizeSkip (default 10) updates.

Partial state update ("wildfire"). After an update, \(\Delta\) is recomputed by back-substitution starting at the new root, but propagation into a subtree stops when the change at its separator is below wildfireThreshold. Far-away parts of the map that didn't move aren't touched.

10. Marginal covariances from the square-root form

Uncertainty is \(\Sigma = \Lambda^{-1} = (R^\top R)^{-1}\), but never form it. It's dense, \(O(n^2)\) memory. You usually need only a few blocks: the diagonal blocks for each pose's uncertainty, or a few off-diagonal ones for data association. With \(\Sigma = R^{-1}R^{-\top}\) and \(R\) upper triangular, the entries satisfy (Golub & Plemmons; Kaess & Dellaert 2009)

computed from the bottom-right up, visiting only entries where \(R\) has nonzeros. The Bayes tree version is the clique recursion of §8. gtsam::Marginals (and ISAM2::marginalCovariance) eliminates if needed, then calls marginalFactor and inverts the small resulting information block.

11. Schur complement for bundle adjustment and smart factors

In bundle adjustment the variables are cameras \(C\) (few, 6–9 dof each) and 3D points \(P\) (many, 3 dof each). Each measurement links one camera to one point, so in the order (cameras, points)

Points are conditionally independent given the cameras, so \(\Lambda_{PP}\) is block-diagonal and trivially inverted. Eliminate all points first (that's the ordering), which leaves the reduced camera system

which is small (cameras only) and solved directly or by PCG. Then back-substitute each point independently. Two cameras become coupled in the reduced system iff they observe a common point.

Smart factors (SmartProjectionPoseFactor, SmartProjectionRigFactor) do this elimination inside the factor. They hold all observations of one landmark, triangulate it internally, and linearize to the Schur complement on the cameras only. The landmark never becomes a variable. RegularImplicitSchurFactor keeps that Schur complement implicit (it stores \(F, E, P\) and applies \((F^\top F - F^\top E P E^\top F)\) to vectors on demand) for iterative solvers.

Fixed-lag smoothing is the same thing over time: eliminate states older than the window and keep their marginal factor, a LinearContainerFactor (BatchFixedLagSmoother, IncrementalFixedLagSmoother). That factor is frozen at its linearization point. This is the source of the consistency issues that "first-estimate Jacobians" (FEJ) address in VIO.

12. Iterative solvers: when even sparse factorization is too big

Conjugate gradient (CG) solves \(H\delta = g\) for symmetric PD \(H\) by minimizing \(\tfrac12\delta^\top H\delta - g^\top\delta\) along directions that are mutually \(H\)-conjugate (\(p_i^\top Hp_j=0\)), so each step's progress is never undone:

r0 = g − H δ0;  p0 = r0
for k = 0, 1, ...:
    α = (r_kᵀ r_k) / (p_kᵀ H p_k)
    δ_{k+1} = δ_k + α p_k
    r_{k+1} = r_k − α H p_k
    β = (r_{k+1}ᵀ r_{k+1}) / (r_kᵀ r_k)
    p_{k+1} = r_{k+1} + β p_k

It needs only products \(Hp = A^\top(Ap)\), which can be computed factor by factor without assembling any matrix (GTSAM: GaussianFactorGraph::multiplyHessianAdd, transposeMultiplyAdd). Its convergence depends on the condition number:

Preconditioning replaces \(H\) with \(M^{-1}H\), where \(M\approx H\) is cheap to invert, so \(\kappa(M^{-1}H)\ll\kappa(H)\). Common choices: Jacobi (\(M=\diag H\)) and block-Jacobi (per-variable blocks). The subgraph preconditioner (Dellaert et al. 2010) splits the graph into a spanning-tree-like subgraph \(T\) and the rest \(C\). \(T\) has zero-fill elimination (tree, leaves first), so \(M = R_T^\top R_T\) is exact for \(T\), and the preconditioned system \(I + (A_CR_T^{-1})^\top(A_CR_T^{-1})\) is well-conditioned when \(C\) is "small". GTSAM: ConjugateGradientSolver, PCGSolver + Preconditioner, SubgraphSolver + SubgraphBuilder.

13. Beyond Gaussians: the same algorithm on other algebras

Variable elimination only needs two operations on factors, combine (multiply) and marginalize out (sum or max), and the distributive law. So the same templated code in gtsam/inference runs for:

Discrete factors in GTSAM are algebraic decision diagrams (DecisionTree, with equal leaves merged), which keeps tables small. In hybrid problems the number of modes grows exponentially as discrete variables are eliminated, so HybridSmoother prunes low-probability modes.

14. Check yourself

1. Redo §4 eliminating \(x_3\) first, then \(x_2\), then \(x_1\). Is there fill? Is the answer the same?
2. Play the elimination game on a 4-cycle \(x_1\)–\(x_2\)–\(x_3\)–\(x_4\)–\(x_1\). Which edge is fill? What's the treewidth?
3. Why does iSAM2 put recently-updated variables last in the ordering?
4. In BA with 100 cameras and \(10^5\) points, what size is the reduced camera system? Which ordering produces it?
5. Why can CG run without ever assembling \(A^\top A\)?
6. Sketch the Bayes tree for an odometry chain of 5 poses eliminated in time order. What does adding a factor between \(x_1\) and \(x_5\) invalidate?