M11-T3 — Factor Graphs

Learning objectives

After this lesson, you should be able to:

• Describe a problem as a factor graph and assemble its information matrix from the factors.
• Marginalise a set of variables with the Schur complement and verify it against the covariance.
• Explain fill-in and why the elimination order changes the cost by orders of magnitude.
• Relate the graph to what a SLAM back end does: sparse least squares on a pose graph.

The intuition

Draw one circle for each unknown: the six positions along a corridor and two landmarks. Draw a small square for each piece of evidence, joined to the unknowns it involves: an odometry reading links two consecutive poses, a landmark reading links one pose and one landmark, a prior pins one pose. This is a factor graph. It is also the problem: the posterior is proportional to the product of all the factors, so the log-posterior is the sum of their squared residuals.

The middle panel shows why this matters. Each factor adds a small block to the information matrix of the least-squares problem, at the rows and columns of the variables it touches. The matrix therefore has a nonzero entry only where two variables share a factor: a band along the chain, plus the landmark rows and columns. For a hundred thousand poses nearly all entries are zero, and the solver can use that.

What can go wrong is shown in the right panel. Solving means eliminating variables one by one; eliminating a variable ties all its neighbours together (they become correlated), and so creates new nonzero entries: fill-in. In a graph of 60 poses and 12 landmarks, eliminating the poses in time order creates 371 new edges (the landmarks become linked to everything), eliminating the landmarks first creates 346 (the poses that saw the same landmark become linked), and a greedy minimum-degree order creates 11. The same problem, the same answer, a thirty-fold difference in the cost.

A factor graph makes the sparsity of the problem visible. Solving it quickly is the art of eliminating variables in an order that keeps the matrix sparse.

Definitions

Variable node

An unknown: a pose, a landmark, a calibration parameter.

Factor $f_k$

A term of the objective that depends on a few variables: $\lVert\mathbf{z}_k-h_k(\theta_{S_k})\rVert^2_{R_k^{-1}}$, where $S_k$ is the set of variables it touches.

Information matrix $\Lambda$

$\sum_k J_k^\top R_k^{-1}J_k$: the Gauss–Newton matrix. Its covariance is $\Lambda^{-1}$.

Information vector $\eta$

$\sum_k J_k^\top R_k^{-1}\mathbf{z}_k$: the right-hand side, so that $\Lambda\hat\theta=\eta$ for a linear problem.

Marginalisation

Removing variables from the problem while keeping their effect on the rest, by integrating them out of the Gaussian.

Fill-in

New nonzero entries in the matrix created by eliminating a variable.

Derivation: marginalisation as a Schur complement, and fill-in

The posterior is a Gaussian in information form: $p(\theta)\propto\exp\bigl(-\tfrac12\theta^\top\Lambda\theta+\eta^\top\theta\bigr)$. Split $\theta=(\mathbf{a},\mathbf{b})$ and integrate out $\mathbf{a}$.

1. $$-\tfrac12\mathbf{a}^\top\Lambda_{aa}\mathbf{a}-\mathbf{a}^\top\Lambda_{ab}\mathbf{b}+\eta_a^\top\mathbf{a}-\tfrac12\mathbf{b}^\top\Lambda_{bb}\mathbf{b}+\eta_b^\top\mathbf{b}$$

   The exponent written in blocks.

2. $$\text{complete the square in }\mathbf{a}:\quad -\tfrac12(\mathbf{a}-\mathbf{a}^*)^\top\Lambda_{aa}(\mathbf{a}-\mathbf{a}^*)+\tfrac12(\eta_a-\Lambda_{ab}\mathbf{b})^\top\Lambda_{aa}^{-1}(\eta_a-\Lambda_{ab}\mathbf{b})$$

   With $\mathbf{a}^*=\Lambda_{aa}^{-1}(\eta_a-\Lambda_{ab}\mathbf{b})$ the best a for a given b. The first term integrates to a constant.

3. $$\Rightarrow;-\tfrac12\mathbf{b}^\top\bigl(\Lambda_{bb}-\Lambda_{ba}\Lambda_{aa}^{-1}\Lambda_{ab}\bigr)\mathbf{b}+\bigl(\eta_b-\Lambda_{ba}\Lambda_{aa}^{-1}\eta_a\bigr)^\top\mathbf{b}$$

   Collecting the terms in b: a Gaussian in information form with the Schur complement as its information matrix.

In covariance form the same operation is trivial: marginalising a Gaussian means dropping the rows and columns of the variables. The work moved: in covariance form marginalisation is free and conditioning is expensive; in information form conditioning is free (drop rows) and marginalisation costs a Schur complement. SLAM uses the information form because its factors are naturally sparse there.

Fill-in

