Factor Graphs

A factor graph writes an estimation problem as a product of simple functions, the factors, each of which depends on only a few unknowns. In SLAM the unknowns are robot or camera poses and landmark positions, and each factor comes from one measurement: an odometry reading links two consecutive poses, an image observation links a camera pose and a 3D point, a GPS fix constrains one pose. The graph is the bookkeeping of which factor touches which variables, and that bookkeeping turns out to be the key to computation. Its structure is the sparsity pattern of the problem’s Jacobian, the order in which variables are eliminated from it decides how expensive the solution is, and an update after a new measurement only touches the part of the graph the measurement reaches.

With Gaussian noise, finding the most probable values of all variables in a factor graph is a sparse nonlinear least-squares problem. Bundle adjustment is the special case with only cameras and points, and structure from motion, visual SLAM and visual-inertial odometry add other factor types to the same machinery. The robotics literature calls this smoothing and mapping (SAM), since it estimates the whole trajectory rather than only the current pose, and graph-based SLAM; the g2o library calls the same structure a hyper-graph. The account below follows Dellaert and Kaess’s (2017) monograph Factor Graphs for Robot Perception and the papers on which it is based.

Definition

A factor graph is a bipartite graph with two kinds of nodes, variables and factors, in which every edge joins a factor to a variable it depends on. It defines a global function as the product of its factors, each a function of only its adjacent variables. In estimation, the global function is the posterior density of the unknowns given the measurements, up to normalization, and the factors are the measurement likelihoods and priors (Dellaert and Kaess, 2017, §1.7).

Intuition

Dellaert and Kaess (2017) start from a toy SLAM problem: a robot at three poses $x_1, x_2, x_3$ sees two landmarks $l_1, l_2$. As a Bayesian network, a generative model, the problem has a node for every unknown and every measurement, with arrows from causes to effects: odometry depends on consecutive poses, a bearing depends on a pose and a landmark. Once the robot has its measurements, they are no longer random. Conditioning on them removes the measurement nodes, and what is left is a product of functions of the unknowns only:

$$p(X \mid Z) \propto p(x_1)\, p(x_2 \mid x_1)\, p(x_3 \mid x_2)\; p(l_1)\, p(l_2)\; l(x_1; z_1)\; l(x_1, l_1; z_2)\, l(x_2, l_1; z_3)\, l(x_3, l_2; z_4).$$

Each term is a factor. The factor graph draws a node for each of the five unknowns, a small node for each of the nine factors, and an edge from each factor to the variables it involves, so the likelihood $l(x_3, l_2; z_4)$ is joined only to $x_3$ and $l_2$.

Dellaert and Kaess give two reasons for the switch from Bayes nets. The measurements are given, so the graph should not spend nodes on them. And the factors are likelihoods, which are functions of the unknowns, not probability densities over them: a bearing measurement is Gaussian in the measured angle but not in the landmark position, and a single image point is consistent with a whole ray of 3D points. Only the product of several such uninformative factors pins the unknowns down. The third reason, developed in the rest of the monograph, is computational: the factor graph is the sparsity pattern of the problem, and the steps of a sparse solver are operations on the graph.

Formal Definition

The factorization

A factor graph $F = (U, V, E)$ has factor nodes $\phi_i \in U$, variable nodes $x_j \in V$, and edges $e_{ij} \in E$ only between factors and variables. With $X_i$ the variables adjacent to factor $\phi_i$, it defines

$$\phi(X) = \prod_i \phi_i(X_i)$$

(Dellaert and Kaess, 2017, eq. 1.24). Any function that factors this way has a factor graph; nothing in the definition requires the factors to be Gaussian, the variables to be continuous, or the function to be a probability.

MAP inference as nonlinear least squares

The maximum a posteriori (MAP) estimate maximizes $\phi(X)$. When every factor has the form of a Gaussian measurement model,

$$\phi_i(X_i) \propto \exp\left\{ -\tfrac{1}{2} \big\lVert h_i(X_i) - z_i \big\rVert^2_{\Sigma_i} \right\},$$

with measurement function $h_i$, measurement $z_i$, covariance $\Sigma_i$ and $\lVert e \rVert^2_\Sigma = e^\top \Sigma^{-1} e$, taking the negative logarithm turns the product into a sum (Dellaert and Kaess, 2017, §2.2):

$$X^{\mathrm{MAP}} = \arg\min_X \sum_i \big\lVert h_i(X_i) - z_i \big\rVert^2_{\Sigma_i}.$$

This is the maximum-likelihood estimate with priors added as extra factors, under the Gaussian correspondence between negative log densities and squared Mahalanobis distances. The minimization combines all the measurements and priors, which is what Dellaert and Kaess mean when they call it sensor fusion.

Gauss–Newton and Levenberg–Marquardt solve it by repeated linearization. With the measurement Jacobian $H_i = \partial h_i / \partial X_i$ at the current estimate $X^0$, and each term whitened by $\Sigma_i^{-1/2}$,

