SLAM as Least Squares: Factor Graphs

The modern backbone of SLAM — write down the whole posterior instead of filtering it away, watch it shatter into springs, and discover that the sparse matrix you get is a picture of the graph.

The map that could not be fixed

Here is Rusty's problem, laid out in full. He drives east along the Apartment corridor, ducks into room C through its doorway, once around the room, and back home along the other side of the corridor — 55 poses, 12 corner reflectors, 176 sightings. His wheel odometry has a 3.5% turn bias: not noise, a bias, the kind that does not average away. By the time he gets home the dead-reckoned path says he finished 1.66 m from where he started.

He also knows he is home. The doorway he sees now is the doorway he saw fifty poses ago. That single recognition contradicts every one of the 54 odometry readings, slightly, and there is no filter update that can distribute the contradiction backwards across all of them.

So stop filtering. Put all 189 unknown numbers on the table at once and ask what values make the whole data set most probable.

Three things in that widget deserve your attention before any mathematics.

The correction is not spread evenly through time — it is spread according to evidence. Pose 0 does not move at all, because the prior pins it. The poses in room C move about 0.4 m. The last pose moves 1.65 m, which is the entire accumulated drift, because that is where the contradiction lives. No rule was written down to produce that gradient; it is what minimizing a sum of squares does when the constraints have different stiffnesses.

Almost everything happens in three iterations. $J$ falls from 3780 to 599 to 161 to 117 and then stops. Least squares on a well-initialized SLAM problem is not an iterative slog; it is two or three re-linearizations and a lot of back-substitution.

One bad spring bends the whole map. Press inject a false loop closure and the smoother — which was just praised for using all its evidence — dutifully uses the evidence that is wrong. The RMSE goes from 0.197 m to 0.765 m. Robustness is not something least squares has; it is something you must add, and the last third of this chapter is about the price.

Building intuition: springs, and a ledger of who is connected to whom

Thrun, Burgard and Fox already gave us the metaphor, in the sentence at the top of this chapter. A control is a spring between two consecutive poses. An observation is a spring between a pose and a landmark. The stiffness of a spring is the information of the measurement that produced it — a laser with 1 cm noise makes a stiffer spring than sonar with 30 cm noise. The robot's estimate is where the whole assembly comes to rest.

That metaphor is worth more than its charm, because it makes three otherwise separate ideas visibly the same thing:

The rest of the chapter needs a little new notation, all of it local to Part V.

The mathematics

The posterior is a graph

Nothing new is assumed here. The Markov property from Chapter 5, the motion model of Chapter 9, the measurement model of Chapter 10, and — for now — known correspondences: we are told which landmark each observation belongs to. (Chapter 16 removes that assumption, and the robust machinery at the end of this chapter is what makes removing it survivable.)

Read the right-hand side as a drawing. Each factor is a small box; each box is wired to the two or three variables it mentions. The prior box touches $x_0$. Each motion box touches a consecutive pair $(x_{\tau-1}, x_\tau)$. Each measurement box touches one pose and one landmark. No box ever touches two landmarks. That last sentence is the whole reason this chapter scales, and we will cash it in twice.

Whitening: why every residual is measured in sigmas

Carrying $\Sigma_k^{-1}$ around inside every norm is clumsy. Factor it out once, at the point where the factor is built. Let $\Sigma_k^{-1} = S_k\T S_k$ (Cholesky, from Chapter 2) and define the whitened residual $\tilde r_k = S_k r_k$. Then

Every residual is now dimensionless and measured in standard deviations, which means residuals from a laser, a wheel encoder and a loop-closure detector are directly comparable. It also means the number the widget prints beside a bad spring — $e = 35\sigma$ — has an unambiguous meaning: this measurement is thirty-five standard deviations from what the current estimate predicts.

Gauss–Newton on the manifold

$J$ is not quadratic — $h$ has an $\operatorname{atan2}$ in it and poses live on $\SEtwo$ — but it is locally quadratic, and that is enough.

The Jacobians are where the manifold discipline of Chapter 7 earns its keep. For the between factor $r = \log(\delta^{-1} x_i^{-1} x_j)$, perturbing $x_j$ on the right gives $\log(E \exp(\Delta_j)) \approx r + J_r^{-1}(r)\Delta_j$, while perturbing $x_i$ pushes the perturbation through the adjoint:

with $J_r$ the right Jacobian of $\SEtwo$. Both are exact, both are cheap, and the library checks them against finite differences of the $\bplus$ perturbation to eight digits.