The entry $(i,j)$ of $\Lambda_{ba}\Lambda_{aa}^{-1}\Lambda_{ab}$ is nonzero whenever variables $i$ and $j$ of $\mathbf{b}$ are both connected, through $\Lambda_{aa}^{-1}$, to a common eliminated variable. Eliminating one variable $v$ therefore makes all its neighbours mutually dependent: a clique of its neighbours is added to the graph.

1. $$\text{eliminate }v;\Rightarrow;\text{new edges between all pairs of neighbours of }v$$

   Because the Schur complement adds a rank-one term $\Lambda_{\cdot v}\Lambda_{vv}^{-1}\Lambda_{v\cdot}$, which is nonzero on every pair of neighbours.

2. $$\text{cost}\approx\sum_v d_v^3,\quad d_v=\text{degree of }v\text{ at its elimination}$$

   The work to eliminate a variable grows with the cube of the number of neighbours it has at that time, and the fill-in with its square. A good order keeps the degrees small.

3. $$\text{minimum degree:}\ \text{eliminate the variable with the fewest neighbours first}$$

   A greedy rule that works very well in practice (COLAMD and AMD are refinements of it).

Why the orders differ

Landmarks are seen from many poses, so eliminating a landmark early links all those poses into a clique (10 poses means 45 new edges for one landmark). Eliminating poses in time order carries each pose's links forward, and when a pose that sees a landmark is eliminated, the landmark becomes linked to the next pose and to every landmark seen along the way. The minimum-degree rule starts from the poses at the ends of the chain (degree one or two) and works inwards, keeping every clique small. Real SLAM solvers choose such an order automatically.

Marginalising is exact for Gaussians but it destroys sparsity (Schur complement fills in). Whether a SLAM back end is fast or hopeless depends on the elimination order, not on the number of variables.

A worked example

Two programs.

1. The small problem. Six poses in a line at 0, 1, …, 5 m and two landmarks at 3.5 and 7 m. The factors are a prior on p0 (0.01 m), five odometry readings (noise 0.1 m) and six landmark readings (noise 0.2 m). The information matrix is 8 × 8 and has the sparsity of the graph: 30 nonzero entries of 64. The MAP estimate gives 0.99, 1.90, 2.99, 3.96, 5.00 for p1…p5 and 3.29 and 7.11 for the landmarks. The standard deviations grow along the chain, 0.01 at the anchor to 0.205 at p5, and are 0.167 and 0.210 for the landmarks.
2. Marginalisation. Eliminating all six poses by the Schur complement leaves a 2 × 2 information matrix [[48.8, −20.1], [−20.1, 31.03]]. Its inverse is [[0.0279, 0.0181], [0.0181, 0.0439]], identical to the landmark block of the full covariance. The two landmarks, which no factor linked, now have an off-diagonal entry: they became correlated through the poses they were seen from. Their uncertainties are not independent any more, and the map has to remember it.
3. Fill-in. A graph of 60 poses and 12 landmarks (179 edges; each landmark seen from 10 consecutive poses). Eliminating the poses in time order creates 371 new edges, landmarks first creates 346, and the greedy minimum-degree order creates 11.

import math
from robotprob import Rng, solve, inverse, matmul, transpose, madd, msub, matvec

# 1. a small factor graph in one dimension: poses p0..p5, landmarks m0, m1. Each factor is (variables, row of coefficients, measurement, sigma)
names = ['p0', 'p1', 'p2', 'p3', 'p4', 'p5', 'm0', 'm1']
idx = {n: i for i, n in enumerate(names)}
rng = Rng(3)
true = {'p0': 0.0, 'p1': 1.0, 'p2': 2.0, 'p3': 3.0, 'p4': 4.0, 'p5': 5.0, 'm0': 3.5, 'm1': 7.0}
factors = [(['p0'], [1.0], 0.0, 0.01)]                                    # the prior anchors the first pose
for i in range(5):                                                         # odometry: p_{i+1} - p_i = 1
    factors.append((['p%d' % i, 'p%d' % (i + 1)], [-1.0, 1.0], 1.0 + rng.gauss(0.0, 0.1), 0.1))
for i, j in [(1, 'm0'), (2, 'm0'), (3, 'm0'), (4, 'm1'), (5, 'm1'), (3, 'm1')]:   # landmark observations: m - p_i
    factors.append((['p%d' % i, j], [-1.0, 1.0], true[j] - true['p%d' % i] + rng.gauss(0.0, 0.2), 0.2))
