To build a map you must know where you are. To know where you are you need a map. SLAM breaks the chicken-and-egg by solving both at once — first with a filter that grows uncertain, then with a graph that snaps the whole world straight.
You wake a robot in a building it has never seen. No GPS — you are indoors. No map — nobody gave it one. It has wheels that count their own turns and a laser that reports how far the nearest walls are. You ask it one thing: drive around, and come back knowing both where you went and what the building looks like.
Pause on how circular that is. To draw the map, the robot must place each wall at the right spot — but "the right spot" is measured relative to the robot, so it needs to know where the robot was when it saw that wall. To know where the robot is, it compares what it sees now against the map — but the map is the thing it is still building. Position needs the map; the map needs position. Neither comes first.
The naive escape is "just trust the wheels." Count wheel turns, integrate them into a position, and call that the robot's pose. This is dead reckoning — reckoning your position purely from your own motion, the way a ship's navigator did before satellites. It is seductive because it needs no map at all.
It also fails, and fails in a specific, ruinous way. Every wheel-tick estimate has a tiny error — a wheel slips a millimeter, the floor is not perfectly flat, the turn was 30.4° not 30°. Each error is small. But you integrate them: today's position is yesterday's position plus today's motion, forever. Small errors do not average out; they accumulate. The estimated path slowly bends away from the truth, and because every wall the robot draws is hung off that drifting path, the whole map smears with it.
Watch it happen. The robot below drives a loop you can see (the faint true path). Its own belief — built only from noisy odometry — is the bright path, and the walls it "maps" are pinned to that belief. Crank the slip up and run it:
The robot drives a square loop. True path is faint; the dead-reckoned belief is bright purple; the mapped landmarks (orange) are placed relative to the drifting belief. Turn up odometry slip and press Run — the belief curls away from truth and the map curls with it. Notice the gap at the end: the robot thinks it is somewhere it is not.
The bright path should end exactly where it started — it drove a closed loop. It does not. That end-of-loop gap is accumulated drift, and it is the central villain of this lesson. Dead reckoning has no way to notice it, because it never compares against anything outside itself.
So what breaks the cycle? Two ideas, and this lesson is the story of moving from the first to the second:
Chapter 0 gave us the intuition; now we say exactly what SLAM computes, because every algorithm in this lesson is a different way of estimating the same quantity. We will name every symbol as it appears.
There are two of them, and SLAM solves for both jointly.
Two streams of data, both noisy.
There is a fork in the road, and which branch you take decides which algorithm you get.
Online (filtering) SLAM. Estimate only the posterior over the current pose and the map:
Past poses are summarized into the current belief and then thrown away — "marginalized out." Memory stays bounded; you can run it forever, in real time. The classic example is EKF-SLAM (Chapter 3).
Full (smoothing) SLAM. Estimate the posterior over the entire trajectory and the map:
Keep every pose. The estimate grows with time, but you can revisit and correct any past pose later — which is exactly what makes loop closure work. This is the factor-graph view (Chapters 5–10).
The single character that differs — xt versus x1:t — is the entire plot of this lesson. Filtering forgets the past to stay cheap; smoothing remembers the past to stay correct. Let's see the difference as a graphical model, where shaded circles are things we measured and open circles are what we want to find:
Toggle between the two. Open circles = unknowns (poses x, landmarks m). Shaded boxes = data (controls u, measurements z). In online SLAM only the current pose is kept (past poses fade). In full SLAM every pose stays in the estimate, so any of them can be corrected later.
Read the arrows as causality. Each pose flows to the next through a control u (the motion model). Each measurement z hangs off a (pose, landmark) pair (the measurement model). The landmarks sit to the side because they are static — every pose can see them. Those two model types are Chapter 2.
Everything in SLAM is built from two probabilistic statements: how the robot moves, and what the robot sees. Get these two right and every algorithm — filter or graph — falls out of them. Let's build each from a concrete picture, then write the math, then explain every symbol.
The robot is at pose xt−1. You command a motion ut (from odometry: "forward 0.5 m, turn 10°"). Where do you end up? In a perfect world, a deterministic function tells you:
Here g is the motion function — for a wheeled robot it is the unicycle/differential-drive update from Lesson 2 (rotate by the commanded turn, step forward by the commanded distance). But the world is not perfect: wheels slip, floors tilt. So we add process noise:
Reading this symbol by symbol: εt (epsilon) is a random nudge added each step. The notation ~ N(0, Rt) means "drawn from a Gaussian (bell curve) centered at zero with covariance Rt." A covariance matrix Rt is just the multi-dimensional version of "variance" — it says how big the nudge typically is in each direction, and whether errors in different directions are linked. Bigger Rt = noisier wheels = trust odometry less.
Now the robot, at pose xt = (x, y, θ), looks at landmark mj = (mj,x, mj,y) and its sensor reports a range r (how far) and a bearing φ (at what angle, relative to where the robot is facing). The geometry is exact — it is just the distance formula and an arctangent:
Symbol by symbol: the range is the straight-line distance from robot to landmark (Pythagoras). The bearing uses atan2 — the "smart arctangent" that returns the correct angle in all four quadrants — to get the absolute direction to the landmark, then subtracts the robot's heading θ so the bearing is measured in the robot's own frame ("20° to my left"). Stacking range and bearing into a vector and adding sensor noise δt (delta):
where h is the measurement function (the range-bearing formula above), zti is the i-th measurement taken at time t, and Qt is the measurement-noise covariance (small Q = trust the sensor a lot).
Numbers make it real. Put the robot at xt = (2, 1, 30°) and a landmark at mj = (5, 5). What should the sensor read?
So the predicted measurement is h = (5.0 m, 23.1°). If the actual sensor returns (5.2 m, 21°), the gap — called the innovation or residual — is (−0.2 m, +2.1°). That residual is gold: it is the disagreement between belief and reality, and every SLAM algorithm works by squeezing residuals toward zero.
Drag the robot and the landmark below and watch the predicted range and bearing update live:
Drag the robot or the landmark. The wedge shows the robot's heading; the readout shows range r and bearing φ (in the robot's own frame). Rotate the robot with the slider — range stays fixed but bearing shifts, because bearing is relative to heading.
−θ is one of the most common SLAM bugs: your landmarks spin around the robot as it turns.The earliest SLAM algorithm took the Extended Kalman Filter you'll meet in Lesson 13 and asked: what if the "state" the filter tracks is not just the robot's pose, but the robot's pose and every landmark, stacked together? That one idea is EKF-SLAM, and it is the cleanest place to see the chicken-and-egg coupling become a number.
We glue the robot pose and all N landmarks into one tall vector y — the augmented state:
Count the entries: 3 for the pose plus 2 per landmark, so 3 + 2N numbers in all. The filter tracks a Gaussian belief over this whole vector — a mean μ (our best single guess for every coordinate) and a covariance Σ, a (3+2N)×(3+2N) matrix of all the uncertainties and — crucially — the correlations between them.
Like any Kalman filter, EKF-SLAM alternates predict (the robot moved) and update (the robot saw something).
Predict. Apply the motion model to the pose part of μ (the landmarks are static, so their means don't move). Then grow the covariance — moving adds process noise Rt, so the robot's uncertainty increases:
The bar means "predicted, before the measurement." Gt is the motion Jacobian — the matrix of partial derivatives of g, which linearizes the (nonlinear) motion so the Gaussian stays a Gaussian. Because only the pose moved, Rt has nonzero entries only in its top-left 3×3 block.
Update. The robot observes landmark j and gets zt. Compute the predicted measurement h(μ̄, j), take the residual, and fold it in through the Kalman gain K:
where Ht is the measurement Jacobian (how the measurement changes as the state changes), S is the innovation covariance, and K is the trust slider between prediction and measurement. The magic is in K = Σ̄ Ht⊤S−1: because Σ̄ carries pose-landmark correlations, a single measurement of one landmark updates the pose and every correlated landmark at once. See one wall clearly, and your belief about the whole map sharpens.
Let's watch a correlation actually grow. We track a 1-D toy to keep the arithmetic on one line: the state is (p, ℓ) — robot position p and one landmark position ℓ, on a line.
Start. The robot is certain it is at the origin, the map is unknown:
Read that covariance: pose variance 0.01 (tiny — we're sure where we are), landmark variance 100 (huge — no idea), and the off-diagonals are 0 — pose and landmark are, for now, uncorrelated.
Observe the landmark. The sensor says "the landmark is at distance 10," i.e. z = ℓ − p = 10, with measurement variance Q = 1. The measurement function is h = ℓ − p, so the Jacobian is H = [−1, 1] (h drops by 1 if p rises by 1; rises by 1 if ℓ rises by 1).
Stare at the off-diagonal: it was 0, now it is +0.0099. The act of observing the landmark from this pose correlated them. The landmark's variance also collapsed from 100 to 0.99 — one good measurement turned "no idea" into "pretty sure." And here is the punchline of EKF-SLAM: from now on, if the robot's pose estimate ever shifts, that off-diagonal will drag the landmark along with it. The map and the trajectory are now permanently entangled in one covariance matrix.
Run the full 2-D version below. Drive the robot; covariance ellipses show the uncertainty of the pose and of each landmark. Watch the pose ellipse grow as it drives blind, then shrink the instant it re-observes a known landmark — and watch every landmark ellipse twitch in sympathy, because they are all correlated.
Press Drive. The robot follows a path, predicting (ellipse grows) and updating whenever a landmark is in sensor range (ellipse shrinks). Purple = pose uncertainty; orange = each landmark's uncertainty; faint orange dots are the true landmarks. Turn sensor range down to starve the filter of updates and watch uncertainty balloon.
That dense, fully-correlated covariance is exactly what makes EKF-SLAM scale badly. With N landmarks, Σ is (3+2N)×(3+2N) — it has on the order of N² entries, and the update touches all of them. Double the landmarks, quadruple the work and memory. A map of a building has thousands of landmarks; N² kills you.
EKF-SLAM works, runs online, and is beautifully simple. Yet modern SLAM systems — the ones in your phone's AR, in self-driving stacks — almost universally abandoned it. To understand why, we have to look hard at the one move that defines a filter: it throws the past away.
Recall from Chapter 1 that online SLAM estimates only p(xt, m | …) — the current pose. Where did poses x1, …, xt−1 go? They were marginalized out: their information was folded into the current belief and the variables themselves deleted. This is what keeps memory bounded.
But marginalization is irreversible, and it has two nasty consequences:
Picture the drifted loop from Chapter 0. The robot has gone around and, thanks to drift, its belief about the final corner is half a meter from where it started. Then it recognizes the starting corner: "this is the same place as pose 1." That is a constraint linking pose t directly back to pose 1.
What should happen? The error shouldn't dump entirely onto the last pose — it should spread backward across the whole loop, nudging every intermediate pose a little, until the trajectory is globally consistent. But spreading error backward requires the past poses to still be there to nudge. A filter marginalized them away. A method that kept them can.
A drifted loop with a loop-closure constraint (dashed red) tying the end back to the start. Filter: the past is gone, so only the last pose can move — the closure yanks it but the loop stays kinked. Smoothing: every pose survives, so the correction distributes around the whole loop and it closes smoothly. Toggle to compare.
The shift this motivates is profound but simple to state: stop filtering, start optimizing. Keep the entire trajectory and map as free variables. Treat every odometry reading and every measurement as a constraint that the variables should satisfy. Then find the configuration of all variables that satisfies all constraints as well as possible — a single, global optimization problem.
You might worry: optimizing over thousands of poses and landmarks sounds far more expensive than a filter. The opposite is true in practice, and the reason is the sparsity we flagged in Chapter 1: each constraint touches only a couple of variables, so the optimization problem is mostly empty and solvable in near-linear time. Filtering destroys that sparsity (every variable becomes correlated with every other); smoothing preserves it. Chapter 7 makes this rigorous.
We decided to keep every variable and treat every reading as a constraint. A factor graph is the picture of exactly that — and it is not just a diagram, it is the literal data structure modern SLAM solvers store and operate on.
A factor graph is bipartite: it has two distinct kinds of node, and edges only ever connect one kind to the other.
An edge connects a factor to every variable that factor depends on — and to no others. That "and to no others" is the sparsity, made visual.
Prior factor. Touches one pose (usually x1). It pins the map to a reference frame — "the first pose is the origin." Without it the whole graph could slide or spin freely (everything is relative); the prior nails it down.
Odometry factor. Touches two consecutive poses (xt−1, xt). It says "the motion between these two poses should match what odometry reported." This is the motion model as a constraint.
Observation factor. Touches one pose and one landmark (xt, mj). It says "the range/bearing from this pose to this landmark should match what the sensor reported." The measurement model as a constraint.
Loop-closure factor. Touches two non-consecutive poses (xi, xj) that turn out to be the same place. Structurally it's just another between-poses constraint — but it reaches across the graph and is the single most powerful edge there is (Chapter 8).
The factor graph encodes a probability: the posterior factors into a product of per-factor likelihoods. Finding the most-probable configuration — the Maximum A Posteriori (MAP) estimate — means maximizing that product. Taking a negative logarithm (which turns products into sums and flips max into min) and using Gaussian noise (whose negative-log is a squared term) gives the form every SLAM solver actually minimizes:
Decoding it: the sum runs over every factor i. ei(x) is that factor's error vector (measured minus predicted). Ωi (capital omega) is the factor's information matrix — the inverse of its noise covariance, so a precise sensor (small covariance) gives a large Ω and its errors are penalized heavily. The expression e⊤Ωe is a weighted squared error (a "squared Mahalanobis distance"). So MAP SLAM is a weighted nonlinear least-squares problem: make all the residuals small, weighting each by how much you trust it.
Build one below. Click to drop poses (purple) and landmarks (orange); the tool auto-adds odometry factors between consecutive poses and lets you draw observation factors. Watch the bipartite structure form — squares (factors) always between circles (variables).
Pick a tool, then click the canvas. Add pose drops a pose and auto-links it to the previous pose with an odometry factor. Add landmark drops a landmark. Observe: click a pose then a landmark to add a observation factor. Close loop: click two poses to add a loop-closure factor. Squares are factors; circles are variables.
Chapter 5 turned SLAM into "minimize a sum of weighted squared errors." Now we actually minimize it. The errors are nonlinear (the measurement function has square roots and arctangents), so we can't solve it in one shot. Instead we use the workhorse of robotics estimation: Gauss-Newton — linearize, solve a linear system, step, repeat.
We minimize F(x) = Σi ei(x)⊤Ωiei(x) over the full variable vector x (all poses and landmarks stacked). Each ei(x) is the residual of factor i — measured value minus what the current x predicts.
Near our current guess x, approximate each error as linear in a small step Δ using a first-order Taylor expansion:
where Ji is the Jacobian of factor i — the matrix of partial derivatives of its error with respect to the variables it touches. Because a factor touches only a few variables (sparsity again!), Ji is zero everywhere except in the columns for those variables.
Substitute the linear approximation into F, expand the squares, and demand that the derivative with respect to Δ be zero (the minimum of a quadratic). Out drops a single linear system — the normal equations:
Name the pieces. The left matrix H = Σi Ji⊤ΩiJi is the (approximate) Hessian — the curvature of the cost, also called the information matrix of the whole problem. The right side b = −Σi Ji⊤Ωiei is the gradient (which way is downhill). Solve H Δ = b for the step Δ, apply it (x ← x + Δ), and repeat from the new point until Δ is tiny.
Let's grind one full Gauss-Newton step by hand. Three poses on a line, positions x1, x2, x3. The constraints:
Give every factor information Ω = 1 for simplicity. The four errors at the current guess (start from odometry: x = (0, 1, 2)) are:
Only the loop-closure error is non-zero — the rest of the graph already agrees with itself; the conflict is entirely "odometry says 2, loop says 1.6." Build H = Σ Ji⊤Ji (each term is the outer product of a row with itself), summing the four:
And the gradient side b = −Σ Ji⊤ei — only the loop term contributes (e=0.4, row [−1,0,1]):
Now H is singular by itself (the graph can slide freely), but the prior fixed x1, so effectively we solve with x1 pinned. Solving HΔ = b with Δ1 = 0 gives:
Update: x ← (0, 1, 2) + Δ = (0, 0.867, 1.733). Look what happened: the 0.4 m of loop-closure error did not dump entirely onto x3. It spread backward — x2 moved −0.133, x3 moved −0.267, exactly the "distribute the correction over the whole loop" behavior we wanted in Chapter 4. The optimization did automatically what the filter could not. Because these constraints are linear, one step is exact; nonlinear SLAM just repeats this a few times.
python import numpy as np # Each factor: (rows of Jacobian, error value, information Omega) # Variables: x1, x2, x3 (indices 0,1,2) def gauss_newton_step(x, factors): n = len(x) H = np.zeros((n, n)) # information / approx Hessian b = np.zeros(n) # gradient (negative) for (J, e, omega) in factors: J = np.array(J, dtype=float) H += J.reshape(-1,1) @ J.reshape(1,-1) * omega # Jᵀ Ω J b += -J * e * omega # -Jᵀ Ω e H[0,:] = 0; H[0,0] = 1; b[0] = 0 # pin x1 via the prior delta = np.linalg.solve(H, b) # solve H Δ = b return x + delta x = np.array([0.0, 1.0, 2.0]) # start from odometry factors = [ ([1,0,0], x[0]-0.0, 1.0), # prior x1=0 ([-1,1,0], (x[1]-x[0])-1.0, 1.0), # odo 1->2 ([0,-1,1], (x[2]-x[1])-1.0, 1.0), # odo 2->3 ([-1,0,1], (x[2]-x[0])-1.6, 1.0), # loop 1->3 ] x = gauss_newton_step(x, factors) print(x) # -> [0. 0.867 1.733] the correction spread backward
And the idiomatic, batch form — stack all factor rows into one big sparse Jacobian J and weight vector, and let the linear solver do the work:
python J = np.array([[1,0,0],[-1,1,0],[0,-1,1],[-1,0,1]], float) e = np.array([0.0, 0.0, 0.0, 0.4]) # residuals at x=(0,1,2) Omega = np.diag([1,1,1,1.0]) # per-factor information H = J.T @ Omega @ J # Jᵀ Ω J b = -J.T @ Omega @ e # -Jᵀ Ω e H[0,:] = 0; H[0,0] = 1; b[0] = 0 # anchor delta = np.linalg.solve(H, b) print(delta) # -> [0. -0.133 -0.267]
We keep promising that solving over thousands of variables is cheap. Here is the payoff. The whole reason graph SLAM scales to entire cities is one structural fact about the Hessian H = Σi Ji⊤ΩiJi: it is sparse — mostly zeros.
Consider one factor, say an odometry factor between poses x5 and x6. Its error depends on only those two variables, so its Jacobian Ji is non-zero only in the x5 and x6 columns. When you form Ji⊤ΩiJi, the result is non-zero only in the four blocks where {x5,x6} meet {x5,x6}.
Summing over all factors, Hab (the block linking variable a to variable b) is non-zero only if some factor directly connects a and b. No shared factor → a structural zero. Since each variable participates in only a handful of factors, each row of H has only a handful of non-zeros. The Hessian's sparsity pattern is exactly the adjacency structure of the factor graph.
To solve HΔ = b we factor H (it is symmetric positive-definite) as H = LL⊤ — Cholesky factorization, where L is lower-triangular — then solve two easy triangular systems. The catch: Cholesky can create new non-zeros, called fill-in, turning a sparse H into a dense L and destroying the speedup.
The fix is variable ordering: permute the rows/columns of H before factoring so fill-in stays minimal. Reorder the variables cleverly (algorithms like COLAMD do this automatically) and the factorization stays nearly as sparse as H. This single trick — sparse Cholesky with a good ordering — is what lets solvers like GTSAM and g2o optimize tens of thousands of poses in milliseconds.
A pose-chain factor graph with landmarks. The grid is the Hessian H: a filled cell means "these two variables share a factor." Add loop closure drops a long-range constraint — watch one off-diagonal cell light up far from the diagonal. Marginalize a pose shows how removing a variable (the EKF move) fills in its neighbors with new correlations.
Notice the structure: odometry factors put entries right next to the diagonal (a band), landmark observations add a few arms, and a loop closure plants a lone entry far off-diagonal. A band is trivially cheap to factor; the off-diagonal loop-closure entries are what good ordering manages. And marginalizing a pose — deleting its row/column — couples all its former neighbors, the dense fill-in that dooms the filter.
We've mentioned loop closure in every chapter. Now it gets its own, because it is the single most important event in any SLAM run — the moment accumulated drift gets annihilated rather than merely slowed.
A loop closure is recognizing that the place you're at now is a place you visited before. The robot drove a long loop, drift bent its trajectory estimate, and then it comes back to a corner it already mapped and says: "I've been here. The pose I'm computing now and the pose x1 from the start are the same physical location."
Structurally, that statement is just a between-poses factor — a constraint xt − x1 ≈ 0 (or whatever relative transform the re-observation implies). But unlike an odometry factor, which only links neighbors, a loop closure reaches across the entire graph. When Gauss-Newton solves the system, that one long-range constraint forces a global correction: as we saw in Chapter 6's hand calculation, the error spreads backward over every pose in the loop, snapping the whole trajectory into consistency.
The geometry of loop closure is easy; recognizing the revisited place is the hard part. This is data association — deciding which current observation corresponds to which previously-seen landmark or place. Get it right and drift evaporates. Get it wrong — a false loop closure, claiming two different places are the same — and you've welded the map into a contorted, confidently-wrong shape that no amount of optimization can fix.
Watch a drifted loop snap straight. Drive the robot around; odometry drifts the loop open. Then hit Detect loop closure and Optimize — Gauss-Newton runs, and the kinked trajectory collapses onto the consistent one, dragging the map with it.
Press Drive loop — the robot's odometry-only estimate (purple) drifts, ending far from the start (you can see the gap). Detect loop closure adds the green constraint tying end to start. Optimize runs Gauss-Newton: watch the drifted loop collapse onto the true loop (faint), correction spread over every pose.
Here is the beautiful part. Everything we built — factor graph, weighted least squares, Gauss-Newton, sparse Cholesky — is not specific to range-bearing SLAM. Swap in a camera and the identical machinery becomes bundle adjustment (BA), the gold standard for refining 3-D reconstructions and the engine inside structure-from-motion (Lesson 9) and visual SLAM.
In BA the variables are camera poses (where each photo was taken, in 3-D) and 3-D points (the landmarks). The data are 2-D pixel observations — where each 3-D point projected onto each image. The factor connecting camera i and point j uses the reprojection error:
where zij is the observed pixel location, π is the camera projection function (the pinhole model from Lesson 9: take a 3-D point, transform it into the camera's frame, divide by depth, apply intrinsics → a pixel), and the error is the gap, in pixels, between where the point should appear and where it was seen. "Bundle adjustment" is named for adjusting the bundle of light rays from each point so they all converge correctly.
BA's Hessian has a special, exploitable sparsity. Camera poses are few; 3-D points are many. Crucially, points don't observe each other and cameras don't observe each other — every factor links one camera to one point. So the point-point block of H is block-diagonal, which lets you eliminate all the points cheaply in one step (the Schur complement), shrinking the problem to just the handful of cameras. This "reduced camera system" trick is why BA over thousands of points stays fast — the same sparsity-exploitation philosophy as Chapter 7, specialized to the camera-vs-point structure.
Cameras (blue) view 3-D points (orange). The thin lines are the rays from each camera to its observation; where a ray misses the point is the reprojection error (red). Press Optimize — BA jointly nudges camera poses and point positions to shrink all the red gaps at once. Press Perturb to knock things out of alignment first.
Time to put every piece in one place. Below is a complete 2-D pose-graph SLAM system. You drive a robot around a loop; it accumulates drifting odometry and sights landmarks along the way; then you trigger a loop closure and watch Gauss-Newton snap the entire trajectory and map into consistency — while a side panel shows the total error dropping per iteration and the factor graph it's solving.
Press Drive to send the robot around the loop, accumulating drifted odometry and landmark sightings. When it returns near the start, Close loop lights the green constraint. Optimize runs Gauss-Newton iteration-by-iteration — the error meter (right) drops and the trajectory + landmarks snap toward truth. Add noise makes the drift worse so the correction is more dramatic.
The error meter is the literal value of F(x) = Σi ei⊤Ωiei — the total weighted squared residual across every factor. Before closure it's small-ish (odometry agrees with itself) but the map is wrong. The instant the loop-closure factor enters, the error jumps (now there's a big residual: odometry says the loop didn't close, closure says it must). Each Gauss-Newton step then drives it back down as the variables rearrange to satisfy everyone. When it flattens, you've reached the MAP estimate — the most probable trajectory and map given all the data.
Try breaking it: crank the drift to maximum before driving, so the loop ends wildly open, then optimize — watch how much of the error one loop-closure factor can absorb. Then imagine what a false closure would do (Chapter 8): the same machinery that fixes a true loop would just as confidently warp the map for a wrong one. The optimizer trusts what you tell it; data association is where the danger lives.
You came in with a chicken-and-egg paradox and leave with two ways to crack it: a filter that fuses motion and measurement online, and a graph that re-optimizes the whole history to kill drift. Here's the whole lesson on one page.
| EKF-SLAM (filtering) | Graph-SLAM (smoothing) | |
|---|---|---|
| Estimates | current pose + map: p(xt, m | …) | full trajectory + map: p(x1:t, m | …) |
| Data structure | dense covariance Σ (3+2N)² | sparse factor graph / information matrix H |
| Past poses | marginalized away (gone) | kept — can be re-optimized |
| Linearization | once, never revisited | re-linearized each iteration |
| Loop closure | weak — can only nudge current pose | strong — correction spreads over whole loop |
| Scaling | O(N²) per update | ~O(N) with sparse Cholesky + ordering |
| Used today? | historical / small problems | yes — GTSAM, g2o, Ceres, ORB-SLAM |
SLAM sits at the crossroads of perception and estimation — here's where to go deeper: