Robust SLAM & Frontiers

Chapter 1: M-Estimators: Replace the Square

The squared cost is pathological for outliers because its derivative — the "influence" a measurement exerts on the solution — grows without bound as the residual grows. A measurement 10σ away pulls 100 times harder than one 1σ away. The fix: replace the squared cost with a function that saturates for large residuals. These are called M-estimators (maximum-likelihood-type estimators), introduced by Huber in 1964.

The general robust SLAM objective replaces each squared term with a robust cost ρ:

where ‖r‖_Ω = (r^TΩr)^1⁄2 is the Mahalanobis norm. The scalar u = ‖r‖_Σ⁻¹ is the whitened residual. We choose ρ(u) to grow slowly for large u.

The influence function: how hard can a measurement pull?

The influence function ψ(u) = ρ′(u) tells you how much a measurement with whitened residual u can perturb the solution. For squared cost, ρ(u)=u²⁄2, so ψ(u)=u — unbounded. For a well-designed robust cost, ψ(u) is bounded: no single measurement can pull the solution by more than a fixed amount, no matter how large its residual. This is the key property that rejects outliers.

Weight function

The weight function w(u) = ψ(u)⁄u = ρ′(u)⁄u tells you the effective weight multiplied by u². For squared cost, w(u)=1 (all equal weight). For robust costs, w(u) decreases as u grows — high-residual measurements get down-weighted. This connects M-estimators to the IRLS algorithm in Chapter 3.

Chapter 2: The Four Cost Functions

Four robust costs appear throughout SLAM literature. Each represents a different tradeoff between how aggressively it down-weights outliers and how convex (and thus solver-friendly) it remains.

Squared (standard LS) — baseline

No down-weighting. Unbounded influence. Optimal when ALL measurements are inliers from a Gaussian distribution.

Huber loss — piecewise quadratic/linear

Below threshold k: squared (full weight). Above k: linear growth, so influence ψ(u)=k (capped). The weight function w(u)=k/|u| decays as 1/u for large residuals. Convex everywhere — the optimizer is well-behaved but influence only saturates (linearly), not truly bounded.

Cauchy (Lorentzian) loss — truly bounded influence

Influence is bounded and actually decreases for very large residuals. A measurement at u=100c has near-zero weight. Non-convex for u>c/√2, introducing the local-minima problem (Chapter 4).

Truncated Least Squares (TLS) — hard rejection

Cost is literally constant beyond threshold c: w(u)=0 for |u|>c. The measurement is completely ignored — hard zeroing of the outlier. Highly non-convex. Most aggressive rejection, most severe local-minima problem. But when GNC (Chapter 5) solves the non-convexity, TLS gives the cleanest separation of inliers from outliers.

Hand-computed comparison at u=1 and u=10 (k=c=1)

The squared cost at u=10 is 100 times worse than at u=1; TLS doesn't even notice. This is the quantitative version of "outliers dominate".

Chapter 3: IRLS: Turning Robust Costs into Weighted LS

Minimizing a robust cost function ∑ρ(r_i) looks like a completely different beast from the NLLS machinery of L9. But there is an elegant bridge: Iteratively Reweighted Least Squares (IRLS).

The key observation: the M-estimator gradient condition ∂f⁄∂θ = 0 can be rewritten as a weighted least-squares normal equation with weights w_i = ρ′(u_i)⁄u_i. This gives us an iterative algorithm:

The inner WLS step uses exactly the sparse Cholesky machinery from L13 — just with modified diagonal weights. Each outer iteration re-computes weights based on current residuals, gradually down-weighting high-residual (outlier) measurements.

Worked 1D IRLS example

Five measurements of a single scalar x. True value x=0. Four inliers: z_1..4 = {0.1, -0.2, 0.15, -0.1} with σ=0.3. One outlier: z₅=5.0 with the same σ=0.3 (whitened residual |z₅-x|/0.3≈16 — enormous).

Iteration 0 (plain LS start): Weights all w_i=1. Solution x=(0.1-0.2+0.15-0.1+5.0)/5=0.99. Clearly pulled by the outlier.

Iteration 1 (Cauchy, c=1): Compute residuals at x=0.99: r_1..4≈0.89..1.19 (whitened: ~3-4), r₅=4.01 (whitened: ~13.4). Weights w_i=1/(1+u_i²/c²): w_1..4≈0.06..0.10, w₅=1/(1+179)≈0.0056. New WLS: x = (∑ w_iz_i)⁄(∑ w_i). Sum of w_iz_i for inliers: ~0.008+(-0.016)+0.012+(-0.008)=-0.004. For outlier: 0.0056×5=0.028. Denominator: ~0.078+0.0056=0.084. x≈0.028⁄0.084=0.28. Much better — outlier weight is 10× smaller than each inlier.