Damping: how far may you trust a linear model?

Gauss–Newton believes its own linearization completely. Where the cost valley curves — which in SLAM means anywhere a large rotation is being corrected — that belief produces a step that lands somewhere absurd. Levenberg–Marquardt adds a knob:

As $\lambda \to 0$ this is Gauss–Newton. As $\lambda \to \infty$ it becomes $\Delta \approx -(\lambda \diag\Omega)^{-1} b$, a very short step along the (scaled) negative gradient: slow, but it cannot be wrong about direction. Marquardt's $\diag\Omega$ scaling — rather than $\lambda I$ — makes the trust region an ellipsoid shaped like the problem, so a well-determined direction is not damped as hard as a poorly-determined one.

The schedule is the algorithm's only cleverness. Compare the reduction the linear model promised with the reduction actually delivered:

If $\varrho$ is healthy, accept the step and shrink $\lambda$; if the cost went up, reject the step outright and grow $\lambda$ by an order of magnitude. Rejecting is cheap — you already have $\Omega$ and only need to re-solve — and it is what makes LM the default in every production back end.

A worked example you can check by hand

Three scalar positions on a line, four unit-information factors:

The prior and the fix disagree: dead reckoning says $y_2 = 2$, the absolute measurement says $1.5$. Start from $y = (0,0,0)$, where the residuals are $r = (0,\, -1,\, -1,\, -1.5)$ and $J = \tfrac12(0 + 1 + 1 + 2.25) = 2.125$.

The Jacobians are the rows $(1,0,0)$, $(-1,1,0)$, $(0,-1,1)$, $(0,0,1)$. Accumulate $\Omega = \sum J_k\T J_k$ and $b = \sum J_k\T r_k$ by hand — each factor writes into a $1{\times}1$ or $2{\times}2$ patch and nothing else:

Notice the shape: tridiagonal, because the only variables sharing a factor are neighbors. Solving $\Omega \Delta = -b$ by elimination gives, in one step (the problem is linear, so Gauss–Newton converges exactly once):

Now read the answer. Both odometry intervals came out $0.875$ — equally shortened, though only the second one is adjacent to the disagreeing measurement. And all four residuals end at exactly $\pm 0.125$, with $J = 4 \cdot \tfrac12 (0.125)^2 = 1/32$. Least squares shared the half-metre contradiction evenly across four equally stiff springs, because that is what minimizes a sum of squares: equal marginal cost everywhere. If you make the prior ten times stiffer, the tension moves to the other end. That is the entire behavior of a SLAM back end, in three numbers you can check on paper.

Sparsity: Ω is a picture of the graph

Now cash in the observation from the factorization: no factor mentions two landmarks.

Elimination is the Schur complement is EIF_reduce

Sparsity in $\Omega$ is not automatically speed. You still have to solve $\Omega \Delta = -b$, and how you do that decides everything.

Solving means eliminating variables one at a time, and eliminating a variable is exactly Gaussian marginalization in information form:

Ordering is the algorithm

Fill-in is created by elimination, so the order of elimination decides how much of it you pay for. Finding the optimal ordering is NP-hard; the heuristics that matter are minimum degree (AMD, COLAMD) and nested dissection (METIS). Here is what they are worth on the Apartment graph, all four solving the identical system to the identical answer:

Two lessons, and the second one is the interesting one.

Ordering is worth an order of magnitude, here 22× in flops between the best and worst choice, and the gap widens with problem size. This is why every serious solver runs a fill-reducing ordering as a first-class step, and why nnz(L) — not nnz(Ω) — is the number to quote.

The famous trick is the worst choice on this graph. Eliminating landmarks first is the classical Schur trick from bundle adjustment, and it is excellent when each landmark is seen from few poses. In the Apartment, the corner reflectors are visible from long stretches of corridor, so eliminating one welds a dozen poses into a clique. Play with the One landmark, seen by all preset in the widget above: eliminating that hub first leaves $\operatorname{nnz}(L) = 351$ and twenty fill edges; eliminating it last leaves 216 and five. Same graph, same posterior, 63% more storage and a correspondingly denser factorization. Rules of thumb about ordering are statements about graph shape, not about algorithms.

One bad spring: robustness

Least squares is maximum likelihood under Gaussian noise, and a Gaussian says a 35σ event has probability smaller than $10^{-260}$. When one happens anyway — a false loop closure, a mis-associated landmark, a laser beam through a glass door — the estimator does not shrug. It concludes that the world must be bent, and bends it.