$$A_i = \Sigma_i^{-1/2} H_i, \qquad b_i = \Sigma_i^{-1/2}\big(z_i - h_i(X_i^0)\big), \qquad \Delta^\ast = \arg\min_\Delta \sum_i \lVert A_i \Delta_i - b_i \rVert^2 = \arg\min_\Delta \lVert A \Delta - b \rVert^2 .$$

Whitening removes the units, so pixels, metres and radians can share one cost (Dellaert and Kaess, 2017, §2.3). Stacking the blocks gives the sparse Jacobian $A$: each factor is a block row of $A$ and each variable a block column, so the factor graph is exactly the block sparsity pattern of the Jacobian (§3.1.2).

Information matrix and square root

The normal equations use the information matrix $\Lambda = A^\top A$, which is factored as

$$\Lambda = A^\top A = R^\top R ,$$

with $R$ upper triangular, followed by forward and back substitution (Dellaert and Kaess, 2017, §2.4). QR factorization of $A$ itself yields the same $R$ up to the signs of its rows, without forming $\Lambda$; it is more accurate, but slower by a factor of about two for dense matrices, and the authors note that sparse Cholesky and LDL factorization also outperform QR on sparse problems. Because $R$ is a square root of the information matrix, Dellaert and Kaess named the approach square root SAM (Dellaert and Kaess, 2006), after square root information filtering, which Dellaert and Kaess (2017, §2.6) trace to 1969 and JPL’s Mariner 10 missions.

At the block level, the sparsity pattern of $\Lambda$ is the adjacency matrix of an undirected graph on the variables, the Markov random field of the problem: two variables are joined when some factor involves both. The factor graph carries more information than this graph, since a ternary factor becomes a triangle in it and its origin is lost; in matrix terms, many Jacobians $A$ give the same $A^\top A$ (Dellaert and Kaess, 2017, §3.1.3).

Variables on manifolds

Rotations and poses do not form vector spaces, so the update $X^0 + \Delta$ is replaced by $a \oplus \xi$, which maps local coordinates $\xi \in \mathbb{R}^n$ to a neighbourhood of the current estimate $a$. For a matrix Lie group such as $SO(3)$ or $SE(3)$ the natural choice is $a \oplus \xi = a \cdot \exp \hat{\xi}$; any smooth retraction that agrees with it to first order also works and may be cheaper to compute (Dellaert and Kaess, 2017, §6.3). The Jacobians are then taken with respect to $\xi$, and the sparsity structure is unchanged. The rotation representations and geometric transformations articles describe the parameterizations and the groups.

Properties

Elimination turns a factor graph into a Bayes net

The variable elimination algorithm eliminates the variables one at a time in a chosen order (Dellaert and Kaess, 2017, §3.2). To eliminate $x_j$, it removes every factor adjacent to $x_j$, multiplies them into a product factor $\psi(x_j, S_j)$, whose other variables form the separator $S_j$, and splits that product into a conditional density on $x_j$ and a new factor on the separator,

$$\psi(x_j, S_j) = p(x_j \mid S_j)\, \tau(S_j).$$

The new factor $\tau(S_j)$ goes back into the graph. When every variable has been eliminated, the factor graph has become a Bayes net, $p(X) = \prod_j p(x_j \mid S_j)$.

For linear factors with Gaussian noise, this is sparse matrix factorization. The product factor collects the block rows of $A$ that touch $x_j$, and a partial QR factorization of that small dense matrix splits it into a conditional and a new factor (§3.3). Each conditional is the block row of $R$ for $x_j$:

$$p(x_j \mid S_j) \propto \exp\left\{ -\tfrac{1}{2} \lVert R_j x_j + T_j S_j - d_j \rVert^2 \right\},$$

a linear-Gaussian density whose mean $R_j^{-1}(d_j - T_j S_j)$ depends linearly on the separator. The whole elimination is multifrontal QR, and its result is the sparse factor $R$; the Bayes net is the graphical model of $R$, as the factor graph is that of $A$ and the Markov random field that of $\Lambda$ (§3.4). Solving the Bayes net in reverse elimination order, each variable from its separator,

$$x_j^\ast = R_j^{-1}\big(d_j - T_j S_j^\ast\big),$$

is back-substitution. Dellaert and Kaess stress that the algorithm does not need Gaussian densities: the same elimination performs MAP inference or marginalization with discrete variables, and the linear algebra is one instance of it (§3.5).

Variable ordering and fill-in

Every elimination order gives the same MAP estimate, but not the same cost (Dellaert and Kaess, 2017, §4.2). Eliminating a variable connects all the variables in its separator, since the new factor $\tau(S_j)$ involves all of them. Edges created this way that were absent from the original graph are fill-in: nonzeros in $R$ where $\Lambda$ has zeros (§4.3). Larger separators mean larger dense blocks to factor, more nonzeros to store, and slower back-substitution.

In the toy example, the order $l_1, l_2, x_1, x_2, x_3$ gives the Bayes net $p(l_1 \mid x_1, x_2)\, p(l_2 \mid x_3)\, p(x_1 \mid x_2)\, p(x_2 \mid x_3)\, p(x_3)$, and the reverse order gives a Bayes net that is almost fully dense. In a simulated 2D problem with 95 poses and 24 landmarks (333 unknowns), eliminating poses before landmarks gave an $R$ with 9,399 nonzeros, and the COLAMD ordering 4,168, with identical solutions (§4.2).