After 3-5 iterations: x converges to ≈0.01. The outlier's weight falls below 0.001. The inlier consensus wins.

Chapter 4: The Non-Convexity Trap

There is a catch. Robust cost functions like Cauchy and TLS are non-convex: the cost landscape has multiple local minima. Standard gradient-based optimizers (Gauss-Newton, IRLS) converge to whichever local minimum they start near. This is a problem because:

1. If initialized near a local minimum dominated by the outlier, the optimizer stays there.
2. The "correct" minimum (where inliers agree) may be far in configuration space from the outlier-dominated start.
3. Unlike the convex squared-cost case, there is no guarantee that gradient descent finds the global optimum.

Ironically, a robust cost can sometimes give a worse solution than plain least-squares if the initial estimate is poor. IRLS is not magic — it's a local optimizer.

The 1D intuition: two bowls

Consider a 1D SLAM problem with one strong inlier constraint pulling x toward 0 and one strong outlier constraint pulling x toward 10. With squared cost: two bowls whose gradients always oppose each other — the optimizer finds the weighted average. With TLS: two flat floors with steep walls. If you start near x=10, you are in the outlier's bowl. IRLS converges to x=10. The inlier bowl at x=0 exists but you never reach it.

Chapter 5: Graduated Non-Convexity (GNC)

Graduated Non-Convexity (GNC) is Carlone's answer to the local-minima problem. The core idea: instead of optimizing the non-convex robust cost directly, start with a convex surrogate and gradually "turn up" the non-convexity, annealing toward the true cost. Each step of this schedule is a small perturbation of a problem you just solved — so the solution tracks the global optimum.

The GNC schedule

Define a family of costs ρ_μ(u) parameterized by a control parameter μ such that:

• At μ = μ₀: ρ_μ0 is convex (e.g., the squared cost). Easy to optimize globally.
• At μ = μ_∞: ρ_μ∞ is the target non-convex cost (e.g., TLS).
• The family smoothly interpolates between them.

Algorithm: solve at μ₀, warm-start at μ₁>μ₀ using the previous solution, repeat until μ=μ_∞.

GNC-TLS: the Carlone construction

For truncated least squares with threshold c², define the GNC surrogate (Yang et al. 2020, Carlone group):

At μ→∞: ρ_μ→c²u²⁄u²=c² (TLS plateau). At μ=1: ρ₁(u)=c²u²⁄(2c²+u²) — smoother, more convex. Schedule: multiply μ by a factor μ = μ×1.4 each step. With ~20 steps, we go from convex to effectively TLS.

Why GNC rejects outliers without a good initial guess

At μ=μ₀ (convex surrogate), there is only one basin — the global optimum is the inlier consensus, because convexity guarantees uniqueness. As μ increases, the outlier basin gradually forms, but the solution is already in the inlier basin and tracks it through each small step. The outlier basin never "captures" the solution because each step's perturbation is small relative to the basin width.

Worked μ schedule example

One inlier at z=0 (σ=1), one outlier at z=10 (σ=1). TLS threshold c=2. Start μ=1, multiply by 1.5 each step.

The estimate converges to the inlier truth from any start because the convex phase pinned the solution to the correct basin.

Chapter 6: Switchable Constraints & Consistency Checks

GNC is not the only approach. Two complementary families attack the same problem from different angles.

Switchable Constraints (SC)

Introduced by Sünderhauf & Protzel (2012). Each loop-closure factor gets a continuous "switch variable" s_i ∈ [0,1] that interpolates between "fully active" (s_i=1) and "fully off" (s_i=0). The augmented objective:

The last term is a regularizer penalizing s_k away from 1 (switching off has a cost). The optimizer simultaneously solves for all poses AND all switch variables. A genuine inlier loop closure is easy to satisfy — its switch variable stays near 1. An outlier generates a large residual at s_k=1 — the optimizer prefers to reduce s_k toward 0, paying only the small regularizer cost. Outliers are "switched off" automatically.

The augmented state is larger (one extra scalar per loop closure), but the problem structure is still sparse. Standard sparse solvers handle it. The switch variables provide explicit "which loop closures did we trust?" output.

Max-Mixtures

Olson & Agarwal (2012) model each loop-closure measurement as a mixture of two Gaussians: one tight (inlier model) and one very broad (outlier model). The cost becomes −log max(π_in·N(r;0,Σ_in), π_out·N(r;0,Σ_out)). The max approximation keeps the problem sparse; effectively the inlier Gaussian dominates for small residuals, the outlier Gaussian for large ones.

Pairwise Consistency Maximization (PCM)