Run the numbers on the Apartment loop with a 1.6 m lie injected into one loop closure, and the theory is visible in three digits:

Read the last column first. Under L2 the false factor got most of what it wanted — its residual is only 6.6σ at the optimum, because every pose between the two it names moved to meet it. Under Geman–McClure it sits at 34.7σ, screaming, and nobody is listening: the map is back to 0.199 m, statistically indistinguishable from the clean solution. Huber lands in between, exactly as its influence function promises.

So why is Huber still the industry default? Because convexity is a guarantee and robustness is a gamble. Huber's objective has one minimum, so the answer does not depend on where you started; Geman–McClure's depends on it completely, and a smoother that starts from bad odometry can converge to a confident, self-consistent, wrong map. The modern escalation ladder is:

1. Huber, when outliers are rare and initialization is poor.
2. Switchable constraints (Sünderhauf & Protzel) and their closed-form cousin, dynamic covariance scaling: give every suspect factor its own switch variable and let the optimizer turn it off — robustness as a modeling decision rather than a choice of loss function.
3. Graduated non-convexity (Yang et al.), which starts with a convex surrogate and anneals it toward the redescending kernel, so you get the redescending behavior and initialization independence.
4. Certifiable solvers (SE-Sync and its descendants), which solve a convex relaxation and hand you a certificate that the answer is the global optimum — when the relaxation is tight.

Or you can stop guessing the scale altogether: Chebrolu et al. put $\rho$ itself in a one-parameter family and estimate its shape parameter jointly with the state, so the kernel is tuned by the data rather than by you.

Why filtering lost

It is worth being precise about the history, because the loser was not stupid and the winner did not appear from nowhere.

The EKF SLAM of Chapter 14 is this graph, filtered. Take the factor graph, and after each step eliminate the previous pose immediately, at whatever linearization point was current. Eliminating $x_{t-1}$ clique-connects everything it touched — which is every landmark it saw — and that clique is the EKF's dense covariance. Marginalization is not a different algorithm; it is this algorithm with a policy of never looking back, and the density and the frozen Jacobians are what that policy costs.

The 2005 book's GraphSLAM already was Gauss–Newton, without the name. Line them up:

One table, twenty-five years. What the modern formulation adds is not the loop but everything around it: the manifold-correct residuals, the ordering theory that makes the solve fast rather than merely correct, the trust region that makes it converge from a bad start, the robust kernels that make it survive a bad front end, and a sparse linear algebra ecosystem that did not exist in 2000.

And SEIF was the heroic wrong turn. Chapter 12 of the baseline noticed that the online information matrix is nearly sparse — most links are weak — and forced it to be exactly sparse by deleting the weak ones, buying constant-time updates. The instinct was right and the mechanism was wrong: deleting a link discards information the filter then does not know it has lost, and the resulting estimator is overconfident. Smoothing gets exact sparsity for free, by never marginalizing in the first place. The debt is still live, though — Chapter 18's sliding-window estimators must marginalize to bound computation, and they meet SEIF's dilemma again with better tools.

Implementation in Rust

The library mirrors the mathematics one-to-one: a Factor knows its keys, its dimension and how to linearize itself; Values knows how to retract; the optimizer knows nothing about SLAM at all.

use nalgebra::{DMatrix, DVector, Matrix2, Matrix2x3, Matrix3, Vector2, Vector3};
use pr_core::geom::{SE2, se2_right_jacobian_inv};  // Ch. 3

#[derive(Clone, Copy, PartialEq, Eq, Hash, Debug)]
pub enum VarKey {
    Pose(usize),
    Landmark(usize),
}

impl VarKey {
    /// Tangent-space dimension. Poses retract through SE(2); points just add.
    pub const fn dim(self) -> usize {
        match self {
            VarKey::Pose(_) => 3,
            VarKey::Landmark(_) => 2,
        }
    }
}

/// Every unknown in the problem. Two containers, one index.
#[derive(Clone, Debug)]
pub struct Values {
    pub poses: Vec<SE2>,
    pub landmarks: Vec<Vector2<f64>>,
}

impl Values {
    /// y ← y ⊞ Δ. The whole reason a heading never gets averaged into nonsense.
    pub fn retract(&mut self, delta: &DVector<f64>, index: &BlockIndex) {
        for (slot, key) in index.order.iter().enumerate() {
            let o = index.offset(slot);
            match *key {
                VarKey::Pose(i) => {
                    let tau = Vector3::new(delta[o], delta[o + 1], delta[o + 2]);
                    self.poses[i] = self.poses[i].boxplus(&tau);
                }
                VarKey::Landmark(l) => {
                    self.landmarks[l] += Vector2::new(delta[o], delta[o + 1]);
                }
            }
        }
    }
}