n = len(names)
L = [[0.0] * n for _ in range(n)]; eta = [0.0] * n
for vs, coef, z, s in factors:                                             # information form: Lambda += a^T a / s^2, eta += a^T z / s^2
    for a, va in enumerate(vs):
        eta[idx[va]] += coef[a] * z / s ** 2
        for b, vb in enumerate(vs):
            L[idx[va]][idx[vb]] += coef[a] * coef[b] / s ** 2
sol = solve(L, eta)
cov = inverse(L)
print('information matrix: which variables share a factor (X) and which do not (.)')
print('      ' + ' '.join(n_.rjust(2) for n_ in names))
for i, n_ in enumerate(names):
    print(n_.rjust(4), ' ', ' '.join(('X' if abs(L[i][j]) > 1e-12 else '.').rjust(2) for j in range(n)))
print('MAP estimate:', {n_: round(sol[i], 2) for i, n_ in enumerate(names)})
print('truth:       ', {k: v for k, v in true.items()})
print('std of every variable from the covariance:', {n_: round(math.sqrt(cov[i][i]), 3) for i, n_ in enumerate(names)})

# marginalise the poses by the Schur complement: keep only the landmarks
P_ = [idx['p%d' % i] for i in range(6)]; M_ = [idx['m0'], idx['m1']]
sub = lambda A, R, C: [[A[r][c] for c in C] for r in R]
Lpp, Lpm, Lmm = sub(L, P_, P_), sub(L, P_, M_), sub(L, M_, M_)
schur = msub(Lmm, matmul(matmul(transpose(Lpm), inverse(Lpp)), Lpm))
cov_mm = sub(cov, M_, M_)
print('information on the landmarks after marginalising the poses (Schur complement):')
for row in schur: print('  ', [round(v, 2) for v in row])
print('its inverse                        :', [[round(v, 4) for v in row] for row in inverse(schur)])
print('landmark block of the full covariance:', [[round(v, 4) for v in row] for row in cov_mm])
print('the landmarks were not linked by any factor before; now their information matrix has an off-diagonal entry:', round(schur[0][1], 2))

# 2. fill-in: the cost of eliminating variables in different orders, for a longer problem (symbolic elimination on the graph)
def graph(T, nl, per, seed=1):
    r = Rng(seed)
    adj = {('p', i): set() for i in range(T)}
    adj.update({('m', j): set() for j in range(nl)})
    for i in range(T - 1):
        adj[('p', i)].add(('p', i + 1)); adj[('p', i + 1)].add(('p', i))
    for j in range(nl):
        start = int(j * (T - per) / max(1, nl - 1))
        for i in range(start, start + per):
            adj[('m', j)].add(('p', i)); adj[('p', i)].add(('m', j))
    return adj
def fill_in(adj, order):
    g = {k: set(v) for k, v in adj.items()}
    added = 0
    for v in order:
        nb = list(g[v])
        for a in range(len(nb)):
            for b in range(a + 1, len(nb)):
                if nb[b] not in g[nb[a]]:
                    g[nb[a]].add(nb[b]); g[nb[b]].add(nb[a]); added += 1
        for u in nb: g[u].discard(v)
        del g[v]
    return added
def min_degree(adj):
    g = {k: set(v) for k, v in adj.items()}; order = []
    while g:
        v = min(g, key=lambda k: (len(g[k]), k)); order.append(v)
        nb = list(g[v])
        for a in range(len(nb)):
            for b in range(a + 1, len(nb)):
                g[nb[a]].add(nb[b]); g[nb[b]].add(nb[a])
        for u in nb: g[u].discard(v)
        del g[v]
    return order
T, NL, PER = 60, 12, 10
g = graph(T, NL, PER)
poses = [('p', i) for i in range(T)]; marks = [('m', j) for j in range(NL)]
edges = sum(len(v) for v in g.values()) // 2
print('a graph with', T, 'poses and', NL, 'landmarks,', edges, 'edges (each landmark seen from', PER, 'consecutive poses)')
print('elimination order           new edges created (fill-in)')
print('landmarks first, then poses ', fill_in(g, marks + poses))
print('poses in time order, then landmarks', fill_in(g, poses + marks))
print('greedy minimum degree       ', fill_in(g, min_degree(g)))

information matrix: which variables share a factor (X) and which do not (.)
      p0 p1 p2 p3 p4 p5 m0 m1
  p0    X  X  .  .  .  .  .  .
  p1    X  X  X  .  .  .  X  .
  p2    .  X  X  X  .  .  X  .
  p3    .  .  X  X  X  .  X  X
  p4    .  .  .  X  X  X  .  X
  p5    .  .  .  .  X  X  .  X
  m0    .  X  X  X  .  .  X  .
  m1    .  .  .  X  X  X  .  X
