A LiDAR draws the world in precise three-dimensional ink — but it draws slowly, smears the page when the robot moves, and goes blind in a featureless tunnel. An IMU is fast, never blinks, and drifts. Fuse them and you get a map you can trust. And the language that ties it all together — the modern lingua franca of robot estimation — is the factor graph.
Picture a delivery robot rolling down a warehouse aisle. On its roof spins a LiDAR — a laser rangefinder that fires thousands of beams a second, each one bouncing off a shelf or a wall and coming back to report, to the centimeter, exactly how far away that surface is. Sweep the laser around and you get a point cloud: a dense, metric, three-dimensional sketch of the room. No camera gives you geometry this clean. The walls are walls, the shelves are shelves, and every point carries a real distance in meters. If you wanted to draw a robot a map of the world, the LiDAR is the pen you'd hand it.
But that pen has three flaws, and they are the entire reason this lesson exists. First, it is slow. A spinning LiDAR completes one full revolution about ten times a second — 10 Hz. Between two complete scans, 100 milliseconds pass, and in 100 milliseconds a robot moving at walking speed has traveled fifteen centimeters and possibly turned several degrees. So you get a gorgeous picture of the world only ten times a second, with nothing to tell you what happened in between.
Second — and this one is sneaky — the LiDAR smears. A spinning laser does not capture the whole point cloud in a single instant, the way a camera flash freezes a scene. It sweeps. The first beam in a scan fires at the start of the revolution; the last beam fires 100 ms later, after the robot has moved. So the "single" point cloud is really thousands of points captured from thousands of different robot positions. If the robot is moving, a straight wall comes back bent, because the early points were measured from one place and the late points from another. This is motion distortion, or scan skew, and an uncorrected skewed cloud poisons everything downstream.
Third, the LiDAR can go blind in plain sight. Roll it into a long, straight, featureless tunnel — or out into an open field, or down a corridor of identical doors — and the laser sees the same geometry no matter where along the axis the robot sits. The point cloud at the tunnel entrance looks identical to the point cloud ten meters in. The LiDAR can tell you precisely how far the walls are sideways, but it has no idea how far the robot has slid forward. We call this geometric blindness degeneracy, and it is the LiDAR's tunnel-equivalent of GPS dying under an overpass.
Now bring in the IMU — the accelerometer-and-gyroscope chip you met in Lesson 13. It is the LiDAR's mirror image. It runs at hundreds to a thousand hertz (fast where the LiDAR is slow). It measures motion continuously and instantaneously, so it knows exactly where the robot was at each microsecond a laser beam fired (it can un-smear the scan). And it works everywhere — tunnel, field, corridor — because it feels motion directly rather than reading geometry (it sees through degeneracy). Its only flaw is the one we hammered in Lessons 0 and 13: it drifts, because position comes from integrating noisy acceleration twice.
Line them up and the complementarity is almost too perfect:
| Property | LiDAR | IMU |
|---|---|---|
| Rate | slow (~10 Hz) | fast (~100–1000 Hz) |
| What it gives | precise, metric 3-D geometry | raw acceleration + angular rate |
| Absolute or relative? | absolute geometry of the scene | change only (must integrate) |
| Error over time | bounded (a scan is as good now as in an hour) | drifts (grows as t²) |
| Motion distortion | smears while it sweeps | none — samples a point in time |
| Failure mode | degenerate in self-similar scenes | works anywhere, always |
| What it does for the partner | kills the IMU's long-term drift | de-skews the scan & bridges scans |
Read the last row top to bottom and the fusion writes itself. The IMU de-skews the LiDAR's smeared sweep and bridges the 100 ms gaps between scans, giving a continuous high-rate pose. The LiDAR, in return, registers each de-skewed scan against the world and resets the IMU's drift ten times a second. Each sensor patches exactly the hole the other has. This is the same complementary logic as GPS+IMU from Lesson 1 — only now the absolute anchor is geometry instead of satellites, which means it works indoors, underground, and on Mars.
Watch it before we name anything. Below, a robot drives a path while a LiDAR sweeps. Run it on LiDAR alone (slow, sparse, and — if you crank the speed — smeared), on IMU alone (smooth but drifting), and fused (de-skewed LiDAR resets the drift every scan).
A robot drives the dashed truth path. Pick a mode and press Run. LiDAR-only jumps between slow scans and bends them at high speed; IMU-only is smooth but slides off truth; fused uses the IMU between scans and snaps to the de-skewed LiDAR each scan. Slide speed up to worsen the LiDAR smear.
LiDAR alone stutters from scan to scan, and as you raise the speed the registered map bends where the smear corrupts it. IMU alone glides but its line peels away from the truth as the run goes on. The fused line is smooth like the IMU and glued to the truth like the LiDAR — because each scan, the laser's geometry yanks the IMU's accumulating drift back to zero.
You have spent sixteen lessons building up the pieces: the Kalman filter (Lesson 5), the IMU (Lesson 13), GPS+IMU complementary fusion (Lesson 1). This lesson assembles them into the representation that modern robotics actually ships: the factor graph, where every sensor reading — a LiDAR registration, an IMU integration, a GPS fix, a loop closure — becomes a factor (a soft constraint), and the robot's whole trajectory becomes a graph of variables (poses) tied together by those factors. Solving the graph means finding the trajectory that best satisfies all the constraints at once. And here is the payoff we will prove in Chapter 3: the Kalman filter you already know is just the simplest possible factor graph — a single chain. Everything in this series has been a factor graph in disguise.
Before a LiDAR scan can be used for anything, it must be repaired. We saw why in Chapter 0: a spinning laser captures its thousands of points over a full 100 ms sweep, and if the robot moves during that sweep, the points are measured from thousands of slightly different poses. The cloud arrives distorted. The first job of any LiDAR-inertial system — before registration, before mapping, before anything — is to undo that distortion. This is motion de-skew (also called motion compensation or de-warping), and it is the single most important thing the IMU does for the LiDAR.
Let's be precise about what goes wrong. Suppose the robot drives straight at 1.5 m/s past a perfectly flat wall. The LiDAR begins its sweep at time t = 0 and finishes at t = 0.1 s. A point measured early in the sweep is recorded relative to the robot's pose at t = 0; a point measured at the end is recorded relative to the robot's pose at t = 0.1 s — but the firmware naively stamps all of them as if they came from a single pose. Over the 0.1 s sweep the robot moved 1.5 × 0.1 = 0.15 m. So the late points are reported 15 cm closer (or farther) than they truly are relative to the assumed pose. The flat wall comes back as a wall with a 15 cm slope baked in — a wall that isn't real.
The fix is conceptually simple and it is exactly why we need a fast sensor. The IMU runs at, say, 200 Hz — so during the 100 ms LiDAR sweep it produces about 20 motion samples. Integrate those samples and you get a little movie of the robot's pose throughout the sweep: pose at t = 0, at t = 5 ms, at t = 10 ms, and so on. Now, for every laser point, you know its exact firing time (the LiDAR stamps each point), so you can look up where the robot was at that instant and transform the point back into a single common reference frame — usually the pose at the start (or end) of the sweep.
Formally, let T(τ) be the robot's pose at time τ within the sweep (a position and orientation, obtained by integrating the IMU from the sweep's start). A point p measured at time τ in the sensor frame is corrected to the reference frame by:
In words: take the raw point p, push it out into the world using the pose it was actually measured from, T(τ), then pull it back into the reference pose Tref. Every point gets the transform appropriate to its own timestamp. After this, all points live in one frame as if the robot had been frozen for the whole sweep — the bent wall straightens out.
Let's de-skew a single point by hand, keeping it 1-D for clarity. The robot drives forward (call it the +x direction) at a constant 1.5 m/s. The LiDAR sweep runs from t = 0 to t = 0.1 s, and we choose the reference frame to be the robot's pose at the start, t = 0. A particular laser point comes back reporting a wall 3.00 m ahead, and it was fired at τ = 0.08 s into the sweep (near the end).
Step 1 — where was the robot at τ = 0.08 s, relative to the start? It drove forward 1.5 × 0.08 = 0.12 m. So when that beam fired, the robot was 0.12 m ahead of the reference pose.
Step 2 — the raw point says "wall is 3.00 m ahead of me right now," but "me right now" is 0.12 m ahead of the reference. So in the reference frame the wall is at 3.00 + 0.12 = 3.12 m... wait — that's the wrong sign, and getting the sign right is the whole game, so let's be careful. The robot has moved toward the wall by 0.12 m, so by the time the beam fired the robot was closer; a wall that reads 3.00 m from the moved-forward pose is actually 3.00 + 0.12 = 3.12 m from the original reference pose. The de-skewed range is:
Without de-skew, the system would have logged this point at 3.00 m in the reference frame — a 12 cm error on a single point, and points fired even later in the sweep would be off by even more. Multiply that by thousands of points across the sweep, each smeared by a different amount, and the "flat wall" registers as a tilted, fattened smear. De-skew removes it point by point, each correction proportional to how late in the sweep that point was fired.
Below, drive the robot through a structured scene and watch the LiDAR's raw (smeared) scan versus the IMU-de-skewed scan, with the registered map building up. Crank the speed slider and the raw scan bends badly while the de-skewed one stays crisp.
A robot drives through a room of walls (the true geometry, dashed). The LiDAR sweeps as it moves. Raw scan is smeared by motion; de-skewed scan uses the IMU to un-bend it; the accumulated map grows from the de-skewed clouds. Raise speed to worsen the smear — watch the raw walls bend while the de-skewed walls stay straight.
We now have clean, de-skewed point clouds — one every 100 ms. The next question is the heart of LiDAR odometry: given two overlapping point clouds, how much did the robot move between them? The answer is registration: find the rigid transform (a rotation and translation) that best snaps the new cloud onto the previous one. If you know how the clouds line up, you know how the robot moved, because the world didn't move — only the sensor did.
The classic algorithm is Iterative Closest Point (ICP), and it's a beautifully simple loop. (1) For each point in the new cloud, find its closest point in the old cloud — guess that they're the same physical surface. (2) Compute the transform that, on average, best aligns those guessed pairs. (3) Apply the transform, which moves the new cloud closer. (4) Repeat — the closest-point guesses get better each round, and the clouds slide into alignment over a handful of iterations.
The naive version, point-to-point ICP, measures alignment error as the straight-line distance between each paired point and its closest neighbor. Minimize the sum of those squared distances and you've aligned the clouds. It works — but it fights the geometry, and here's why. A LiDAR scan samples a surface, but it never samples the same spot on that surface twice. Sweep a flat wall now, drive forward, sweep it again: the second scan hits the wall at slightly different spots than the first. Point-to-point ICP, demanding that each new point land exactly on top of an old point, will try to slide the clouds along the wall to pair up the nearest dots — a motion that doesn't actually reduce the real misalignment at all. The points are allowed to slide freely within the surface, and point-to-point can't tell good sliding from bad.
Here is the residual that point-to-plane minimizes. Take a new point p and its corresponding old surface, described by a point q on the surface and the surface's unit normal n (the direction perpendicular to the local plane). After applying a candidate transform that moves p to p', the error is the distance from p' to the plane — which is just the component of the gap (p' − q) along the normal:
Read each symbol. p' is the transformed new point. q is a point on the old surface. n is the unit normal of that surface. The dot product n · (p' − q) projects the gap onto the normal direction, throwing away any component that runs along the surface — exactly the component we want to ignore. When p' sits anywhere on the plane, the gap is purely sideways, the dot product is zero, and the residual vanishes. Point-to-plane minimizes the sum of r² over all correspondences, sliding the cloud until every new point lies in its surface.
Numbers make it concrete. Suppose a new point sits at p' = (2.1, 0.3) (2-D for clarity). It corresponds to a wall whose nearest sampled point is q = (2.0, 0.0), and the wall is vertical, so its normal points horizontally: n = (1, 0). The gap is:
The point-to-point error would be the full length of that gap: √(0.1² + 0.3²) = √0.10 ≈ 0.316. But that 0.3 in the second coordinate is motion along the wall — harmless. The point-to-plane residual projects onto the normal:
The point-to-plane residual is just 0.1 m — the true perpendicular distance into the wall — while point-to-point reported 0.316, three times larger, padded with the irrelevant sideways gap. Minimizing 0.1 pushes the point straight into the wall and stops; minimizing 0.316 would also try to drag it sideways to the nearest old dot, a motion that registers nothing. Point-to-plane sees the real misalignment; point-to-point sees noise on top of it.
In 2014 Zhang and Singh published LOAM (LiDAR Odometry and Mapping), which made LiDAR odometry fast and accurate enough to be practical, and its central trick is feature extraction. Rather than registering every one of the tens of thousands of raw points (expensive and noisy), LOAM extracts two kinds of feature points from each scan by looking at local curvature — how much a point's neighbors bend around it:
So LOAM registers each scan by minimizing a mix of point-to-edge and point-to-plane residuals over only the few hundred most informative feature points — a fraction of the full cloud. The edges pin down the directions a plane can't constrain (a smooth wall leaves you free to slide along it, but a corner doesn't), and the planes pin down the directions the edges leave loose. Together they constrain the full 6-degree-of-freedom motion — except in degenerate scenes where the features don't span all directions, which is exactly the failure we'll dissect in Chapter 6.
Here is a single point-to-plane ICP step in commented numpy. It finds correspondences, builds the linearized least-squares system, solves for the incremental transform, and applies it. Run this in a loop and the clouds converge.
python import numpy as np from scipy.spatial import cKDTree def icp_point_to_plane_step(P, Q, normals, R, t): # P: new cloud (N,3). Q: old cloud (M,3) with per-point unit normals. # R,t: current estimate of the transform mapping P into Q's frame. P_t = (R @ P.T).T + t # apply current transform to new cloud tree = cKDTree(Q) _, idx = tree.query(P_t) # closest old point for each new point q = Q[idx] # matched surface points n = normals[idx] # surface normal at each match # Linearize: small rotation w (3,) + translation d (3,). For each pair, # residual r = n . (P_t + w x P_t + d - q). Stack into A x = b, x = [w, d]. A = np.hstack([np.cross(P_t, n), n]) # (N,6) Jacobian rows b = -np.sum(n * (P_t - q), axis=1) # (N,) point-to-plane residuals x, *_ = np.linalg.lstsq(A, b, rcond=None) # normal equations, least squares w, d = x[:3], x[3:] dR = np.eye(3) + np.array([[0,-w[2],w[1]], # small-angle rotation update [w[2],0,-w[0]], [-w[1],w[0],0]]) R_new = dR @ R t_new = dR @ t + d rms = np.sqrt(np.mean(b**2)) # registration error this iteration return R_new, t_new, rms # loop until rms stops dropping
The compact mental model: each iteration linearizes the point-to-plane error around the current guess, solves one least-squares system for the best small twist (rotation w + translation d), applies it, and re-pairs the points. Five or six iterations and the rms bottoms out — that final transform is the robot's motion between the two scans, the raw material of LiDAR odometry.
Play with a registration below: two clouds start offset, and each Step finds correspondences and pulls them into point-to-plane alignment while the residual readout drops toward zero.
The reference cloud (a corner of two walls) is fixed. The new cloud starts offset and rotated. Press Step to run one point-to-plane ICP iteration — correspondences (thin lines) form, the cloud snaps onto the surfaces, and the residual falls. Auto runs to convergence; the slider sets the starting offset.
We can now de-skew a scan and register it to recover motion. But a robot doesn't take one step — it takes thousands, accumulating odometry that drifts, occasionally getting a GPS fix, occasionally recognizing a place it has been before. How do we fuse all of these heterogeneous measurements into one consistent estimate of the entire trajectory? The answer, and the conceptual heart of this lesson, is the factor graph. It is the representation that unifies everything in this series — including, as we'll prove, the Kalman filter you already know.
A factor graph is a picture of an estimation problem with two kinds of nodes:
The graph is bipartite: factors only connect to variables, never to each other. An odometry measurement (the robot moved forward 1 m) becomes a factor connecting two consecutive pose variables. A GPS fix becomes a factor connecting one pose to a fixed global anchor. A loop closure (the robot recognizes it's back at an earlier place) becomes a factor connecting two non-consecutive poses, reaching across the graph. An IMU integration becomes a factor connecting two poses (and their biases/velocities). Every sensor reading is a factor; the trajectory is the variables.
residual² / σ² — the registration error, the GPS gap, the IMU mismatch, each weighted by how noisy that sensor is. A factor graph is, quite literally, a sum of weighted squared residuals waiting to be minimized.The whole point of building the graph is to solve it. "Solving" means finding the assignment of all the variables (the whole trajectory) that is most probable given all the measurements — the maximum a posteriori (MAP) estimate. Since each factor contributes a squared residual and probabilities multiply (so log-probabilities add), MAP estimation becomes a single grand least-squares problem:
where x is the stacked vector of all variables (every pose), each ri(x) is the residual of factor i (how badly the current trajectory violates measurement i), and σi² is that measurement's noise variance (precise sensors get a small σ, so their factors weigh heavily — the inverse-variance weighting from Lesson 1, now operating on the whole graph at once). The trajectory that minimizes this sum is the one that best reconciles all the sensors simultaneously. We solve it with Gauss-Newton (or Levenberg-Marquardt): linearize each residual, solve one big linear system for the best correction, update, repeat until it stops improving — exactly the inner loop of the ICP step from Chapter 2, now applied to the entire trajectory.
Let's solve the smallest non-trivial graph completely, in 1-D, to see MAP estimation actually compute. Two pose variables, x0 and x1, and three factors:
The cost is the weighted sum of squared residuals:
Minimize by setting the two partial derivatives to zero. ∂J/∂x0 = 0 gives 2x0 − 2((x1−x0)−10) = 0, i.e. 2x0 − x1 = −10. And ∂J/∂x1 = 0 gives ((x1−x0)−10) + (x1−11)/4 = 0, i.e. −x0 + 1.25 x1 = 12.75. Two linear equations:
Solve. From the first, x1 = 2x0 + 10. Substitute: −x0 + 1.25(2x0 + 10) = 12.75 → −x0 + 2.5x0 + 12.5 = 12.75 → 1.5x0 = 0.25 → x0 = 0.167. Then x1 = 2(0.167) + 10 = 10.33.
Look at what the graph did. The odometry said "moved +10 from the start," the GPS said "you're at 11," and the prior said "start at 0." These three disagree (0+10 = 10, but GPS says 11). The MAP solution, x1 = 10.33, sits between the odometry's 10 and the GPS's 11 — closer to 10 because the GPS factor is four times noisier (σ² = 4 vs 1), so it pulls more gently. The prior and odometry also nudged x0 slightly off zero to 0.167. Every measurement got a vote weighted by its precision, and the graph found the single trajectory that compromises among all of them optimally. That is MAP estimation, and it scales from these two poses to a million-pose lifelong map by exactly the same recipe.
If the Kalman filter already solves the chain, why bother with the full graph? Because real robotics breaks the chain. A loop closure — "I've been here before" — is a factor connecting two poses that are far apart in time but close in space; it makes the graph non-chain, with a cycle, and a forward-only filter literally cannot represent it (it already threw away the old pose). The factor graph keeps all the poses as variables, so a loop-closure factor can reach back and tie pose 1000 to pose 5, and re-solving the graph propagates that correction backward through the whole trajectory — un-bending all the accumulated drift at once. That global, retroactive correction is smoothing (as opposed to the filter's forward-only filtering), and it is what the next chapters are built on.
With de-skew, registration, and factor graphs in hand, we can describe the two great families of modern LiDAR-inertial systems. They make opposite engineering bets, and choosing between them is the central practical decision of the field. One is the factor-graph / loosely-coupled approach, exemplified by LIO-SAM; the other is the tightly-coupled iterated filter, exemplified by FAST-LIO2.
Shan and colleagues' LIO-SAM (2020) is the canonical factor-graph LiDAR-inertial system, and it is "loosely coupled" in a specific sense: the LiDAR scans are first processed into relative pose measurements (LiDAR odometry, à la LOAM), and those poses, not the raw points, enter the graph. The graph then fuses four kinds of factors into one pose graph solved incrementally by iSAM2 / GTSAM:
The win of the graph approach is everything Chapter 3 promised: it naturally fuses heterogeneous sensors (just add a factor type), it does smoothing (loop closures retroactively fix the whole trajectory), and it's globally consistent (GPS factors anchor it to the world). The cost is that maintaining and re-solving a graph — even incrementally — is heavier than a filter, and LIO-SAM works on keyframe poses (processed LiDAR odometry) rather than raw points, which is the "loose" coupling.
Xu and colleagues' FAST-LIO / FAST-LIO2 (2021–2022) makes the opposite bet. It is tightly coupled — it fuses the raw LiDAR points directly with the IMU, with no intermediate "LiDAR odometry" step — and it does so not with a graph but with an iterated error-state Kalman filter (IEKF). The IMU drives a high-rate prediction (propagating the state forward through each IMU sample), and every de-skewed LiDAR point is folded in as a point-to-plane measurement update, with the filter iterating the update (re-linearizing several times) to handle the nonlinearity, like a mini Gauss-Newton inside each step.
The obvious worry: a single scan has tens of thousands of points, so the measurement vector is enormous. A textbook Kalman update inverts a matrix the size of the measurement count — that would be hopelessly slow for thousands of points per scan. FAST-LIO's headline contribution is a reformulation of the Kalman gain using a matrix identity (the Sherman-Morrison-Woodbury identity) so the update's cost scales with the state dimension (a couple dozen numbers: pose, velocity, biases) instead of the measurement dimension (thousands of points):
Because the state is tiny and fixed (~18 numbers) while the measurements are many and growing, this flips an impossible computation into a cheap one — thousands of LiDAR points get fused per scan in a couple of milliseconds. The second ingredient is the ikd-Tree (incremental k-d tree): a map data structure that supports inserting new points and deleting far-away ones and nearest-neighbor queries, all incrementally, so the map updates in real time without rebuilding. Together these make FAST-LIO2 the low-latency champion: raw points, tightly fused, at very high rate.
| LIO-SAM (factor graph) | FAST-LIO2 (iterated filter) | |
|---|---|---|
| Estimation engine | factor graph, iSAM2/GTSAM | iterated error-state Kalman filter |
| Coupling | loose (LiDAR odometry → poses) | tight (raw points + IMU) |
| What enters the estimator | relative-pose factors, preintegrated IMU, GPS, loops | every de-skewed point as a point-to-plane update |
| Loop closure / global consistency | native (just add a factor; GPS too) | not built in (it's pure odometry) |
| Smoothing vs filtering | smoothing (revisits the past) | filtering (forward-only) |
| Latency | low, but graph upkeep adds overhead | lowest — gain trick + ikd-Tree |
| Best when | multi-sensor, loop closure, global maps | tightest latency, fewest moving parts |
Chapter 3 said MAP estimation is one big least-squares solve over the whole graph. But a robot that runs for hours accumulates millions of poses — re-solving a million-variable least-squares problem from scratch every time a new scan arrives is hopeless. The thing that makes lifelong smoothing actually real-time is incremental smoothing, and the algorithm is iSAM2 (incremental Smoothing And Mapping) by Kaess and colleagues (2012), the engine inside GTSAM and LIO-SAM.
The insight is that when a new measurement arrives, it does not change the whole trajectory — it only meaningfully affects a small, nearby part of the graph. A new odometry factor between poses 999 and 1000 mostly touches poses near the end; poses 1 through 900 barely move. So instead of re-solving everything, iSAM2 re-solves only the part of the graph that actually changed. The bookkeeping that makes this exact (not approximate) is a clever data structure called the Bayes tree — a tree-shaped factorization of the graph in which adding a measurement requires updating only the branches from the affected variables up to the root, leaving the rest untouched.
Here is where smoothing earns its keep, and it is the most visually dramatic moment in all of SLAM. A robot drives a big loop — down a hall, around a building, back toward where it started. Along the way its LiDAR odometry drifts: small registration errors accumulate, so by the time it returns near the start, its estimated position is off by, say, two meters. Its map is bent; the end of the loop doesn't meet the beginning.
Then it recognizes a place. The current scan matches a scan from the very start of the run — "I've been here before." This recognition (via scan context or a similar place-recognition descriptor) produces a loop-closure factor: a relative-pose constraint saying "pose 1000 and pose 5 are actually almost the same place." Adding that one factor to the graph and re-solving snaps the entire drifted trajectory back into global consistency. The two-meter gap closes, and the correction propagates backward through every pose in the loop, distributing the accumulated error smoothly across the whole path. A filter cannot do this — it threw pose 5 away long ago. Only a graph (or smoother) that kept the past as variables can reach back and fix it.
Watch it happen. Below, a robot drives a loop while its odometry drifts (the estimate peels away from the truth). Press Close loop to add the loop-closure factor — the drifted trajectory snaps back and the correction ripples through the whole path.
A robot drives a loop. Its odometry estimate drifts away from the true loop (dashed) — the ends don't meet. Press Drive to accumulate drift, then Close loop to add a loop-closure factor tying the end pose back to the start. The graph re-solves and the corrected trajectory snaps into a clean closed loop, the error spread evenly along the path.
Let's quantify what "snapping back" does, with a tiny version. A robot drives a loop and, by the end, odometry estimates it returned to (2.0, 0.0) — but a loop closure recognizes it is truly back at the start, (0.0, 0.0). The loop-closure factor says "end ≈ start," so the end has a 2.0 m error to erase. If the loop has, say, 100 poses and the drift accumulated roughly uniformly, the smoother distributes the 2.0 m correction across them — about 2.0 / 100 = 0.02 m per pose — rather than yanking only the last pose (which would put a visible kink there). The result is a trajectory that closes and stays smooth, because the least-squares solve spreads the residual to minimize total squared error. This even distribution is the signature of a correct smoother: a kink at the closure point is a bug (Chapter 9).
We close the technical arc with the LiDAR's most insidious failure, the one that makes the IMU non-negotiable: degeneracy. We met it in Chapter 0 — a long, straight, featureless tunnel where the LiDAR can't tell how far the robot has slid forward. Now we make it precise: how does a system detect that it's in a degenerate scene, and what does it do about it? The answer is one of the most elegant uses of eigenvalues in robotics.
Recall from Chapter 2 that registration solves a least-squares problem: find the transform minimizing the point-to-plane residuals. That solve has, at its core, an information matrix (the AᵀA from the normal equations, sometimes called the Hessian) — a small matrix, one row/column per degree of freedom of the motion, that encodes how strongly the scan constrains each direction of motion. In a feature-rich scene (corners, walls in every orientation), this matrix is well-conditioned: every direction is constrained, the solve is confident.
In a degenerate scene, one direction is unconstrained. In a straight tunnel, the walls constrain sideways and vertical motion strongly, but along the tunnel axis there is nothing — sliding the cloud forward along the axis changes none of the point-to-plane residuals (the walls look identical). Mathematically, the information matrix has a near-zero eigenvalue, and its eigenvector points along the tunnel. That eigenvalue is the system telling you, quantitatively, "I have no information about motion in this direction."
Suppose a registration's information matrix has eigenvalues, in a 2-D (x along the tunnel, y across it) toy case:
The standard test is the condition number (ratio of largest to smallest eigenvalue) or simply a threshold on the smallest eigenvalue. Here λalong = 3 is far below a typical threshold (say 100), and the ratio 240/3 = 80 is huge — both scream "the along-tunnel direction is degenerate." The system now knows, this scan, that it cannot trust LiDAR for forward motion.
The fix is the complementary logic of the whole lesson, now applied surgically. Where the LiDAR is confident (the strong eigen-directions), trust the registration. Where it is degenerate (the weak eigen-direction), ignore the LiDAR and fall back on the IMU (or wheel odometry, or any other source) for that direction only. Concretely, you can project out the degenerate direction from the LiDAR update — let the laser correct sideways and vertical motion, but let the IMU carry forward motion through the tunnel until features reappear. This is sometimes called solution remapping (Zhang et al.) and it is precisely why a tightly-coupled IMU rescues LiDAR where a lone LiDAR drifts hopelessly down the axis.
And it connects straight back to Lesson 1's inverse-variance weighting: a degenerate direction is one where the LiDAR's effective variance has shot to infinity (zero information = infinite variance), so the optimal weight on the LiDAR in that direction slides to zero and the IMU takes over — automatically, if you've set up the information matrix correctly. The eigenvalue test is just the multi-dimensional version of "GPS variance → ∞ in the tunnel" from the very first lesson.
See it below. A robot drives down a tunnel; the registration's eigenvalues are shown as two bars. As it enters the tunnel, the along-axis eigenvalue collapses, the LiDAR-only estimate drifts down the axis, and the IMU-aided estimate stays locked to the truth.
A robot drives left to right; the middle stretch is a featureless tunnel. The bars show the registration's across-axis and along-axis eigenvalues. In the tunnel the along-axis eigenvalue collapses (geometric blindness). The LiDAR-only estimate drifts along the axis; the IMU-aided estimate detects the collapse and leans on the IMU, staying on truth.
Every lesson in this series ends with the same four chapters — Use Cases, Practical Application, Debugging, Connections — because a technique you can't point at in a shipped system is one you don't really own. LiDAR-inertial fusion and factor graphs are not academic curiosities; they are the localization-and-mapping backbone of an entire generation of robots. Here is where they run, and the open-source stacks that power them.
Autonomous driving & HD mapping. The high-definition maps that self-driving cars localize against are built by survey vehicles running LiDAR-inertial SLAM — a LiDAR (often a 64- or 128-beam spinning unit) fused with a high-grade IMU and GPS in a factor graph, producing centimeter-accurate maps over entire cities. At runtime the car localizes against that map by registering its live de-skewed scans, with the IMU bridging and de-skewing exactly as in this lesson.
Mobile robots & warehouses. Delivery robots, patrol robots, and warehouse AMRs use LiDAR-inertial odometry (frequently FAST-LIO2 or LIO-SAM directly) to navigate GPS-denied indoor spaces. The IMU is what lets them turn fast corners without smearing the map, and loop closure keeps multi-hour shifts globally consistent.
Drones & aerial mapping. A drone carrying a lightweight LiDAR + IMU can map a forest canopy, a quarry, or the inside of a building — FAST-LIO2's low latency is prized here because a fast-moving, agile drone demands tight, high-rate state estimation that a heavier graph might lag on.
Legged robots. Quadrupeds (Spot, ANYmal) and humanoids fuse LiDAR + IMU + leg odometry. The IMU is doubly essential here — not only for de-skew but because a walking robot's body lurches violently with each step, smearing scans badly; without aggressive de-skew the map is unusable.
Surveying & construction. Handheld and backpack LiDAR scanners (think GeoSLAM, NavVis, and the LiDAR mode in modern phones/tablets) build as-built models of buildings and sites. They are LiDAR-inertial SLAM in a box: walk through a building and the de-skewed, loop-closed factor graph hands you a millimeter-grade point-cloud model.
Search-and-rescue & subterranean. The DARPA Subterranean Challenge was won by LiDAR-inertial systems precisely because tunnels and mines are GPS-denied and degenerate — the exact conditions where IMU-aided degeneracy handling (Chapter 6) is the difference between a map and a lost robot.
| Stack | What it is | Family |
|---|---|---|
| LOAM / A-LOAM | the original LiDAR odometry; edge+planar features (Ch 2) | LiDAR-only odometry |
| LIO-SAM | factor-graph LiDAR-inertial w/ GPS + loop closure | graph (loosely coupled) |
| FAST-LIO / FAST-LIO2 | tight iterated-filter LiDAR-inertial, ikd-Tree | filter (tightly coupled) |
| GTSAM / iSAM2 | the factor-graph + incremental-smoothing library | solver backend |
| Ceres | general nonlinear least-squares solver (Google) | solver backend |
Notice the layering. LOAM gave the world LiDAR odometry. LIO-SAM and FAST-LIO2 are the two LiDAR-inertial systems built on top — one graph, one filter — the exact pair from Chapter 4. Underneath the graph systems sits a solver backend, almost always GTSAM (which contains iSAM2) or Ceres, the same factor-graph machinery used for visual SLAM, bundle adjustment, and calibration. Learn the factor graph once and you have the key to all of them.
You now know de-skew, registration, factor graphs, the two architectures, smoothing, and degeneracy. This "now build it" chapter turns that into the decisions a real deployment forces on you — which architecture to pick, how to tune feature extraction and loop closure, when to add GPS, and how to fit it all in a compute budget.
This is the first fork, and it follows directly from Chapter 4. Run these questions:
The rule of thumb: FAST-LIO2 for the tightest, fastest pure odometry (a racing drone, a minimal stack); LIO-SAM for anything needing global consistency or multi-sensor fusion (a survey vehicle, a multi-hour warehouse shift, a robot with GPS). Many real systems run FAST-LIO-style odometry as a front-end and a factor graph as a back-end for loop closure — the best of both.
LOAM-style feature extraction (Ch 2) has knobs that matter:
You don't add a graph node for every single scan — that's wasteful. You add a keyframe only when the robot has moved enough (e.g. >1 m or >10° since the last keyframe). Keyframes keep the graph sparse while still capturing the trajectory. For loop closure, the workhorse is scan context: a compact rotation-invariant descriptor of a scan that lets you quickly ask "have I seen a scan like this before?" against all past keyframes. When a candidate match is found, you verify it with ICP (Ch 2) before trusting it — because a false loop closure is catastrophic (Ch 9). Tune the matching threshold conservatively: a missed loop closure costs you some drift; a false one corrupts the whole map.
Outdoors, even with loop closure, a LiDAR-inertial system slowly drifts in the global frame (its map is internally consistent but may be rotated/translated relative to the Earth). Adding GPS factors — absolute-position constraints on occasional keyframes — pins the trajectory to real-world coordinates and bounds global drift, exactly the complementary role GPS played in Lesson 1, now as one more factor in the graph. Weight them by GPS quality (use the reported covariance); de-weight or drop them in urban canyons where multipath makes GPS lie.
Two structures dominate the budget. The map grows without bound if you keep every point — FAST-LIO2's ikd-Tree handles this by incrementally deleting points far from the robot, keeping a bounded local map. The graph grows with keyframes — iSAM2 keeps re-solving cheap, but for truly lifelong operation you eventually marginalize (summarize and drop) very old parts of the graph. Budget rule of thumb: a few hundred features per scan, voxel-downsampled clouds, keyframes every meter, a bounded local map — and you fit comfortably on an embedded CPU at sensor rate.
LiDAR-inertial systems fail in characteristic ways, and most failures are quiet — a slowly bending map, a map that suddenly tears in half — rather than loud crashes. This recurring chapter catalogs the classic failure modes, the symptom each shows, and the specific test that catches it before it ruins a map.
If de-skew is off, mis-timed, or fed a bad IMU, every scan is captured smeared and the accumulated map looks fattened, doubled, or curved — straight walls become thick blurs, especially during fast turns.
In a tunnel, long corridor, open field, or featureless room, the registration is unconstrained along one direction (Ch 6) and the estimate drifts along that direction — the robot thinks it's standing still while it slides forward, or vice versa.
A factor graph needs the IMU's initial bias and the initial gravity direction to be roughly right, or preintegration is garbage from the first step. A robot that starts moving before the IMU is initialized, or that mis-estimates gravity, produces a trajectory that tilts or curves immediately.
The most catastrophic failure. A place-recognition descriptor wrongly decides "I've been here before" — two different corridors that happen to look alike — and adds a loop-closure factor between poses that are not the same place. Re-solving then violently warps the map to satisfy the false constraint, often tearing it in two.
The IMU and LiDAR are rigidly mounted apart, with a fixed transform (the extrinsic) and ideally a shared clock. If the extrinsic calibration is wrong or the clocks are offset (Lesson 15's territory), de-skew applies the IMU's motion at the wrong place or the wrong time, smearing or warping subtly even at low speed.
Even with everything working, pure LiDAR-inertial odometry drifts slowly — without loop closures or GPS, a long traverse ends up off by a fraction of its length. This is expected, not a bug, but it becomes a problem if you assumed odometry alone would stay globally accurate.
This lesson is the conceptual high point of the series, because the factor graph is the frame that contains everything else. Let's place it among its neighbors, gather the references, and leave you a cheat-sheet.
Step back and the whole series collapses into one picture. Every estimator you've met — the Bayes filter, the Kalman filter, the EKF/UKF, LiDAR-inertial SLAM, visual-inertial SLAM — is a factor graph being solved under different assumptions:
| Method | Graph shape | How it's solved |
|---|---|---|
| Kalman filter | a chain | forward-only, Gaussian summary of the past |
| EKF / iterated EKF | a chain, nonlinear | forward-only, re-linearized (FAST-LIO2) |
| Pose-graph SLAM | chain + loop-closure edges | batch or incremental smoothing |
| Full LiDAR-inertial SLAM | poses + biases + GPS + loops | iSAM2 incremental (LIO-SAM) |
Learn the factor graph and you have not learned one more method — you've learned the language the rest are dialects of. That is why Dellaert and Kaess titled their book Factor Graphs for Robot Perception: not "a technique," but the perception substrate itself.
| Concept | One-line takeaway |
|---|---|
| De-skew | use the high-rate IMU to transform each point by its own timestamp's pose into one frame — un-bend the smeared sweep |
| Point-to-plane ICP | minimize r = n·(p′−q): perpendicular distance into the surface, ignoring harmless sliding along it |
| LOAM features | extract a few hundred edge (high-curvature) + planar (low-curvature) points; register only those |
| Factor graph | variables (poses) + factors (measurements = squared residuals); MAP = minimize the sum of all squared residuals |
| Kalman = chain graph | a Kalman filter is a chain-structured factor graph solved forward-only |
| LIO-SAM | factor graph: LiDAR-odom + preintegrated-IMU + GPS + loop factors, smoothed by iSAM2 — global consistency |
| FAST-LIO2 | tight iterated Kalman filter on raw points; gain reformulation + ikd-Tree — lowest latency |
| IMU preintegration | sum many IMU samples into one reusable relative-motion factor, independent of initial pose/bias |
| iSAM2 / Bayes tree | re-solve only the sub-graph a new measurement affects — lifelong smoothing in real time |
| Loop closure | one factor tying present to a recognized past pose; re-solving propagates the correction backward, erasing drift |
| Degeneracy | near-zero eigenvalue of the registration information matrix = an unconstrained direction; lean on the IMU there |
"What I cannot create, I do not understand." — Richard Feynman. You have now seen, derived, and simulated every piece of a LiDAR-inertial SLAM system; the factor graph is the single sentence that holds them together, and you can write it.