Factor Graph SLAM: GTSAM, iSAM2 and Pose Graph Optimization

A robot that drives a kilometre of corridors with wheel odometry ends up somewhere that is not where it thinks it is. Every step adds a little error, the errors compound, and by the time the robot sees a door it has already mapped, the two copies of that door sit metres apart. The question is not how to prevent drift, which is impossible, but how to use the moment of recognition to repair the entire trajectory at once. That is the job of factor graph SLAM: a formulation in which every measurement is a constraint, the whole history is one sparse optimization problem, and a solver redistributes the error over every pose that contributed to it.

It matters now because the factor graph has become the common back end of modern lidar-inertial and visual-inertial systems, and because incremental solvers such as iSAM2 make it cheap enough to run online on an embedded computer. If you understand the structure, you can read the source of GTSAM, g2o or Ceres-based SLAM stacks and predict where they will be fast, where they will blow up, and why.

What this covers: the maximum a posteriori (MAP) view of SLAM as nonlinear least squares, factor graphs versus Bayes nets, the sparsity of the information matrix, Gauss-Newton and Levenberg-Marquardt, elimination ordering and fill-in, the iSAM2 Bayes tree, IMU preintegration factors, robust loop-closure handling, a comparison with g2o, Ceres and filters, and runnable Python for a small pose graph.

Context and Background

Simultaneous localization and mapping (SLAM) estimates a robot’s trajectory and a map of its surroundings from noisy, partial sensor data. For two decades the dominant formulation was filtering. The extended Kalman filter (EKF) kept a single Gaussian over the current pose and all landmarks, which meant a dense covariance matrix whose size grew quadratically with the map. Particle filters such as FastSLAM sidestepped that by sampling, but they struggle in high-dimensional states and degenerate when loop closures arrive. If you want the filtering side in depth, our article on digital twin state estimation with Kalman, particle and ensemble filters covers the trade-offs, and our overview of SLAM architecture for autonomous robots places the estimator inside the larger perception pipeline.

The shift to smoothing began with the observation that the SLAM information matrix, the inverse of the covariance, is naturally sparse when you keep the whole trajectory rather than marginalizing old poses away. Dellaert and Kaess formalized this in their 2006 square-root SAM work, and the later monograph “Factor Graphs for Robot Perception” (Foundations and Trends in Robotics, 2017) is the standard reference for the whole framework. The incremental solver at the heart of this article, iSAM2, was published by Kaess and colleagues in the International Journal of Robotics Research, volume 31, issue 2, pages 217 to 236, in 2012. The paper’s abstract describes a sparse nonlinear incremental optimization algorithm that uses incremental variable re-ordering and fluid relinearization, and states that it eliminates the need for periodic batch steps.

The software ecosystem has consolidated around a few libraries. GTSAM (Georgia Tech Smoothing and Mapping) is a C++ library with Python bindings, maintained in the open at the borglab/gtsam repository. As of this writing, the latest stable release shown on its releases page is 4.2.2, while the 4.3 line exists only as alpha pre-releases (4.3a0 to 4.3a2), whose notes warn that parts of the API are not yet stable. The Python wheels for 4.2 are installable with pip install gtsam. g2o, Ceres Solver and several lidar-inertial stacks implement the same underlying mathematics with different data structures and different emphasis.

The thesis of this article is that the factor graph is best understood as a modeling language and the solver as a replaceable engine. The modeling language tells you what a measurement is: a probabilistic constraint on a small set of variables. The engine tells you how fast you can satisfy all the constraints, and that depends almost entirely on the graph’s sparsity structure and on the order in which you eliminate variables. Once those two ideas are clear, most of the rest of SLAM back-end engineering is bookkeeping.

The Factor Graph SLAM Reference Architecture

Direct answer: Factor graph SLAM treats the unknown trajectory and landmarks as variables and every measurement (odometry, IMU, landmark observation, loop closure) as a factor, a probabilistic constraint over a few variables. The maximum a posteriori estimate of all variables is the minimizer of a sum of squared, noise-weighted residuals, solved by iterative linearization and sparse linear algebra.

Figure 1: A small factor graph. Circles are unknown variables, boxes are factors. Each factor touches only the variables it constrains, which is why the resulting linear system is sparse. The loop closure factor links the newest pose back to the first.

Figure 1 shows the structure. The graph is bipartite: variable nodes (poses x0 to x3 and a landmark l1) connect only to factor nodes, never directly to each other. A prior factor on x0 pins the gauge, because absolute position is otherwise unobservable. Odometry factors connect consecutive poses. Landmark factors connect a pose and a landmark. The loop closure factor, drawn last, connects x3 back to x0 and is the reason the whole structure matters: it turns a chain into a cycle and creates information flow between distant parts of the trajectory.