/// A whitened residual and one Jacobian block per key.
pub struct Linearization {
    pub residual: DVector<f64>,
    pub jacobians: Vec<DMatrix<f64>>,
}

pub trait Factor {
    fn keys(&self) -> &[VarKey];
    fn dim(&self) -> usize;
    /// Returns Σ^{-1/2} r and the matching Σ^{-1/2} J, so the caller never
    /// has to know which sensor this factor came from.
    fn linearize(&self, v: &Values) -> Linearization;
    fn kernel(&self) -> Kernel {
        Kernel::L2
    }
}

/// One control, or one loop closure: r = (xᵢ⁻¹ xⱼ) ⊟ δ.
pub struct BetweenFactor {
    pub keys: [VarKey; 2],       // [Pose(i), Pose(j)]
    pub delta: SE2,
    pub sqrt_info: Matrix3<f64>,
    pub kernel: Kernel,
}

impl Factor for BetweenFactor {
    fn keys(&self) -> &[VarKey] { &self.keys }
    fn dim(&self) -> usize { 3 }

    fn linearize(&self, v: &Values) -> Linearization {
        let (VarKey::Pose(i), VarKey::Pose(j)) = (self.keys[0], self.keys[1]) else {
            unreachable!("a between factor joins two poses")
        };
        let (xi, xj) = (v.poses[i], v.poses[j]);
        let rel = xi.inverse() * xj;                                 // xᵢ⁻¹ xⱼ
        let r: Vector3<f64> = (self.delta.inverse() * rel).log();    // r = rel ⊟ δ

        // Right perturbation: xⱼ moves the residual through J_r⁻¹, while xᵢ
        // must first be transported into j's frame — hence the adjoint.
        let jr_inv = se2_right_jacobian_inv(&r);
        let dj = self.sqrt_info * jr_inv;
        let di = -&dj * (xj.inverse() * xi).adjoint();

        Linearization {
            residual: DVector::from(self.sqrt_info * r),
            jacobians: vec![DMatrix::from(di), DMatrix::from(dj)],
        }
    }

    fn kernel(&self) -> Kernel { self.kernel }
}

/// Range–bearing sighting: Chapter 10's model, reused verbatim as a factor.
pub struct RangeBearingFactor {
    pub keys: [VarKey; 2],        // [Pose(t), Landmark(j)]
    pub z: Vector2<f64>,          // (range, bearing)
    pub sqrt_info: Matrix2<f64>,
    pub kernel: Kernel,
}

impl Factor for RangeBearingFactor {
    fn keys(&self) -> &[VarKey] { &self.keys }
    fn dim(&self) -> usize { 2 }

    fn linearize(&self, v: &Values) -> Linearization {
        let (VarKey::Pose(t), VarKey::Landmark(j)) = (self.keys[0], self.keys[1]) else {
            unreachable!("a sighting joins a pose and a landmark")
        };
        let x = v.poses[t];
        let d = x.inverse_transform_point(&v.landmarks[j]);         // body frame
        let q = (d.x * d.x + d.y * d.y).max(1e-9);
        let range = q.sqrt();
        let r = Vector2::new(range - self.z[0], wrap_angle(d.y.atan2(d.x) - self.z[1]));

        let dh_dd = Matrix2::new(d.x / range, d.y / range, -d.y / q, d.x / q);
        // ∂d/∂Δ for a right perturbation of the pose: translate, then rotate.
        let dd_dx = Matrix2x3::new(-1.0, 0.0, d.y, 0.0, -1.0, -d.x);
        let dd_dm = x.rotation().matrix().transpose();

        Linearization {
            residual: DVector::from(self.sqrt_info * r),
            jacobians: vec![
                DMatrix::from(self.sqrt_info * dh_dd * dd_dx),
                DMatrix::from(self.sqrt_info * dh_dd * dd_dm),
            ],
        }
    }
}

Assembly and the solve. The blocks go into a nalgebra-sparse COO triplet list — sparsity taught through the API before performance enters — and then into faer for the numeric factorization, which is the same engine factrs uses underneath, so graduating later changes the API and not the answer.

use faer::linalg::solvers::Llt;
use faer::sparse::SparseColMat;
use nalgebra_sparse::CooMatrix;