Finding the ordering with minimum fill-in is NP-hard (§4.4), so solvers use heuristics:

• Minimum degree orderings eliminate the least-connected variable next. Multiple minimum degree (MMD) eliminates all variables of minimal degree at once; approximate minimum degree (AMD) keeps only approximate degrees up to date and is faster. COLAMD (column approximate minimum degree), which Dellaert and Kaess (2017, §4.7) credit to Davis, Gilbert, Larimore and Ng (2004) among others, works on the columns of $A$ (Dellaert and Kaess, 2006).
• Nested dissection, which Dellaert and Kaess (2017, §4.7) credit to George (1973), recursively cuts the graph into two parts by a small separator and orders each part before its separator. For graphs that can be embedded in the plane, separators of size $O(\sqrt{n})$ exist and the factorization costs $O(n^{1.5})$; for chain-like graphs it costs $O(n)$, the familiar result for banded matrices (Dellaert and Kaess, 2017, §4.4.2). Graph partitioners such as METIS find the cuts in practice.

Two facts about SLAM and SfM orderings come from the same sources. First, the “Schur complement trick”, eliminating all landmarks first, is itself an ordering: it is the reduced camera system of bundle adjustment, and eliminating the poses first instead would leave a fully connected clique of landmarks. In the same section, a Cholesky factor fell from about 2.8 million nonzeros in its original ordering to about 250,000 with an approximate minimum degree ordering, and to about 130,000 when the heuristic was applied to the blocks of poses and landmarks rather than to scalar unknowns (Dellaert and Kaess, 2017, §4.5). The text presents these figures as the 333-unknown simulation above, but the matrices plotted with the counts have about 4,000 unknowns. The authors note that in structure from motion, with hundreds of thousands of points, the Schur complement trick can still be preferable for cache coherence and locality. Second, the variables of SLAM come in blocks (a pose is several scalars), and ordering the block graph rather than the scalar one is cheaper and better: Dellaert and Kaess (2006) report improvements of 2 to sometimes 100 times from running COLAMD on the block structure, with 15 as a good rule of thumb. They also note the worst case, in which every landmark is seen from every pose: then any ordering fills in completely, and the two natural orderings are the two forms of the partitioned inverse. Sensors with limited range and occlusion make this case rare in robotics.

Marginalization, filtering and fixed-lag smoothing

Removing old variables without losing their information is marginalization. Its cost depends on the form of the density (Dellaert and Kaess, 2017, §5.3.1): in covariance form it means deleting rows and columns; in information form it requires a Schur complement, $\Lambda_{yy} - \Lambda_{xy}^\top \Lambda_{xx}^{-1} \Lambda_{xy}$, which connects all the neighbours of the removed variables; and in square-root form it is free for a block of variables eliminated first, which can simply be dropped from $R$.

A fixed-lag smoother keeps only the last $n$ states: it alternates adding the factors of a new state with marginalizing the oldest. The Kalman filter is the fixed-lag smoother with $n = 1$ (Dellaert and Kaess, 2017, §5.3.2), and the extended Kalman filter and smoother are the special case of iSAM2, below, in which the Bayes tree is a chain (§5.4.3). Dropping the oldest variable from the factor graph instead of marginalizing it from the Bayes net is not equivalent: it discards the information of every earlier measurement. Marginalization is also why filtering scales badly in SLAM. Dellaert and Kaess (2017, §2.6) list the observations behind square root SAM: the covariance and information matrices of a filter both become fully dense over time, while the information matrix of the smoothing problem, which keeps every pose, is and stays sparse, and can be factored efficiently.

Incremental smoothing: iSAM

A SLAM system needs an estimate after every measurement, and the problem grows with each one. Solving it in batch every time wastes the work done before. iSAM (Kaess, Ranganathan and Dellaert, 2008) updates the factor $R$ instead. A new measurement adds a row to $A$; appending it below $R$ and removing its nonzeros with Givens rotations gives the new factor, and new variables are added as empty columns first. Because both $R$ and the new row are sparse, only a constant number of rotations is needed during exploration, and the authors’ simulation of a linear exploration task showed the number of rotations per step independent of the length of the trajectory.

Loops break this. Revisiting a place links the current pose to landmarks observed long before, which are linked to the old part of the trajectory, and the incremental factor fills in. The authors show this on a simulated figure-eight loop traversed twice: the factor $R$ fills in as the loops are closed, and after reordering by an approximate minimum degree heuristic it is again sparse. iSAM therefore combines incremental updates with periodic variable reordering, refactoring the whole Jacobian with a COLAMD block ordering, every 100 steps in their example, and it relinearizes the nonlinear factors only at those reordering steps, since the Jacobian is refactored then anyway. In the authors’ accepted manuscript, iSAM processed the Victoria Park laser dataset (6,969 frames after dropping those of a stopped vehicle, 3,640 landmark measurements, a 4 km trajectory recorded over 26 minutes, with several places traversed eight times) in 7.7 minutes including data association and a full solution after every step, on a 2 GHz Pentium M laptop. Its final factor had 207,422 entries for 21,187 variables, 9.79 per column. The same paper recovers the parts of the covariance needed for data association from $R$ without computing the full covariance matrix.