SLAM as MAP estimation

Let X be all unknown variables and Z all measurements. Bayes’ rule gives the posterior p(X | Z), proportional to the prior p(X) times the likelihood p(Z | X). Assuming measurements are conditionally independent given the variables they touch, the posterior factorizes into a product of small terms, one per measurement. That product is the factor graph: each factor f_i is a function of only its adjacent variables X_i.

The MAP estimate maximizes that product. Under additive Gaussian noise, each measurement model is z_i = h_i(X_i) + noise with noise covariance Sigma_i, and each factor is proportional to exp(-0.5 * ||h_i(X_i) – z_i||^2_Sigma). Taking the negative log turns the maximization of a product into the minimization of a sum:

X_MAP = argmin over X of the sum over i of ||h_i(X_i) – z_i||^2 weighted by the inverse of Sigma_i.

This is a nonlinear least squares problem. The Mahalanobis norm weights each residual by its measurement certainty: a precise lidar scan match counts more than a slipping wheel encoder. Nothing about the formulation is specific to robots; it is the same estimation problem that bundle adjustment solves in photogrammetry, and the two fields converged on near-identical solvers.

Factor graph versus Bayes net versus Markov random field

Three graphical models appear in the literature, and they are easy to confuse. A Bayes net is a directed acyclic graph encoding a factorization into conditional densities, such as p(x1 | x0) p(x2 | x1). A Markov random field is an undirected graph of cliques. A factor graph is a bipartite graph that makes the factorization explicit by drawing each factor as its own node.

The factor graph is the most informative for SLAM because it distinguishes constraints that a Markov random field would hide. Three variables joined by a single three-way factor and three variables joined by three pairwise factors both appear as a triangle in an MRF, yet they are different models with different costs. The factor graph shows the difference, and solvers exploit it. The Bayes net matters for a different reason: it is what you get after elimination, a directed structure from which the estimate is read off by back substitution. The factor graph is the problem; the Bayes net is the solved form.

Pose graph versus full SLAM

A pose graph is a factor graph in which the only variables are robot poses and every factor is a relative-pose constraint. Landmarks are gone: a scan match between two lidar keyframes or a visual loop closure becomes a single between-pose factor. This is why pose graph optimization is the workhorse of lidar SLAM. The graph is smaller, the factors are simple, and the unknown is a set of poses on the Lie group SE(2) or SE(3).

Full SLAM keeps landmarks as variables and observation factors as constraints. It is more accurate when landmark observations are informative, since the geometry is not collapsed into an approximate relative transform, but it carries more variables and more data association risk. Many production systems sit in between: a visual-inertial system keeps landmark (or reprojection) factors inside a sliding window and uses a pose graph for global loop closure.

Why the information matrix is sparse

Linearize each residual around the current estimate: r_i(X + dx) is approximately r_i(X) + J_i dx, where J_i is the Jacobian. Stacking all weighted residuals gives a big least squares problem: minimize ||A dx – b||^2, where A is the whitened Jacobian and b the whitened residual. The normal equations are (A^T A) dx = A^T b. The matrix A^T A is the information matrix, written H or Lambda.

Each row of A comes from one factor and is nonzero only in the columns of the variables that factor touches. A 2D odometry factor touches two poses, so its row block has six nonzero columns out of 3N. Therefore the information matrix has a nonzero block (i, j) only when variables i and j share a factor. For a pure odometry chain that is block tridiagonal. A loop closure adds a single off-diagonal block pair. The matrix stays sparse as the trajectory grows, whereas the EKF covariance, the dense inverse, does not. This is the central fact of the subject: the information form is sparse, the covariance form is dense, and smoothing keeps the information form.

Solving the Optimization: Linearization, Gauss-Newton and Levenberg-Marquardt

Because h_i is nonlinear, particularly in rotations, we solve by repeated linearization. Starting from an initial estimate X0, compute the whitened Jacobian A and residual b, solve the linear problem for dx, update the estimate, and repeat. The only thing that varies between algorithms is how the step is chosen and how the update is applied to a manifold.

Gauss-Newton

Gauss-Newton solves (A^T A) dx = -A^T r directly at each iteration. It converges quadratically near a good minimum when residuals are small, which is the usual regime for SLAM after a decent initial guess. Its failure mode is predictable: if the initial estimate is poor, as when a long drifting trajectory has just received a large loop closure, the linearization is invalid, A^T A can be near singular along weakly observed directions, and the step overshoots.

Levenberg-Marquardt