pub struct System {
    pub omega: SparseColMat<usize, f64>,
    pub b: DVector<f64>,
    pub cost: f64,
}

/// `linearize_graph` — Table 15.1. One pass over the factors; O(1) work each.
pub fn linearize_graph(g: &FactorGraph, v: &Values, index: &BlockIndex) -> System {
    let n = index.total();
    let mut coo = CooMatrix::<f64>::new(n, n);
    let mut b = DVector::zeros(n);
    let mut cost = 0.0;

    for f in &g.factors {
        let lin = f.linearize(v);
        let e = lin.residual.norm();
        // IRLS: one scalar per factor is the entire cost of robustness.
        let w = f.kernel().weight(e);
        cost += f.kernel().rho(e);

        for (a, key_a) in f.keys().iter().enumerate() {
            let oa = index.offset_of(key_a);
            let ja = &lin.jacobians[a];
            b.rows_mut(oa, ja.ncols()).add_assign(w * ja.transpose() * &lin.residual);

            for (c, key_c) in f.keys().iter().enumerate() {
                let oc = index.offset_of(key_c);
                let block = w * ja.transpose() * &lin.jacobians[c];
                for p in 0..block.nrows() {
                    for q in 0..block.ncols() {
                        // Duplicate triplets are summed on conversion to CSC,
                        // which is exactly the accumulation Ω = Σ Jᵀ J needs.
                        coo.push(oa + p, oc + q, block[(p, q)]);
                    }
                }
            }
        }
    }

    System { omega: SparseColMat::try_new_from_triplets(&coo).unwrap(), b, cost }
}

pub struct LmConfig {
    pub max_iterations: usize,
    pub tol: f64,
    pub lambda0: f64,
}

/// `levenberg_marquardt` — Table 15.3.
pub fn levenberg_marquardt(
    g: &FactorGraph,
    init: Values,
    index: &BlockIndex,
    cfg: &LmConfig,
    ordering: Ordering,
) -> (Values, Report) {
    let mut y = init;
    let mut lambda = cfg.lambda0;
    let mut cost = g.cost(&y);
    let mut report = Report::default();

    for _ in 0..cfg.max_iterations {
        let sys = linearize_graph(g, &y, index);
        let damped = sys.damped(lambda);                  // Ω + λ diag Ω
        // The symbolic pattern only changes when the graph does, so the
        // fill-reducing ordering is computed once and reused every iteration.
        let llt = Llt::try_new_with_symbolic(ordering.symbolic(&damped), damped.as_ref())
            .expect("Ω is singular — is a prior factor missing?");
        let delta = llt.solve(&-&sys.b);

        let mut trial = y.clone();
        trial.retract(&delta, index);
        let trial_cost = g.cost(&trial);

        // Gain ratio: what the linear model promised vs. what we got.
        let predicted = 0.5 * delta.dot(&(lambda * sys.diag().component_mul(&delta) - &sys.b));
        let rho = (cost - trial_cost) / predicted;

        if trial_cost < cost {
            y = trial;
            cost = trial_cost;
            lambda = (lambda * (1.0 - (2.0 * rho - 1.0).powi(3)).max(1.0 / 3.0)).max(1e-9);
            report.push_accepted(cost, delta.norm(), lambda);
            if report.relative_decrease() < cfg.tol { break; }
        } else {
            lambda *= 10.0;
            report.push_rejected(lambda);
        }
    }
    (y, report)
}

And the worked example, pinned by a test, exactly as in every other chapter of this book:

use approx::assert_relative_eq;
use ch15_graph::{FactorGraph, LinearFactor, Values, gauss_newton, linearize_graph};