A pre-optimization front-end filter developed by Mangelson et al. (2018). Before adding any loop closures to the optimizer, PCM builds a "consistency graph": nodes are candidate loop closures, edges connect pairs that are geometrically consistent with each other (their combined relative-pose forms a plausible loop). Outlier loop closures tend to be inconsistent with the true inliers. PCM finds the maximum clique in the consistency graph — the largest set of mutually consistent loop closures — and only passes those to the back-end optimizer.

Chapter 7: Certifiable & Global Optimality

GNC gives empirically excellent results, but how do you know the solution you got is actually the global optimum? Can you prove it? This is the question of certifiable perception — one of Carlone's central research contributions.

The SE-Sync idea

Rosen et al. (2019) showed that the pose graph optimization problem (SLAM with only relative-pose measurements, no landmarks) can be reformulated as a semidefinite program (SDP). SDPs are convex — they can be solved globally. The catch: the original problem has non-convex constraints (rotation matrices must lie in SO(3)). SE-Sync relaxes these constraints to get an SDP, solves it globally, then checks if the relaxation is tight (i.e., the SDP solution is also feasible for the original problem).

Certificate of global optimality

The SDP gives a dual certificate: a proof that no better solution exists. Specifically, if the relaxation is tight, the dual variable (a PSD matrix called the certificate matrix) has rank 1 — meaning the global optimum of the relaxed problem coincides with a valid rotation solution. When the rank is 1, you have a certificate. When rank > 1, the relaxation is not tight and you know you need to look further.

Practical use in a SLAM pipeline

SE-Sync is not meant to replace iSAM2 for real-time operation — one SDP solve is slow (seconds to minutes for large graphs). Instead it is used as a verification step: run iSAM2 in real-time, periodically run SE-Sync offline to certify that the accumulated map is globally optimal. If certification fails, trigger a re-optimization with GNC. Think of it as a "sanity check" layer.

Chapter 8: Showcase — Robust SLAM Live

A 2D SLAM loop with adjustable outlier injection and method comparison. Adjust the number of outlier loop closures, pick a solver, and see the resulting map. Which constraints get rejected? How bad does plain LS get before robust methods kick in?

Chapter 9: Frontiers & VNAV Series Cheat Sheet

Where the field is going

VNAV Complete Pipeline Cheat Sheet

The full SLAM pipeline in one picture

Related Gleams across the site

The SLAM / robust estimation thread connects to other series. From the Estimation section: Bayes Filter (the filtering vs smoothing distinction), Extended Kalman Filter (the original sequential SLAM approach), Modern SLAM (systems-level: ORB-SLAM3, VINS-Mono). From AI Architectures: NeRF & 3D Gaussian Splatting (the dense neural mapping frontier). From Decision/Control: Model-Based RL (the robot needs this map for planning).

python
# Squared vs Huber vs Cauchy vs TLS cost comparison
import numpy as np

def rho_sq(u): return 0.5 * u**2
def rho_huber(u, k=1.0):
    return np.where(np.abs(u) <= k,
                    0.5*u**2,
                    k*(np.abs(u) - k/2))
def rho_cauchy(u, c=1.0):
    return (0.5*c**2) * np.log(1 + (u/c)**2)
def rho_tls(u, c=1.0):
    return np.minimum(u**2, c**2)

# IRLS weight functions: w(u) = rho'(u) / u
def w_huber(u, k=1.0):
    return np.where(np.abs(u) <= k, 1.0, k/np.abs(u))
def w_cauchy(u, c=1.0):
    return 1.0 / (1 + (u/c)**2)
def w_tls(u, c=1.0):
    return np.where(np.abs(u) <= c, 1.0, 0.0)

# IRLS on 1D data with one outlier
z = np.array([0.1, -0.2, 0.15, -0.1, 5.0])  # last = outlier
x = 0.0  # initial guess
for it in range(10):
    u = (z - x)   # residuals (sigma=1 assumed)
    w = w_cauchy(u, c=1.0)
    x = np.sum(w * z) / np.sum(w)
    print(f"iter {it}: x={x:.4f}, w_outlier={w[-1]:.4f}")

# GNC-TLS schedule
def gnc_tls_weight(u, c=1.0, mu=1.0):
    # Yang et al. 2020 Eq. (11)
    num = c**2 * mu
    denom = (mu + 1) * c**2 + u**2
    rho_mu = num * u**2 / denom
    drho = (2 * num * u * denom - num * u**2 * 2 * u) / denom**2
    w = drho / (2 * u) if abs(u) > 1e-9 else num / denom
    return max(0, w)

mu = 1.0
x = 0.0
for step in range(20):
    u = z - x
    w = np.array([gnc_tls_weight(ui, c=1.0, mu=mu) for ui in u])
    x = np.sum(w * z) / (np.sum(w) + 1e-9)
    mu *= 1.4
    print(f"mu={mu:.1f} x={x:.4f} w_out={w[-1]:.4f}")