Levenberg-Marquardt (LM) damps the system: (A^T A + lambda * D) dx = -A^T r, where D is the identity or the diagonal of A^T A. A large lambda gives a small, gradient-descent-like step; a small lambda recovers Gauss-Newton. The solver adapts lambda: after each step it compares the actual cost reduction with the reduction the linear model predicted. If the step was good, lambda shrinks; if the cost went up, the step is rejected and lambda grows. GTSAM exposes this as LevenbergMarquardtOptimizer, and it is the default choice for batch problems with uncertain initialization. Dogleg is a related trust-region method that GTSAM also offers.

Working on manifolds

Rotations do not live in a vector space. Adding an angle increment to a rotation matrix element-wise breaks orthogonality. The standard fix is to optimize on the manifold: represent the update as a small tangent-space vector, apply it through a retraction (for SE(3), usually the exponential map composed with the current pose), and linearize residuals in that local chart. GTSAM’s variable types, such as Pose2, Pose3 and Rot3, implement the needed retract and localCoordinates operations, so the optimizer itself stays generic. In the NumPy example later, which is 2D, we simply wrap the heading angle, which is the 2D special case of the same idea.

A worked example of the numbers

Take a 2D pose graph with N = 1,000 keyframes. The state dimension is 3N = 3,000. A dense information matrix would hold 9 million entries, about 72 MB in double precision. The real matrix holds roughly 3,000 diagonal-block entries plus about 2,997 odometry blocks (3×3 each, two off-diagonals) plus a few hundred loop closure blocks: on the order of 10^5 nonzeros. That is illustrative arithmetic, not a benchmark, but the ratio, a few percent density and falling as N grows, is the reason a sparse Cholesky of the pose graph is practical on a small embedded computer where a dense factorization (cubic in 3,000) is not.

Variable Elimination, Ordering and Fill-In

Solving the sparse normal equations is where a SLAM back end spends its time, and where cleverness pays. The factor graph view gives a clean way to see what a sparse Cholesky or QR factorization is actually doing.

Direct answer: Variable elimination removes one variable at a time: it gathers the factors touching that variable, factors them into a conditional density on the variable given its neighbours, and returns a new factor over those neighbours. The sequence of conditionals forms a Bayes net. The order of elimination decides how many new edges (fill-in) appear, and therefore how expensive the solve is.

Figure 2: From factor graph to Bayes tree. An ordering heuristic such as COLAMD picks the elimination sequence, elimination produces a Bayes net, cliques are grouped into a Bayes tree, and back substitution yields the state update. Poor orderings create fill-in that feeds back into the cost.

What elimination produces

Eliminating variable x_j from the factors that mention it produces a conditional p(x_j | S_j), where S_j is the set of neighbouring variables that shared a factor with x_j. In linear-Gaussian form this is one row block of the square-root information matrix R from the QR (or Cholesky) factorization of A. The leftover product of factors becomes a new factor on S_j, which then gets eliminated later. After the last variable, you have a Bayes net: a chain of conditionals, each of the form “x_j equals a linear function of later variables plus noise”. Solving is back substitution from the last-eliminated variable to the first, a cost linear in the number of nonzeros in R.

Fill-in and why ordering matters

When x_j is eliminated, all of S_j become connected to each other through the new factor, even if they were not directly connected before. Those new edges are fill-in. A bad ordering can turn a sparse graph into a nearly dense one. The classic example is a star graph: eliminate the hub first and every leaf becomes connected to every other leaf (quadratic fill-in); eliminate leaves first and no fill-in appears at all.

Finding the optimal ordering is NP-hard, so every SLAM library uses a heuristic. The standard one is COLAMD (column approximate minimum degree), which approximates the order that minimizes fill by repeatedly selecting the variable with the fewest neighbours. GTSAM exposes it as Ordering.Colamd(graph) and uses it by default in batch solvers. A constrained variant, CCOLAMD, lets you force certain variables to the end of the order, a trick iSAM2 uses to keep recently touched variables near the root.

For a trajectory with no loop closures, a natural ordering (oldest pose first) gives zero fill-in: the graph is a chain. A loop closure that links the newest pose to the oldest creates a long cycle, and any ordering must now introduce fill-in somewhere along that cycle. This is not a bug in the heuristic; it is the true information structure. A loop closure over a trajectory of length N costs on the order of fill proportional to the loop’s length in the worst case, and it is the principal reason SLAM back-end runtimes jump when a long loop is closed.

Illustrative cost numbers