The Bayes tree and iSAM2

The Bayes net produced by elimination is chordal: every cycle longer than three has a chord. Grouping its variables into cliques gives a tree, the Bayes tree, a directed form of the clique tree (or junction tree) that keeps the elimination order (Dellaert and Kaess, 2017, §5.4.1; Kaess et al., 2012). Each node holds a conditional $p(F_k \mid S_k)$ of its frontal variables given a separator shared with its parent, and the root clique holds the variables eliminated last. The cliques correspond to groups of rows of $R$.

The tree makes incremental updates explicit. Information flows up the tree during elimination, from the leaves to the root, and a factor’s information enters only when its first variable is eliminated. A new factor on some variables can therefore affect only the cliques containing those variables and their ancestors up to the root (§5.4.2). To update, iSAM2 detaches that top part of the tree, turns it back into a factor graph, adds the new factors, re-eliminates it in any convenient order, and reattaches the unaffected subtrees. The authors first presented the Bayes tree in 2010 and iSAM2 in 2011, and their journal paper (Kaess et al., 2012) combines the two; iSAM2 adds three ingredients to the tree update:

• Incremental variable ordering. Applying COLAMD to the re-eliminated part minimizes fill-in for the current step only. iSAM2 instead uses constrained COLAMD (CCOLAMD) to force the most recently accessed variables to the end of the ordering, into the root clique, because the next measurements will probably involve them. On the simulated Manhattan dataset this sharply reduced the number of variables affected per step, except at large loop closures, at the cost of a small increase in fill-in.
• Fluid relinearization. A variable is relinearized only when its update since the last linearization point exceeds a threshold $\beta$; the cliques containing it, as frontal or separator variables, and their ancestors are then recomputed from the original nonlinear factors, using linear factors cached during elimination for the rest.
• Partial state updates. Back-substitution starts at the root and stops descending a branch when the change in its variables falls below a threshold $\alpha$.

This removes iSAM’s periodic batch steps. In the authors’ manuscript, iSAM2 ran with $\alpha = 0.001$, $\beta = 0.1$ and relinearization every 10 steps, and was compared with iSAM1, HOG-Man and SPA on 2D and 3D pose graphs and landmark datasets. The authors summarize that batch factorization (SPA, SAM) quickly gets expensive, that iSAM2 consistently performed better than SPA and similarly to iSAM1, and that iSAM1 degraded on datasets with denser constraints (at least five times as many constraints as poses) because of fill-in between its reorderings, while the fill-in of iSAM2’s factor stayed close to that of the batch ordering. They also note that their elimination code was a research implementation, compared with the optimized CHOLMOD library used by SPA, and that elimination took 46% of iSAM2’s time on the W10000 dataset.

Examples

Factor types in vision

• Bundle adjustment and structure from motion: variables are cameras (pose, possibly intrinsics) and 3D points; each image observation is a binary factor on one camera and one point, the reprojection error whitened by the pixel noise.
• Visual SLAM: the same projection factors, plus odometry factors between consecutive poses, a prior that fixes the gauge, and loop-closure factors from place recognition. SLAM has a worked 2D pose graph with one loop closure.
• Inertial navigation: an IMU produces measurements far faster than a camera. Dellaert and Kaess (2017, §7.1) describe how IMU measurements between camera frames are pre-integrated into a single factor, following Lupton and Sukkarieh, and that Forster et al. coupled such factors with fixed-lag smoothing in visual-inertial odometry; Forster et al.’s journal paper (2017, §VIII-B) itself computes the estimate with iSAM2 and states that it does not marginalize out past states (visual-inertial odometry); GTSAM’s README describes its preintegrated IMU factor as based on these two works.
• Calibration: unknown sensor parameters, shared by many measurements, become variables; the example graph in g2o’s documentation is a small SLAM problem whose measurement functions depend on unknown calibration parameters.

Ordering, fill-in and incremental updates in a small visual SLAM graph

The script builds a factor graph for a camera that drives once around a circle of radius 10 m, in 60 steps, looking outward at 120 points on a wall 16 m from the centre. Images are 640 × 480 pixels with a focal length of 300 pixels; a point is observed by every camera whose image it falls in, with 1-pixel noise. Consecutive poses are linked by odometry factors, a tight prior on the first pose fixes the gauge, and each point enters the graph at its second sighting, so that every intermediate problem has a unique solution. The factors are linearized at the ground truth by central differences, so all numbers below describe one Gauss–Newton step. A pose is perturbed by a rotation vector applied on the left of its camera-from-world rotation and an additive change of its translation, a retraction other than $a \cdot \exp \hat{\xi}$; the sparsity does not depend on this choice. Parts A to C then measure:

• A. the nonzeros of the Cholesky factor $R$ of the final information matrix in four orderings, and the difference between the Gauss–Newton steps they give;
• B. for a fixed creation ordering (each step’s new variables appended at the end), which variables’ rows of $R$ change at each step;
• C. during exploration, reordering the graph after step $s - 1$ by minimum degree, either freely or with the variables touched at step $s - 1$ forced last, as iSAM2 does, and then adding step $s$.