/// The 1-D chain from §15.4: prior y₀ = 0, odometry +1, +1, and a fix y₂ = 1.5.
/// Every factor has unit information, so Ω is tridiagonal(−1, 2, −1) and the
/// whole thing can be checked on paper.
#[test]
fn micro_chain_matches_the_hand_calculation() {
    let g = FactorGraph::from(vec![
        LinearFactor::new(&[0],    &[1.0],       0.0, 1.0),   // prior
        LinearFactor::new(&[0, 1], &[-1.0, 1.0], 1.0, 1.0),   // odometry
        LinearFactor::new(&[1, 2], &[-1.0, 1.0], 1.0, 1.0),   // odometry
        LinearFactor::new(&[2],    &[1.0],       1.5, 1.0),   // absolute fix
    ]);
    let index = g.default_index();
    let init = Values::scalars(vec![0.0, 0.0, 0.0]);

    let sys = linearize_graph(&g, &init, &index);
    assert_relative_eq!(sys.cost, 2.125, epsilon = 1e-12);
    assert_relative_eq!(sys.omega[(0, 0)], 2.0, epsilon = 1e-12);
    assert_relative_eq!(sys.omega[(0, 1)], -1.0, epsilon = 1e-12);
    assert_relative_eq!(sys.omega[(0, 2)], 0.0, epsilon = 1e-12);   // not neighbors
    assert_relative_eq!(sys.b[2], -2.5, epsilon = 1e-12);

    let (y, report) = gauss_newton(&g, init, &index, &Default::default());
    assert_relative_eq!(y.scalars[0], -0.125, epsilon = 1e-9);
    assert_relative_eq!(y.scalars[1], 0.750, epsilon = 1e-9);
    assert_relative_eq!(y.scalars[2], 1.625, epsilon = 1e-9);

    // Both intervals shrink by the same amount: equal stiffness, equal share.
    assert_relative_eq!(y.scalars[1] - y.scalars[0], 0.875, epsilon = 1e-9);
    assert_relative_eq!(y.scalars[2] - y.scalars[1], 0.875, epsilon = 1e-9);
    // A linear problem is solved by the *first* Gauss–Newton step; every later
    // iteration moves nothing, which is how the loop knows it has converged.
    assert!(report.steps()[0].step_norm > 1.0);
    assert!(report.steps().last().unwrap().step_norm < 1e-12);
    assert_relative_eq!(report.final_cost(), 1.0 / 32.0, epsilon = 1e-12);
}

The same graph, in factrs

Once the ideas are yours, stop hand-rolling. factrs 0.3 is a GTSAM-shaped Rust library — typed variables, typed factors, automatic differentiation through dual numbers, GN and LM, robust kernels, serde, rerun output — and it builds the identical Apartment graph in about thirty lines:

use factrs::{
    assign_symbols,
    core::{BetweenResidual, GaussNewton, Graph, Huber, PriorResidual, Values},
    dtype, fac,
    linalg::{Const, ForwardProp, Numeric, VectorX, vectorx},
    residuals::Residual2,
    traits::*,
    variables::{SE2, VectorVar2},
};

// Type-tagged keys: X(3) is a pose and L(3) is a landmark, and the compiler
// will not let you hand one to a factor that expects the other.
assign_symbols!(X: SE2; L: VectorVar2);

/// factrs ships prior and between residuals; a range–bearing sighting is ours
/// to write. The only method that matters is `residual2`, generic over
/// `Numeric` — factrs differentiates it with dual numbers, so this file
/// contains no hand-derived Jacobians at all.
#[derive(Clone, Debug)]
#[factrs::mark]
pub struct RangeBearing {
    range: dtype,
    bearing: dtype,
}

impl Residual2 for RangeBearing {
    type V1 = SE2;
    type V2 = VectorVar2;
    type DimIn = Const<5>;
    type DimOut = Const<2>;
    type Differ = ForwardProp<Self::DimIn>;

    fn residual2<T: Numeric>(&self, x: SE2<T>, m: VectorVar2<T>) -> VectorX<T> {
        let d = x.inverse().apply(m.into());          // landmark in the body frame
        let range = (d[0] * d[0] + d[1] * d[1]).sqrt();
        vectorx![
            range - T::from(self.range),
            d[1].atan2(d[0]) - T::from(self.bearing)
        ]
    }
}

fn main() {
    let data = apartment_loop(42); // the same dataset the widget optimizes
    let mut values = Values::new();
    let mut graph = Graph::new();

    for (i, p) in data.odometry_chain().enumerate() {
        values.insert(X(i), p);
    }
    for (l, m) in data.landmark_guesses().enumerate() {
        values.insert(L(l), VectorVar2::new(m.x, m.y));
    }

    graph.add_factor(fac![PriorResidual::new(data.first_pose()), X(0), 0.01 as std]);

    for e in data.edges() {
        // Loop closures get the robust kernel; odometry does not need one.
        graph.add_factor(match e.kind {
            EdgeKind::Odometry => fac![BetweenResidual::new(e.delta), (X(e.i), X(e.j)), 0.04 as std],
            EdgeKind::Loop => {
                fac![BetweenResidual::new(e.delta), (X(e.i), X(e.j)), 0.05 as std, Huber::default()]
            }
        });
    }
    for z in data.observations() {
        graph.add_factor(fac![
            RangeBearing::new(z.range, z.bearing),
            (X(z.pose), L(z.lm)),
            (0.18, 0.085) as std
        ]);
    }

    let mut opt: GaussNewton = GaussNewton::new_default(graph);
    println!("{:#?}", opt.optimize(values).unwrap());
}