Consider a corridor-and-rooms graph with 5,000 poses and 40 loop closures that each span 200 poses on average. With a good ordering the Cholesky factor might gain a band of dense blocks around each loop: 40 loops, each adding perhaps 200 x 200 / 2 scalar blocks, so around 800,000 extra blocks in a pessimistic case. A poor ordering could add orders of magnitude more. These figures are illustrative; real fill depends on topology and on whether loops overlap, but they show why the ordering heuristic is not an implementation detail.

iSAM2 and the Bayes Tree: Incremental Smoothing

Batch optimization re-solves everything on every new measurement. A robot producing keyframes at 10 Hz for an hour accumulates 36,000 poses, and re-factoring the whole system each time is wasteful because most of it did not change. iSAM2 exploits the observation that a new factor only perturbs part of the solution.

The Bayes tree

A Bayes net from elimination can be regrouped into cliques of variables that are eliminated together, forming a tree called the Bayes tree. Each clique stores a conditional density of its frontal variables (those eliminated in it) given its separator (variables shared with its parent clique). Roots hold the most recently eliminated variables. The paper’s abstract describes the Bayes tree as a directed structure encoding a factored probability density and notes that updates to the factorization become simple edits of the tree.

Why a tree helps: the Bayes net contains the same information but no clear indication of which part is affected by a change. In the tree, a new factor over variables A and B only affects the cliques containing A or B and the path from them to the root. Everything in unrelated subtrees stays untouched, and so do the cached conditional densities and their back-substitution results.

Figure 3: The iSAM2 update. New factors identify the affected part of the Bayes tree, which is detached up to the root, optionally relinearized, re-eliminated with a local ordering, and reattached. Partial back substitution updates only the changed variables.

The update step, in order

Figure 3 summarizes the cycle. First, new factors and new variables arrive from the front end. Second, the algorithm finds the cliques that contain any variable involved in a new factor, and removes them plus all their ancestors up to the root, converting that sub-tree back into a factor graph. Third, it adds the new factors. Fourth, it re-eliminates this small factor graph using a local ordering that places the variables touched most recently at the end (via constrained COLAMD), which keeps the next update cheap. Fifth, the new cliques are grafted back onto the untouched sub-trees. Sixth, back substitution runs only through the cliques whose delta changed beyond a threshold.

For odometry-like operation, where each new pose attaches only to the previous one, the affected path is the root clique alone, so the update is effectively constant time. A loop closure touches an old clique deep in the tree, forcing re-elimination along the path to the root. The cost scales with the size of that path rather than with the full history. This is what the paper means when it describes eliminating periodic batch steps: the incremental algorithm is, in the common case, always doing the minimal necessary work.

Fluid relinearization

Linearization points go stale. In the batch solver you relinearize every iteration. In an incremental one, relinearizing everything on every step defeats the point. iSAM2 instead tracks, for each variable, how far its current delta has drifted from the value at which it was linearized. Only variables whose delta exceeds a threshold are marked, and only the cliques containing them, plus ancestors, are relinearized in that update. The authors call this fluid relinearization.

In GTSAM, the controlling parameters live in ISAM2Params: the relinearization threshold, a skip count that checks the threshold only every few updates, and the factorization type. To my recollection the defaults are a threshold of 0.1 and a skip of 10, but confirm against the current class documentation before relying on those numbers, because I did not re-verify them from the source this run. The trade-off is direct: a tighter threshold gives accuracy closer to batch at higher cost; a looser one is faster but can leave stale linearizations that bias the estimate after large corrections.

What iSAM2 does not do

It is not magic. Cost is data-dependent: a loop closure that connects the very oldest pose to the newest forces re-elimination along a long path, and the worst case approaches a batch solve. It also assumes the graph keeps growing. If you need to bound memory and runtime for a robot operating for days, you need marginalization or a fixed-lag smoother, which in GTSAM is IncrementalFixedLagSmoother; marginalizing old variables creates dense factors on their neighbours and can reduce sparsity, a cost that visual-inertial systems accept deliberately.

IMU Preintegration Factors

An inertial measurement unit reports acceleration and angular velocity at 100 to 1,000 Hz, far faster than keyframes. Adding a state variable per IMU sample would make the graph enormous. Adding one only at keyframe rate and integrating the raw measurements between keyframes inside the optimizer would force re-integration every time the linearization point for orientation or bias changed.

The preintegration idea

Preintegration, introduced by Lupton and Sukkarieh and put on a rigorous manifold footing by Forster, Carlone, Dellaert and Scaramuzza (IEEE Transactions on Robotics, 2017), solves this by integrating the raw measurements between two keyframes into a single relative motion increment, expressed in the body frame of the first keyframe, independent of the initial pose and velocity. The increment consists of a relative rotation, a relative velocity change and a relative position change, together with a covariance propagated from the sensor noise model and Jacobians with respect to the gyroscope and accelerometer biases.