import sys
import numpy as np
from scipy.spatial.transform import Rotation as Rot

seed = int(sys.argv[1]) if len(sys.argv) > 1 else 0
rng = np.random.default_rng(seed)
T, L, f, W, H = 60, 120, 300.0, 640, 480         # poses (one lap), landmarks, focal length, image size
s_px, s_rot, s_tr, s_prior = 1.0, 0.01, 0.05, 1e-3  # noise: pixels, odometry (rad, m), prior on pose 0

# Ground truth: a camera on a 10 m circle looking outward at points on a wall 16 m from the centre.
def true_pose(t):                                    # camera-from-world rotation R and translation t
    th = 2 * np.pi * t / T
    R_wc = np.array([[np.sin(th), 0, np.cos(th)], [-np.cos(th), 0, np.sin(th)], [0, -1, 0]])
    return R_wc.T, -R_wc.T @ (10 * np.array([np.cos(th), np.sin(th), 0.0]))

poses = [true_pose(t) for t in range(T)]
phi = rng.uniform(0, 2 * np.pi, L)
X = np.column_stack([16 * np.cos(phi), 16 * np.sin(phi), rng.uniform(-2, 2, L)])

def project(R, t, Xw):
    Xc = R @ Xw + t
    return np.array([f * Xc[0] / Xc[2] + W / 2, f * Xc[1] / Xc[2] + H / 2]), Xc[2]

def perturb(R, t, d):                                # local update: rotation vector, translation
    return Rot.from_rotvec(d[:3]).as_matrix() @ R, t + d[3:]

# Render the observations: landmark j is seen in image t if it projects inside the 640 x 480 frame.
vis = np.array([[(lambda uv, z: z > 0 and 0 <= uv[0] < W and 0 <= uv[1] < H)(*project(*p, x))
                 for x in X] for p in poses])
dim = {v: 6 for v in range(T)} | {T + j: 3 for j in range(L)}   # variables: poses 0..T-1, landmarks T..
born = {v: v for v in range(T)} | {T + j: int(np.where(vis[:, j])[0][1]) for j in range(L)}
# (a landmark joins the graph at its second sighting, with both observations, so every system is solvable)

def proj_factor(t, j):
    z = project(*poses[t], X[j])[0] + s_px * rng.standard_normal(2)
    def res(dp, dx):
        return (project(*perturb(*poses[t], dp), X[j] + dx)[0] - z) / s_px
    return (max(t, born[T + j]), (t, T + j), res)

def odo_factor(t):
    (Ra, ta), (Rb, tb) = poses[t - 1], poses[t]
    Rz = Rot.from_rotvec(s_rot * rng.standard_normal(3)).as_matrix() @ Rb @ Ra.T
    tz = tb - Rb @ Ra.T @ ta + s_tr * rng.standard_normal(3)
    def res(da, db):
        (Ra_, ta_), (Rb_, tb_) = perturb(Ra, ta, da), perturb(Rb, tb, db)
        Rr = Rb_ @ Ra_.T                             # relative pose b from a, compared with the measurement
        return np.r_[Rot.from_matrix(Rz.T @ Rr).as_rotvec() / s_rot, (tb_ - Rr @ ta_ - tz) / s_tr]
    return (t, (t - 1, t), res)

factors = [(0, (0,), lambda d: d / s_prior)]         # (step added, variables, whitened residual)
for t in range(T):
    factors += [odo_factor(t)] if t > 0 else []
    factors += [proj_factor(t, j) for j in np.where(vis[t])[0]]

# Linearize every factor at the ground truth (central differences): whitened Jacobian blocks.
off = np.cumsum([0] + [dim[v] for v in range(T + L)])
N = off[-1]
def linearize(vs, res):
    zero = [np.zeros(dim[v]) for v in vs]
    r0, blocks = res(*zero), []
    for k, v in enumerate(vs):
        cols = []
        for i in range(dim[v]):
            dp, dm = [z.copy() for z in zero], [z.copy() for z in zero]
            dp[k][i], dm[k][i] = 1e-6, -1e-6
            cols.append((res(*dp) - res(*dm)) / 2e-6)
        blocks.append(np.column_stack(cols))
    return np.hstack(blocks), np.concatenate([np.arange(off[v], off[v + 1]) for v in vs]), r0

lin = [linearize(vs, res) for _, vs, res in factors]
Lam_steps, g_steps, Lam, g = [], [], np.zeros((N, N)), np.zeros(N)
for step in range(T):                                # information matrix after each step
    for (s, _, _), (J, cols, r) in zip(factors, lin):
        if s == step:
            Lam[np.ix_(cols, cols)] += J.T @ J
            g[cols] -= J.T @ r
    Lam_steps.append(Lam.copy()); g_steps.append(g.copy())

def graph(step):                                     # variable adjacency (the MRF of the information matrix)
    adj = {}
    for s, vs, _ in factors:
        if s <= step:
            for v in vs:
                adj.setdefault(v, set()).update(set(vs) - {v})
    return adj