MAP estimate: {'p0': 0.0, 'p1': 0.99, 'p2': 1.9, 'p3': 2.99, 'p4': 3.96, 'p5': 5.0, 'm0': 3.29, 'm1': 7.11}
truth:        {'p0': 0.0, 'p1': 1.0, 'p2': 2.0, 'p3': 3.0, 'p4': 4.0, 'p5': 5.0, 'm0': 3.5, 'm1': 7.0}
std of every variable from the covariance: {'p0': 0.01, 'p1': 0.1, 'p2': 0.137, 'p3': 0.162, 'p4': 0.186, 'p5': 0.205, 'm0': 0.167, 'm1': 0.21}
information on the landmarks after marginalising the poses (Schur complement):
   [48.8, -20.1]
   [-20.1, 31.03]
its inverse                        : [[0.0279, 0.0181], [0.0181, 0.0439]]
landmark block of the full covariance: [[0.0279, 0.0181], [0.0181, 0.0439]]
the landmarks were not linked by any factor before; now their information matrix has an off-diagonal entry: -20.1
a graph with 60 poses and 12 landmarks, 179 edges (each landmark seen from 10 consecutive poses)
elimination order           new edges created (fill-in)
landmarks first, then poses  346
poses in time order, then landmarks 371
greedy minimum degree        11

In robotics

What a SLAM back end does

A pose-graph or bundle-adjustment solver builds a factor graph from the odometry, scan matches, loop closures and landmark observations, linearises it, builds the sparse information matrix, orders the variables (AMD or COLAMD), factorises it by sparse Cholesky and solves for the correction, then repeats. Libraries such as GTSAM, g2o and Ceres implement exactly this. The SLAM course describes the pipeline in its back-end lessons.

Marginalisation in practice

• Sliding windows (visual–inertial odometry): the oldest poses are marginalised out when new ones arrive, keeping the problem at a fixed size. The Schur complement leaves a dense block on the variables they were linked to, a “prior factor” that must be kept consistent with the linearisation point.
• Landmark elimination in bundle adjustment: the many landmarks are eliminated first by the Schur complement (each is linked to only a few cameras, so the cliques are small), leaving a smaller “reduced camera system”. This is why bundle adjustment is feasible with millions of points.
• Keyframes: marginalising non-key poses keeps only a skeleton of poses and the information between them.

Reading the graph

The graph says which parts of the problem are independent (disconnected components), which variables decide the others (high degree), and where a bad measurement does harm (it enters the matrix at the rows of its variables). A visual inspection of the sparsity pattern of the information matrix is a quick diagnostic.

Covariance recovery

The covariance is $\Lambda^{-1}$, which is dense even when $\Lambda$ is sparse. Full covariance is therefore rarely computed for large problems; the marginal of a few variables is recovered from the factorisation (as the check against the Schur complement shows), and the rest is left implicit.

Common misconceptions

Key takeaways

1. A factor graph has a node per variable and a factor per measurement; the posterior is the product of the factors, and the information matrix is the sum of their blocks Aᵀ A.
2. The information matrix is sparse in exactly the pattern of the graph; the covariance is generally dense.
3. Marginalising by the Schur complement Λ_bb − Λ_ba Λ_aa⁻¹ Λ_ab is exact; its inverse is the block of the full covariance (checked to four decimals).
4. Eliminating a variable connects all its neighbours: fill-in. In the example, landmarks that no factor linked became correlated (−20.1).
5. The elimination order can change the fill-in thirty-fold (371 or 346 new edges against 11): minimum-degree orderings are standard in SLAM solvers.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• F. Dellaert and M. Kaess, “Factor Graphs for Robot Perception”, Foundations and Trends in Robotics 6 (1–2), 2017 , the standard reference on this lesson’s material. Free to read as the author’s PDF; copyrighted.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, chapter 11 (the GraphSLAM algorithm: information form, reduction of landmarks, and the Schur complement). Paid book.
• C. Stachniss, “Graph-based SLAM using Pose Graphs” (video lecture) . Free to watch.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , the sparse batch estimation and the Schur complement in the chapters on SLAM and bundle adjustment. Free to read as the author’s PDF; the book is copyrighted.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

The last lesson of the module takes the factor graph to its main use: nonlinear least squares and pose-graph optimisation, with loop closures, gauge freedom, and robust losses against wrong constraints.

Related content

Nothing below is required to finish this lesson.

• Before
  M11-T2: MAP Estimation and Weighted Least Squares

  Show that MAP estimation with Gaussian noise is weighted least squares, and read the information matrix as the confidence of the answer.

Other courses on this site treat neighbouring topics from their own angle:

• SLAM M4-T2: Pose Graphs
• SLAM M4-L1: Visualize a Pose Graph
• UAV M4-T12: Pose graph optimization
• SLAM: Robust Optimization