Because the increment does not depend on the global state, it is computed once. When the optimizer changes its estimate of the bias, the increment is corrected with a first-order update using those stored Jacobians instead of re-integrating the raw samples. The result is a single factor linking pose i, velocity i, pose j, velocity j and the bias at i, which makes the graph’s variable set at each keyframe a pose, a velocity and a bias.

In GTSAM

GTSAM implements this as PreintegratedImuMeasurements (built from PreintegrationParams) and ImuFactor, with a CombinedImuFactor that also encodes a random-walk model on bias between keyframes. A typical flow creates the params with the gravity convention (for instance PreintegrationParams.MakeSharedU(9.81) for a z-up frame), sets the gyro and accelerometer noise densities and integration covariance, feeds each IMU sample through integrateMeasurement(acc, gyro, dt), and at the next keyframe adds ImuFactor(X(i), V(i), X(j), V(j), B(i), preint). Check the exact signature against the version you install, since the 4.3 alpha series is still changing.

Failure modes specific to inertial factors

Three problems dominate in practice. First, bias observability: when the robot is stationary or moves with constant velocity, accelerometer bias and gravity direction are weakly separable, and the optimizer will wander along the null space unless a bias random-walk factor and sensible priors tie it down. Second, time synchronization: a few milliseconds of offset between IMU and lidar corrupts the preintegrated increment during fast rotation, producing consistent drift the optimizer cannot explain. Third, noise tuning: the continuous-time noise densities on the datasheet are optimistic, and inflating them is a common empirical fix. For a deeper comparison of lidar-inertial pipelines built around these ideas, see our article on LIO-SAM versus FAST-LIO2 versus KISS-ICP, where LIO-SAM in particular is a factor-graph back end around GTSAM.

Loop Closure and Robust Kernels: Surviving Outliers

A loop closure is the most valuable and most dangerous factor in the graph. Valuable because it is the only constraint that corrects accumulated drift. Dangerous because a false one, produced when two corridors look alike, is accepted by a least squares solver with the same enthusiasm as a true one, and it bends the whole map to satisfy it.

Figure 4: A practical SLAM pipeline. Sensors feed a front end and IMU preintegration, keyframes feed place recognition, candidates are geometrically verified and then pass through a robust kernel before entering the iSAM2 back end.

Why squared error is fragile

The quadratic cost gives a residual of size r a penalty proportional to r squared and an influence (the derivative) proportional to r. An outlier with a residual of ten sigma therefore pulls with ten times the force of a one-sigma inlier and a hundred times the cost, and the gradient it generates dominates the solution. Because loop closures are rare, one bad one among a handful of good ones can be fatal.

M-estimators and robust noise models

The standard remedy is an M-estimator that replaces the squared loss with a function that grows linearly (Huber) or saturates (Cauchy, Geman-McClure, Tukey). Huber is quadratic below a threshold k and linear beyond it, so large residuals are down-weighted rather than ignored; it is convex, which preserves the single-minimum property near the solution. Cauchy and Geman-McClure are non-convex, rejecting outliers more aggressively but introducing local minima and a stronger dependence on initialization.

Solvers implement an M-estimator by iteratively reweighted least squares: at each iteration, compute residuals, assign each factor a weight w(r) from the kernel, and solve the weighted normal equations. In GTSAM you attach a robust model to a factor’s noise: wrap a base Gaussian with noiseModel.Robust.Create(noiseModel.mEstimator.Huber.Create(k), base). The kernel names available in mEstimator include Huber, Cauchy, Tukey, Welsch, GemanMcClure and others; list them from your installed version instead of trusting memory.

Switchable constraints and graduated non-convexity

Robust kernels reduce the influence of a bad loop closure but do not decide whether to accept it. Sunderhauf and Protzel’s switchable constraints add a switch variable per loop closure, with a prior pulling it toward on; the optimizer may turn a constraint off if it conflicts with the rest of the graph at a price. Graduated non-convexity (GNC) starts from a convex surrogate and gradually transforms it toward the robust loss, which lowers sensitivity to the starting point. Pairwise consistency maximization (PCM) works before optimization: it keeps only the largest subset of loop closures that are mutually consistent. Which of these you adopt depends on the failure budget; many deployed stacks combine a conservative front-end verification step with a Huber kernel and nothing more elaborate.

The front-end gate matters more than the kernel

The best defence is not letting the false positive in. Place recognition returns candidates, geometric verification (scan-to-submap ICP fitness, or inlier counts after RANSAC) accepts or rejects them, and a covariance gate rejects closures whose implied transform is wildly inconsistent with the current pose estimate relative to its uncertainty. A kernel is your last line of defence, not your first. A useful practice is to log the accepted loop closures with their inlier scores and review them after each field run: you will find borderline cases you would never have noticed from the final map.