def min_degree(adj, last=()):
    """Greedy minimum-degree elimination ordering; variables in `last` are eliminated after all others."""
    adj, order, rest = {v: set(n) for v, n in adj.items()}, [], set(adj)
    while rest:
        v = min((rest - set(last)) or rest, key=lambda u: (len(adj[u] & rest), u))
        nb = adj[v] & rest
        for a in nb:
            adj[a] |= nb - {a}                       # eliminating v connects its neighbours (fill-in)
        order.append(v)
        rest.remove(v)
    return order

def factor_R(step, order):                           # Cholesky triangle R (Lam = R^T R) in this ordering
    idx = np.concatenate([np.arange(off[v], off[v + 1]) for v in order])
    return np.linalg.cholesky(Lam_steps[step][np.ix_(idx, idx)]).T, idx

def changed(R_old, order_old, R_new):
    """Variables whose rows of R differ after an update (new variables are appended at the end)."""
    n, pos, out, tol = R_old.shape[0], 0, [], 1e-9 * np.abs(R_new).max()
    for v in order_old:
        rows = slice(pos, pos + dim[v])
        pos += dim[v]
        if np.abs(R_new[rows, :n] - R_old[rows]).max() > tol or np.abs(R_new[rows, n:]).max(initial=0) > 0:
            out.append(v)
    return out

# A. The full graph after one lap, factored in four orderings.
adj = graph(T - 1)
creation = sorted(adj, key=lambda v: (born[v], v >= T, v))
orders = {"creation order": creation,
          "landmarks first": sorted(adj, key=lambda v: v < T),
          "poses first": sorted(adj, key=lambda v: v >= T),
          "minimum degree": min_degree(adj)}
Lam_full = Lam_steps[-1]
dx_ref = np.linalg.solve(Lam_full, g_steps[-1])
print(f"{N} unknowns, {len(factors)} factors; nonzeros in the upper triangle of the information matrix: "
      f"{int((np.triu(Lam_full) != 0).sum())}")
for name, order in orders.items():
    R, idx = factor_R(T - 1, order)
    dx = np.zeros(N)
    dx[idx] = np.linalg.solve(R, np.linalg.solve(R.T, g_steps[-1][idx]))   # forward, back substitution
    print(f"  {name:16s} nonzeros in R {int((R != 0).sum()):7d}   max |dx - dx_ref| {np.abs(dx - dx_ref).max():.0e}")

# B. Incremental updates in creation order: which rows of R change at each step?
loop = next(s for s, vs, _ in factors if any(v < T and s - v >= 10 for v in vs))
counts, prev = {}, None
for step in range(T):
    order = [v for v in creation if born[v] <= step]
    R, _ = factor_R(step, order)
    if prev:
        counts[step] = (len(changed(*prev, R)), len(prev[1]), int((R != 0).sum()))
    prev = (R, order)
before = np.array([counts[s][0] for s in range(1, loop)])
print(f"\nB. steps 1-{loop - 1} (exploring): rows recomputed per step min {before.min()}, "
      f"median {np.median(before):.0f}, max {before.max()} variables")
for s in range(loop, T):
    print(f"   step {s} (loop closed): {counts[s][0]:3d} of {counts[s][1]} variables recomputed, "
          f"nonzeros in R {counts[s][2]}")

# C. Reorder after step s-1, then add step s: unconstrained vs. recent variables forced last.
res = {"creation order": [], "minimum degree": [], "min. degree, recent last": []}
for s in range(10, loop):
    adj_prev = graph(s - 1)
    recent = {v for st, vs, _ in factors if st == s - 1 for v in vs}
    new = [v for v in creation if born[v] == s]
    for name, order in [("creation order", [v for v in creation if born[v] < s]),
                        ("minimum degree", min_degree(adj_prev)),
                        ("min. degree, recent last", min_degree(adj_prev, last=recent))]:
        R_old, _ = factor_R(s - 1, order)
        R_new, _ = factor_R(s, order + new)
        res[name].append((len(changed(R_old, order, R_new)), int((R_new != 0).sum())))
print(f"\nC. steps 10-{loop - 1}, mean over steps")
for name, v in res.items():
    v = np.array(v)
    print(f"   {name:24s} rows recomputed {v[:, 0].mean():5.1f}   nonzeros in R {v[:, 1].mean():7.0f}")

Output (NumPy 2.3.5, SciPy 1.16.3; the script runs in a few seconds):

720 unknowns, 851 factors; nonzeros in the upper triangle of the information matrix: 16551
  creation order   nonzeros in R   51897   max |dx - dx_ref| 6e-12
  landmarks first  nonzeros in R   38694   max |dx - dx_ref| 7e-12
  poses first      nonzeros in R  142658   max |dx - dx_ref| 7e-12
  minimum degree   nonzeros in R   35562   max |dx - dx_ref| 6e-12