It agrees with our solver to $10^{-8}$ and runs at comparable speed, because it is calling the same sparse Cholesky underneath. What the rewrite buys is type-tagged keys, automatic differentiation instead of hand-derived Jacobians, and serde graph I/O. What no Rust crate gives you yet is the Bayes tree; for incremental smoothing, GTSAM is still the reference implementation.

Putting it together

Run the whole thing on the Apartment loop that opened this chapter — the same corridor, the same biased wheels, the same corner reflectors that the filter of Chapter 14 had to swallow one at a time — and the numbers close Part V's arc:

Seven Levenberg–Marquardt steps, 232 factors, 189 unknowns, $J$ from 3780 to 116.5 — a mean residual of 0.88σ across the whole graph, with the worst single factor at 2.6σ, which is what a correctly weighted least-squares fit is supposed to look like. The numeric factorization is 34 thousand floating-point operations under a minimum-degree ordering: microseconds of arithmetic. The reason this beats the filter is not that it is cleverer. It is that it kept the past, so it could change its mind about it.

Three doors lead out of here.

Chapter 16 removes the known-correspondence assumption. Scan matching produces the between-factors from raw LiDAR, place recognition proposes the loop closures, and the robust kernels of this chapter are what stand between a false proposal and a ruined map — the front end and back end finally meet, as RustSLAM-2D.

Chapter 17 attacks the same full posterior from the opposite side: sample the trajectory, and the map factorizes into independent little Gaussians. Same posterior, completely different computational bet.

Chapter 18 scales the graph to cameras and IMUs, where the number of variables outruns the compute budget and something must be marginalized after all. That is where SEIF's ghost comes back — and where the fill-in you learned to count in the Sparsity Scope becomes an engineering decision with real consequences.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Assemble the micro chain by hand

   Using only the Jacobian rows $(1,0,0)$, $(-1,1,0)$, $(0,-1,1)$, $(0,0,1)$, accumulate $\Omega$ and $b$ for the 1-D example and solve $\Omega\Delta = -b$ by elimination. Then answer in one sentence why both odometry intervals shrank to 0.875 when only one of them is adjacent to the disagreeing measurement. Finally, make the prior ten times stiffer ($\sigma_0 = 0.1$) and predict, before recomputing, which residual grows.

2. Foundation exerciseFDifficulty 2 of 3The pattern claim, and the clique it hides

   Prove that $\Omega_{[j][k]} \ne 0$ if and only if variables $j$ and $k$ appear together in some factor. Draw the block pattern for a graph of 5 poses and 2 landmarks, where landmark 1 is seen from poses 0, 2 and 4. Then Schur-eliminate landmark 1 and show that the pose blocks $(0,2), (0,4), (2,4)$ become nonzero — citing the Gaussian marginalization identity you used. How many fill edges does eliminating landmark 1 create in general, as a function of how many poses saw it?

3. Foundation exerciseFDifficulty 3 of 3Huber's weight, from stationarity

   Derive $w = \rho'(e)/e$ from the stationarity condition of $\sum_k \rho(e_k)$, being careful about the chain rule through $e_k = \norm{r_k(y)}$. Then show that L2's influence $\rho'(e) = e$ is unbounded while Huber's saturates at $k$, and compute the asymptotic weight of a 30σ residual under Huber($k{=}1$), Cauchy($c{=}1$) and Geman–McClure($c{=}1$). Which of the three still moves the map, and by how much relative to a 1σ inlier?

4. Conceptual exerciseCDifficulty 2 of 3Predict, then verify: which ordering wins?

   In w15.2, use the Loop graph. Before touching anything, predict whether chronological or landmarks-first gives the smaller $\operatorname{nnz}(L)$, and write down your reasoning. Check. Now switch to the One landmark, seen by all preset and predict again — your rule of thumb should fail here. Explain the failure in terms of induced cliques, in one paragraph, and state the graph property that actually decides the winner.

5. Conceptual exerciseCDifficulty 2 of 3Predict, then verify: where robustness runs out

   In w15.1, set the kernel to Huber with $k = 1$ and raise the false-closure error until the RMSE readout doubles. Record that magnitude. Now switch to Geman–McClure and find the magnitude at which it fails. Then use w15.4 to explain the difference in terms of the two influence functions, and — using the bad-init row of the stat tiles — write the one-paragraph pitch for graduated non-convexity that you would give to a colleague who wants to ship Geman–McClure tomorrow.