Runnable Example: A Small Pose Graph in NumPy and GTSAM

The following script builds a 2D pose graph from scratch with NumPy. The robot drives a 4 m by 4 m square (16 one-metre steps, with a quarter turn every fourth step), odometry carries Gaussian noise, and a single loop closure ties the last pose back to the first. It implements the equations from the earlier sections: residual between(xi, xj) - z, analytic Jacobians, whitening by inverse variance, a gauge prior on x0 and a Gauss-Newton loop. I ran this script for this article; the numbers in the comments came from that run with the fixed seed shown, and are illustrative of this toy problem only.

import numpy as np

def wrap(a): return (a + np.pi) % (2*np.pi) - np.pi

def between(xi, xj):
    c, s = np.cos(xi[2]), np.sin(xi[2])
    d = xj[:2] - xi[:2]
    return np.array([c*d[0] + s*d[1], -s*d[0] + c*d[1], wrap(xj[2]-xi[2])])

def residual_jac(xi, xj, z):
    c, s = np.cos(xi[2]), np.sin(xi[2])
    d = xj[:2] - xi[:2]
    r = between(xi, xj) - z
    r[2] = wrap(r[2])
    Ji = np.array([[-c, -s, -s*d[0] + c*d[1]],
                   [ s, -c, -c*d[0] - s*d[1]],
                   [ 0,  0, -1]])
    Jj = np.array([[ c, s, 0],
                   [-s, c, 0],
                   [ 0, 0, 1]])
    return r, Ji, Jj

def optimize(x, factors, iters=10):
    n = len(x)
    for it in range(iters):
        H = np.zeros((3*n, 3*n)); b = np.zeros(3*n); cost = 0
        for i, j, z, sig in factors:
            r, Ji, Jj = residual_jac(x[i], x[j], z)
            W = np.diag(1/np.asarray(sig)**2)
            cost += r @ W @ r
            si, sj = slice(3*i, 3*i+3), slice(3*j, 3*j+3)
            H[si, si] += Ji.T@W@Ji; H[sj, sj] += Jj.T@W@Jj
            H[si, sj] += Ji.T@W@Jj; H[sj, si] += Jj.T@W@Ji
            b[si] += Ji.T@W@r;      b[sj] += Jj.T@W@r
        H[:3, :3] += 1e6*np.eye(3)               # gauge prior on x0
        dx = np.linalg.solve(H, -b).reshape(n, 3)
        x = x + dx; x[:, 2] = wrap(x[:, 2])
        print(f"iter {it}: cost={cost:.4f} max|dx|={np.abs(dx).max():.2e}")
        if np.abs(dx).max() < 1e-9: break
    return x

def compose(p, d):
    c, s = np.cos(p[2]), np.sin(p[2])
    return np.array([p[0]+c*d[0]-s*d[1], p[1]+s*d[0]+c*d[1], wrap(p[2]+d[2])])

rng = np.random.default_rng(0)
sig = (0.1, 0.1, 0.05)
gt, x, factors = [np.zeros(3)], [np.zeros(3)], []
for k in range(16):
    d = np.array([1.0, 0, np.pi/2 if (k+1) % 4 == 0 else 0])
    gt.append(compose(gt[-1], d))
    z = d + rng.normal(0, sig)
    factors.append((k, k+1, z, sig))
    x.append(compose(x[-1], z))
factors.append((16, 0, between(gt[16], gt[0]), (0.05, 0.05, 0.02)))
x = np.array(x)
print("drift before:", np.round(x[16]-gt[16], 3))   # [0.4 0.095 0.16]
xo = optimize(x.copy(), factors)
print("error after: ", np.round(xo[16]-gt[16], 3))  # [0.001 0.004 0.003]

In the run, the final pose started 0.40 m off in x and 0.16 rad off in heading. The cost fell from about 131.6 to about 2.95 after one iteration, to 1.26 by the third, and then stopped changing, with the final pose within about 4 mm of ground truth. Two lessons carry over to real systems. The loop closure spread a 0.4 m correction across sixteen poses rather than snapping the last one into place. And the optimizer needed only a handful of iterations because the initial guess was already close, which is not guaranteed when drift is large.

The equivalent GTSAM program expresses the same graph with the library handling Jacobians, manifolds and the sparse solve. The names below are the Python API as of the 4.2 series; check them against your installed version.

import numpy as np
import gtsam