B. steps 1-53 (exploring): rows recomputed per step min 1, median 12, max 24 variables
   step 54 (loop closed): 162 of 162 variables recomputed, nonzeros in R 29253
   step 55 (loop closed): 173 of 173 variables recomputed, nonzeros in R 35361
   step 56 (loop closed): 168 of 176 variables recomputed, nonzeros in R 39378
   step 57 (loop closed): 173 of 177 variables recomputed, nonzeros in R 43503
   step 58 (loop closed): 175 of 178 variables recomputed, nonzeros in R 47682
   step 59 (loop closed): 176 of 179 variables recomputed, nonzeros in R 51897

C. steps 10-53, mean over steps
   creation order           rows recomputed  12.0   nonzeros in R   14753
   minimum degree           rows recomputed  50.9   nonzeros in R   15665
   min. degree, recent last rows recomputed  11.8   nonzeros in R   13869

A. The information matrix is the same in every ordering; only its factor changes. All four orderings give the same Gauss–Newton step to within $10^{-11}$, so the ordering is purely a matter of cost, but the factor’s size varies by a factor of four. Eliminating the poses first is by far the worst: by construction, each eliminated pose connects the landmarks it saw to the next pose, so the landmark block fills in, the clique Dellaert and Kaess describe. Eliminating the landmarks first, the reduced camera system of bundle adjustment, does well here, because each point is seen only from a few neighbouring poses and the pose graph it leaves behind is a ring. Greedy minimum degree, a dozen lines in the script, beats every hand-made ordering. The creation order, natural for an incremental solver, is worse than both because of the loop: the first poses and landmarks share factors with the last ones, so eliminating them early carries fill around the whole ring.

B. While the camera explores, a step recomputes between 1 and 24 variables’ rows of $R$, a median of 12, however many variables the graph already holds: the new factors touch only the newest variables, which sit at the end of the ordering. At step 54 the camera sees a point it first saw from pose 0, and that factor links the end of the ordering to its beginning: every row of $R$ changes, and in the six loop-closing steps the factor grows from 29,253 to 51,897 nonzeros, the full-graph value from part A. This is the behaviour Kaess et al. (2008) describe for iSAM, constant-time updates during exploration and fill-in at loop closures, and why iSAM reorders periodically: reordering the final graph by minimum degree would cut the factor to 35,562 nonzeros (part A).

C. Reordering for fill alone has a cost for the next update. When the graph is reordered freely by minimum degree before each step, the next step recomputes on average 50.9 variables’ rows, four times as many as with the creation order, because the recently observed variables, which the next step’s factors will touch, end up anywhere in the ordering. Forcing them last, as iSAM2 does with constrained COLAMD, brings this back to 11.8. Kaess et al. (2012) reported a small increase in fill-in from their constrained ordering; here the constrained ordering also has the lower fill. In our reading, this is because the fill is counted after the new step’s variables have been appended at the end of each ordering, which the unconstrained ordering did not anticipate, whereas iSAM2 reorders only the re-eliminated top of the tree and its comparison ordered the affected variables including the new ones. The script takes the seed as its argument; with seeds 1 and 2 the four orderings in part A rank the same way, every existing row of $R$ changes at the first loop-closing step, and the constrained ordering again recomputes the fewest rows in part C with the lowest mean fill.

The example measures structure only. A real solver iterates the linearization, relinearizes as the estimate moves, factors $R$ with a sparse library rather than the dense Cholesky used here for clarity, and would detect and verify the loop closure rather than know it.

History

Dellaert and Kaess (2017, §3.6) trace the elimination algorithm to Gauss, who used it in the early 1800s for least-squares problems in astronomy, computing the orbits of Ceres and Pallas; the method he found is what is now called Cholesky factorization. The graph view came with sparse linear algebra: Parter appears to have been the first to analyze sparse matrices with graphs, and described Cholesky factorization on a graph as repeatedly making a vertex’s neighbourhood a clique and removing the vertex. The same elimination reappeared in dynamic programming, database query processing, constraint satisfaction and probabilistic reasoning, where it became the peeling and bucket elimination algorithms.

Factor graphs themselves come from coding theory. In the same account, Tanner introduced bipartite graphs to describe low-density parity-check codes; Wiberg and colleagues extended Tanner graphs with hidden state variables; and Frey and colleagues introduced factor graphs, in which the factors can be arbitrary functions. Kschischang, Frey and Loeliger (2001) showed that the sum-product algorithm on factor graphs covers a wide range of settings, including hidden Markov models, with the Viterbi algorithm and the Kalman filter as special cases. In robotics, it was noticed that the information matrix of SLAM is the matrix of a Markov random field (Dellaert and Kaess, 2017, §3.6).

Smoothing over whole trajectories had a long history in surveying and photogrammetry as bundle adjustment, and in computer vision as structure from motion, before a flurry of work between 2000 and 2005 applied it to SLAM (Dellaert and Kaess, 2017, §2.6). Square root SAM (Dellaert and Kaess, 2006) argued that factoring the sparse smoothing information matrix is faster than the EKF yet exact, that column ordering heuristics automatically exploit the locality of SLAM, and that factorization has a graphical-model interpretation. iSAM (2008) made the factorization incremental, the Bayes tree (2010) explained incremental factorization as editing a tree of cliques, and iSAM2 (2011, journal version 2012) used it to drop iSAM’s periodic batch steps. Dellaert and Kaess’s 2017 monograph gathers this line of work in the language of factor graphs.