6. Practical exercisePDifficulty 2 of 3A bearing-only factor and a Cauchy kernel

   Implement BearingOnlyFactor (one scalar residual, the wrapped bearing) and add Kernel::Cauchy to the kernel enum. Verify your Jacobian against a finite-difference of the $\bplus$ perturbation to $10^{-6}$ — this is the single most common source of a "my optimizer diverges" bug, and the test takes ten lines. Then build a graph in which a landmark is observed by bearing only from two poses and show that $\Omega$ is singular when the two bearings are parallel. What does that singularity mean physically?

7. Practical exercisePDifficulty 3 of 3Ordering, measured

   Implement minimum-degree ordering in order.rs (greedy, simulating fill as you go) and compare $\operatorname{nnz}(L)$ and wall-clock factorization time against chronological and landmarks-first on a 500-pose synthetic loop. Plot nnz(L) versus problem size for all three on log axes and report the empirical exponents. Then port the same graph to factrs 0.3 and to tiny-solver 0.18 and report agreement (should be $10^{-8}$) and relative timing. Which of the three implementations would you actually ship, and why?

References

1. Thrun, S., Burgard, W., and Fox, D. (2005) Probabilistic Robotics. MIT Press.link to Probabilistic Robotics (opens in a new tab)

   Chapter 11 is this chapter's ancestor — the construct–reduce–solve pipeline that, iterated, is Gauss–Newton, and the source of the spring-mass framing this chapter runs on. Chapter 12's SEIF is the cautionary tale.

2. Dellaert, F. and Kaess, M. (2006) Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. The International Journal of Robotics Research 25(12), 1181–1203.doi:10.1177/0278364906072768 (opens in a new tab)

   The paper that made smoothing beat filtering, by pointing out that the sparse matrix factorization community had already solved the hard part. The ordering section above is its argument, thirty years on.

3. Kümmerle, R., Grisetti, G., Strasdat, H., Konolige, K., and Burgard, W. (2011) g2o: A General Framework for Graph Optimization. IEEE International Conference on Robotics and Automation (ICRA), 3607–3613.doi:10.1109/ICRA.2011.5979949 (opens in a new tab)

   The back end a generation of SLAM systems was built on, and the clearest statement of the graph-optimization interface that factrs and Ceres also implement.

4. Sünderhauf, N. and Protzel, P. (2012) Switchable Constraints for Robust Pose Graph SLAM. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 1879–1884.doi:10.1109/IROS.2012.6385590 (opens in a new tab)

   Robustness as a modeling decision rather than a loss function: give every loop closure a switch variable and let the optimizer turn it off. Step 2 of this chapter's escalation ladder.

5. Dellaert, F. and Kaess, M. (2017) Factor Graphs for Robot Perception. Foundations and Trends in Robotics 6(1–2), 1–139.doi:10.1561/2300000043 (opens in a new tab)

   The standard modern treatment, and the source of this chapter's formulation. Read it next; its elimination-game chapter is the rigorous version of the Sparsity Scope.

6. Yang, H., Antonante, P., Tzoumas, V., and Carlone, L. (2020) Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection. IEEE Robotics and Automation Letters 5(2), 1127–1134.doi:10.1109/LRA.2020.2965893 (opens in a new tab)

   How to get a redescending kernel's outlier immunity without its initialization dependence: anneal the surrogate from convex to non-convex. The answer to the failure w15.4 shows.

7. Chebrolu, N., Läbe, T., Vysotska, O., Behley, J., and Stachniss, C. (2021) Adaptive Robust Kernels for Non-Linear Least Squares Problems. IEEE Robotics and Automation Letters 6(2), 2240–2247.doi:10.1109/LRA.2021.3061331 (opens in a new tab)

   Stop hand-picking ρ and its scale: put the kernel in a one-parameter family and estimate the shape parameter jointly with the state.

8. Doherty, K., Papalia, A., Huang, Y., Rosen, D., Englot, B., and Leonard, J. (2024) MAC: Graph Sparsification by Maximizing Algebraic Connectivity. arXiv:2403.19879 [cs.RO].link to MAC: Graph Sparsification by Maximizing Algebraic Connectivity (opens in a new tab)

   The other way to control fill-in: instead of ordering the graph you have, choose which measurements to keep, using the spectral quantity that predicts estimation error. Where Chapters 18 and 24 pick this up.