graph = gtsam.NonlinearFactorGraph()
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-3, 1e-3, 1e-3]))
odo_noise   = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
loop_noise  = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.02]))
huber = gtsam.noiseModel.Robust.Create(
    gtsam.noiseModel.mEstimator.Huber.Create(1.345), loop_noise)

graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(0, 0, 0), prior_noise))
# odometry: for each k, graph.add(gtsam.BetweenFactorPose2(k, k+1, gtsam.Pose2(dx, dy, dth), odo_noise))
# loop closure with a robust kernel
graph.add(gtsam.BetweenFactorPose2(16, 0, gtsam.Pose2(0, 0, 0), huber))

initial = gtsam.Values()
# initial.insert(k, gtsam.Pose2(xk, yk, thk)) for every pose from dead reckoning

params = gtsam.LevenbergMarquardtParams()
result = gtsam.LevenbergMarquardtOptimizer(graph, initial, params).optimize()

# Incremental version
isam = gtsam.ISAM2(gtsam.ISAM2Params())
new_graph, new_values = gtsam.NonlinearFactorGraph(), gtsam.Values()
# ... add the newest factors and initial guesses for new variables only ...
isam.update(new_graph, new_values)
estimate = isam.calculateEstimate()

The incremental half shows the key API difference: you hand iSAM2 only new factors and new variables each step and call update; it maintains the Bayes tree internally. Calling update more than once without new data runs extra relinearization passes, which is a common trick after a loop closure. In the batch half, the dead-reckoned initial values matter: LM is a local optimizer, and a poor initialization is the first thing to check when a graph converges to a twisted map.

Comparison: GTSAM, g2o, Ceres and Filters

The libraries differ less in mathematics than in what they make easy.

Aspect	GTSAM	g2o	Ceres Solver	EKF or UKF SLAM
Core abstraction	Factor graph, Bayes net, Bayes tree	Hypergraph of vertices and edges	General nonlinear least squares problem	Recursive Gaussian state
Incremental solving	Yes, iSAM2 and fixed-lag smoothers	Mainly batch; incremental variants are less central	No built-in incremental SLAM; manual windowing	Natural (per step)
Manifold support	Built-in Lie group types	Per-vertex update rules	Local parameterizations or manifolds, user-supplied	Per-implementation
Robust kernels	`noiseModel.mEstimator`	Robust kernel classes on edges	Loss functions	Gating, usually ad hoc
IMU preintegration	Built-in factors	Third-party	Write your own cost function	Propagated inline
Memory growth	Grows with trajectory unless marginalized	Grows with graph	Grows with problem	Quadratic in landmarks
Best fit	Online back end with loop closure and inertial	Large batch pose graphs and bundle adjustment	Custom residuals, offline calibration	Short-horizon state estimation

A few cautions on reading the table. g2o (introduced by Kuemmerle and colleagues at ICRA 2011) is a mature, fast batch optimizer; I list its incremental support as limited based on its typical use rather than a fresh check of its current feature set, so verify against its repository. Ceres is exceptionally flexible and widely used for bundle adjustment and calibration, but because it has no Bayes tree analogue you cannot get iSAM2-style incremental updates without building them yourself. Filters remain correct choices when the state is small and the horizon short: an EKF on a 15-state IMU-and-GPS navigation problem is simpler and cheaper than a factor graph, and our state estimation comparison discusses that regime. The point at which smoothing wins is when you need to revise the past, which is exactly what loop closure requires.

Trade-offs, Gotchas, and What Goes Wrong

Unbounded growth. A full smoother keeps every pose. At 10 Hz keyframes that is 36,000 poses per hour, and even sparse the Bayes tree grows. Production systems control this by keyframe selection (add a keyframe only after a displacement or rotation threshold, or a minimum interval), by sliding-window smoothing, or by splitting into submaps with a pose graph above them.

Over-confident odometry. If a factor’s covariance is too small, the optimizer trusts it more than it deserves and a legitimate loop closure cannot pull the trajectory. If it is too large, drift is not constrained. Covariances from scan matchers are famously optimistic; inflating them, or deriving them empirically from residual statistics on a logged run, is a first-order tuning step.

Perceptual aliasing. Place recognition is the weakest link. Long featureless corridors, repeated shelving in warehouses and identical rooms in a factory produce false candidates that geometry alone cannot disprove. Combine appearance with metric consistency, and prefer to miss a loop closure than to add a false one: a missed closure leaves drift, a false one corrupts the map in ways downstream planners will not detect.

Gauge freedom and degeneracy. Without a prior or fixed anchor the problem has a null space of rigid transformations; the solver may fail or return arbitrary offsets. Similarly, a long straight corridor has an unobservable translation along its axis for a lidar scan matcher, and no amount of back-end cleverness restores information the sensors never provided.