Software

Three open-source C++ libraries implement this kind of sparse nonlinear least squares and are named in the SLAM literature. Their documentation describes them in different terms.

• GTSAM (Georgia Tech Smoothing and Mapping) describes itself as implementing smoothing and mapping “using Factor Graphs and Bayes Networks” as its computing paradigm rather than sparse matrices (README, release 4.3.0, the latest release as of November 2026). Its source offers COLAMD, METIS, natural and custom orderings, and constrained COLAMD variants that force given variables last or first. Its iSAM2 implementation defaults to Gauss–Newton steps (Powell’s dog-leg is an option), Cholesky rather than QR factorization, a relinearization threshold of 0.1, and relinearization only every 10 calls to its update function; release 4.3.0 also has an option, off by default, that triggers a batch reordering when the number of nonzeros in the Bayes tree has doubled since the last one. GTSAM ships the preintegrated IMU factor mentioned above.
• g2o (general graph optimization) describes itself as a framework for optimizing nonlinear error functions that can be represented as a graph, with SLAM and bundle adjustment as typical instances (README, release 20241228_git). Its documentation formulates the problem on a directed hyper-graph, whose hyper-edges, one per error term, may connect any number of vertices, the same structure as a factor graph; it notes that the block structure of the Hessian is the adjacency matrix of the hyper-graph. Updates on manifolds are written with an operator $\boxplus$. g2o leaves the linear solve to external libraries: its block solver implements the Schur complement and passes the reduced system to a linear solver based on CHOLMOD, CSparse or preconditioned conjugate gradients.
• Ceres Solver is described in its README as a library for modeling and solving large, complicated optimization problems, used in production at Google since 2010; it speaks of parameter blocks and residual blocks rather than variables and factors. For sparse problems, its documentation describes sparse Cholesky factorization with an approximate minimum degree ordering by default, or nested dissection, and Schur-based solvers (dense, sparse and iterative) for bundle adjustment. A user can pass an ordering as elimination groups; for the Schur solvers the first group must be an independent set, such as all the 3D points, and if the user leaves the choice to Ceres, it finds one with an approximate maximum independent set algorithm.

Least squares is sensitive to wrong data associations, which no ordering can fix. GTSAM (Dellaert, 2012), iSAM and g2o (Kümmerle et al., 2011) let factors use robust error functions, and switchable constraints, max-mixtures and the RRR method model the association decision itself (Dellaert and Kaess, 2017, §7.4); in our reading these matter most in pose graph optimization, the variant with only poses as variables, where a single false loop closure can bend the whole trajectory.

Common Misconceptions

• “The variable ordering changes the answer.” Every ordering gives the same MAP estimate up to rounding, as part A shows; the ordering decides the size of the factor and therefore the time and memory.
• “A sparse information matrix means a cheap solve.” The cost depends on the factor $R$, not on $\Lambda$. The same 16,551-nonzero information matrix in the example gives factors with between 35,562 and 142,658 nonzeros.
• “The Schur complement is a separate trick from sparse factorization.” Eliminating landmarks first is one elimination ordering. It is the natural one for bundle adjustment, but Dellaert and Kaess (2017, §4.5) show that a general-purpose heuristic can do far better on SLAM problems.
• “Smoothing over the whole trajectory must be slower than filtering.” Filtering marginalizes old poses, which makes its information matrix dense; smoothing keeps them, and its information matrix stays sparse (Dellaert and Kaess, 2017, §2.6).
• “Marginalizing a variable means deleting it.” Deleting a variable together with its factors throws away the information they carried; marginalization keeps it as a new factor on the variable’s neighbours (§5.3.2).
• “iSAM2 is an approximation of batch optimization.” The Bayes tree update is exact for the linearized problem; the approximations are the thresholds that decide when to relinearize and how far to propagate the solution, and Kaess et al. (2012) evaluate how they trade accuracy for speed.
• “Factor graphs are only for Gaussian problems.” The definition and the elimination algorithm hold for any factors; Gaussian factors are the case in which elimination is sparse matrix factorization (Dellaert and Kaess, 2017, §3.5).

Where It Is Used

• SLAM back-ends: the SLAM article describes the front-end that produces factors and the back-end that optimizes them; Cadena et al. (2016), cited there, call MAP estimation of this kind the de facto standard. Robot perception puts it among the estimators robots use.
• Bundle adjustment and structure from motion: bundle adjustment is the factor graph of cameras and points, solved with the Schur complement ordering; structure from motion and systems such as COLMAP run it repeatedly.
• Visual and visual-inertial odometry: fixed-lag smoothers over a window of recent frames, with projection and preintegrated IMU factors, and marginalization of older states (visual odometry).
• Pose graphs and dense mapping: pose-only graphs with odometry and loop-closure factors, and dense systems that correct their maps through such a graph; Dellaert and Kaess (2017, §7.2) describe Kintinuous, which extends KinectFusion to large environments: it applies loop closures to a pose graph and carries the correction to the dense map by deforming it.
• Filtering as a special case: the Kalman filter is the fixed-lag smoother of lag one on a chain-shaped factor graph.