Stale linearization in incremental mode. iSAM2’s thresholded relinearization means that after a large loop correction, the estimate can be a few updates behind the true optimum. Running one or two additional update calls with no new data, or lowering the threshold temporarily, is the usual remedy.

Initialization. Poor initial values, for instance from a heading error of tens of degrees before a loop closes, can send LM into a local minimum, especially with non-convex kernels. Chain odometry for initial guesses, and consider a coarse rotation averaging step before nonlinear refinement for very large loops.

Real-time scheduling. The worst-case update is a long-path re-elimination. If the control stack cannot tolerate a latency spike, run the back end on its own thread, publish the last good estimate, and treat the corrected map as asynchronous.

Practical Recommendations

Start by choosing the formulation to fit the sensing. For lidar or lidar-inertial robots, use a pose graph with scan-matching between-factors, IMU preintegration between keyframes, and global loop closures; for camera-based systems, keep a local window with reprojection factors and add a pose graph for loop closure. Use GTSAM with iSAM2 when you need online operation and are willing to learn the factor-graph vocabulary; reach for g2o or Ceres for offline batch refinement, calibration, or when your residuals do not fit GTSAM’s types.

Treat the front-end gate as seriously as the optimizer. A conservative loop-closure policy, geometric verification and a Huber kernel will outperform an elaborate back end fed unfiltered candidates. Log everything: factors, residuals, kernel weights, and loop-closure acceptance reasons. Replaying a recorded graph offline against both batch and incremental solvers is the quickest way to separate a modeling bug from a solver bug.

Finally, validate against ground truth or, failing that, against self-consistency: closing the same loop twice should not change the answer, and the map should be stable when you re-drive a route.

Pin the GTSAM version and prefer the stable 4.2 series for production; treat the 4.3 alphas as experimental.
Add a prior on the first pose and a bias random-walk factor for inertial variables.
Calibrate noise models from data, not datasheets; inflate scan-matching covariances.
Gate loop closures with geometric verification, then wrap them in a robust kernel.
Use keyframe selection and a fixed-lag or submap strategy to bound growth.
Run the optimizer off the control thread and monitor update latency.
Replay logged graphs offline to regression-test every change.

Frequently Asked Questions

What is factor graph SLAM?

Factor graph SLAM is a formulation of simultaneous localization and mapping in which unknown poses and landmarks are variable nodes and each measurement is a factor, a probabilistic constraint over a few variables. The most probable trajectory and map are found by minimizing a sum of noise-weighted squared residuals. Because each factor touches few variables, the problem is sparse, which allows fast and incremental solvers such as iSAM2.

What is the difference between a factor graph and a pose graph?

A pose graph is a special factor graph whose variables are only robot poses and whose factors are relative-pose constraints, from odometry or loop closures. A general factor graph can also include landmarks, IMU velocities and biases, calibration parameters and priors. Pose graphs are smaller and simpler, which is why they dominate lidar SLAM, while richer factor graphs appear in visual-inertial systems.

How does iSAM2 differ from batch optimization?

Batch optimization re-linearizes and re-factorizes the entire problem every time. iSAM2 stores the solved form as a Bayes tree and, when new factors arrive, modifies only the affected cliques and their path to the root, relinearizing selectively. It therefore avoids periodic batch steps. Cost per update depends on how far back in the trajectory the new factors reach, with the worst case approaching a full solve.

Why does variable ordering matter so much?

Eliminating a variable connects all of its neighbours, creating fill-in that makes the factor denser and slower to compute. Different orderings of the same graph can differ in cost by orders of magnitude. Finding the best ordering is NP-hard, so libraries use heuristics such as COLAMD. A good ordering keeps the Cholesky or QR factor close to as sparse as the original graph.

How do you handle false loop closures?

Use layers. First, verify candidates geometrically, for example with ICP fitness or RANSAC inliers, and gate on consistency with current uncertainty. Second, wrap accepted loop closures in a robust kernel such as Huber or Cauchy so that large residuals have limited influence. Third, for harder cases use switchable constraints, graduated non-convexity or pairwise consistency maximization. Prefer missing a closure to accepting a wrong one.

Should I use GTSAM, g2o or Ceres?

Use GTSAM when you want an online back end with incremental smoothing, Lie-group variable types and built-in IMU preintegration. Choose g2o for large batch pose graph or bundle adjustment jobs where its edge and vertex model fits. Choose Ceres when you need custom residuals, calibration or general least squares. Many teams use more than one: GTSAM online, Ceres or g2o for offline refinement.