How a Robot Senses the World: LiDAR, IMU & ICP

Chapter 1: The Sensor Zoo — Two Axes That Organize Everything

A robot might carry a dozen different sensors, and at first they seem like an unrelated grab-bag: encoders, gyros, lasers, cameras, GPS. But Stanford organizes the whole ecosystem along just two simple axes, and once you have them, every sensor you ever meet snaps into place.

Axis 1: what does it measure — me, or the world?

The first question to ask any sensor is whose business it is measuring: the robot's own internals, or the environment outside.

• Proprioceptive sensors measure values internal to the robot — motor speed, wheel rotation, joint angles, battery voltage, the robot's own acceleration and rotation rate. (Think of proprioception in your own body: with eyes closed you still know where your hand is.) Encoders and the IMU are proprioceptive.
• Exteroceptive sensors acquire information from the environment — distance to a wall, light intensity, sound amplitude, the geometry of a room. LiDAR, cameras, sonar, and a compass are exteroceptive.

Axis 2: does it listen, or does it shout?

The second axis is about energy: does the sensor passively soak up energy already in the environment, or does it emit its own and watch the echo?

• Passive sensors measure ambient energy entering the sensor — a camera collects light that was already there; a thermometer reads heat that already exists. Challenge: performance depends heavily on the environment (a camera is useless in the dark).
• Active sensors emit energy into the environment and measure the reaction — LiDAR fires a laser pulse and times the echo; sonar chirps and listens. Challenge: the emitted energy might disturb the environment, or two robots' active sensors might interfere with each other.

Put the two axes together and you get a 2×2 grid that classifies essentially every robot sensor. Tap a cell below to drop a sensor into it and see why it lands there.

The universal error model

No matter which cell a sensor lives in, we model its output the same way. If v is the true value and m is what the sensor reports, the error is simply their difference:

and we split that error into two pieces with very different personalities:

Here b is the systematic error (bias) — a deterministic offset that can in principle be modeled and calibrated away (a miscalibrated encoder always over-counts by the same factor). And ε is the random error (noise) — a stochastic jitter we cannot predict, only characterize with a probability distribution. The notation ε ~ N(0, σ²) reads "ε is drawn from a Gaussian (normal) distribution with mean zero and variance σ-squared" — the standard, convenient assumption that noise is symmetric and bell-shaped around zero. (Stanford warns this is "often a coarse simplification" — a sonar bouncing off a smooth wall produces a one-sided, non-Gaussian error — but it is the workhorse model and the one every filter in Lessons 13–14 assumes.)

Quantifying accuracy and precision — with numbers

These aren't fuzzy words; Stanford gives them formulas. For a true value v and measurement m, accuracy is how close the reading is to truth, expressed as a fraction:

A rangefinder reads m = 2.05 m at a true v = 2.00 m, so its accuracy is 1 − |2.05 − 2.00|/2.00 = 1 − 0.025 = 0.975, or 97.5%. Now fire that same sensor ten times and get {2.04, 2.06, 2.05, 2.05, 2.04, 2.06, 2.05, 2.04, 2.06, 2.05}. The spread is tiny (range 0.02 m) — high precision — but they all cluster around 2.05, not 2.00, so there's a systematic +0.05 m bias dragging accuracy below 100%. Precise but not perfectly accurate. (Note also that accuracy needs the true value v, which is exactly what you usually don't have on a real robot — making accuracy hard to measure in the field, while precision you can always check by repeating.)

The datasheet vocabulary

One more vocabulary set you'll see in every sensor datasheet. Dynamic range — the ratio of largest to smallest input the sensor handles, often in decibels, DR = 10·log₁₀(ratio). Resolution — the smallest difference it can distinguish (an 8000-count encoder resolves about 0.045° of wheel rotation). Bandwidth — readings per second, in Hertz; this one bites hard, because a robot's top safe speed is capped by how fast its obstacle sensor reports (a 10 Hz LiDAR on a fast robot leaves big blind gaps between scans). Linearity — whether output tracks input proportionally. And cross-sensitivity — responding to the wrong thing, like a magnetic compass that twitches near a steel beam, often making the sensor useless indoors.

Chapter 2: Wheel Encoders & Odometry — Counting Your Way Home

The cheapest, oldest way for a wheeled robot to estimate where it is: count how far its wheels have turned. That counting device is a wheel encoder, and the act of integrating those counts into a position estimate is odometry — literally, "road measuring."

How an optical encoder counts

An encoder is an electro-mechanical device that converts motion into a sequence of digital pulses. The classic optical encoder shines a light through slits cut into a disc fixed to the wheel's axle; as the wheel turns, the slits chop the light into a train of pulses falling on a photodiode. Count the pulses and you know how far the disc — and so the wheel — has rotated. It is proprioceptive (it watches the robot's own wheel) and its reading lives in the robot's frame.

Resolution is quoted in counts per revolution (CPR). A typical mobile-robot encoder has 2000 CPR. A clever trick called quadrature — adding a second detector offset by 90° — lets you (a) tell direction from which pulse rises first and (b) quadruple the resolution for free, so 2000 CPR becomes 8000 counts.

From counts to distance — with actual numbers

Let's convert encoder ticks to travelled distance by hand. Suppose:

• wheel radius R = 0.05 m (a 10 cm-diameter wheel),
• encoder resolution N = 8000 counts/revolution (2000 CPR in quadrature),
• and in one control step the encoder reports Δc = 400 counts.

One full revolution rolls the wheel forward by its circumference, 2πR. So the distance per count is the circumference divided by counts-per-revolution:

And the distance travelled this step is just counts times distance-per-count:

For a two-wheeled differential-drive robot you do this for both wheels, getting left and right travel Δs_L and Δs_R. The robot's forward step and turn over that interval are the average and the difference, scaled by the wheelbase L (axle width):

and you fold those into the pose by dead reckoning — adding each step to the running estimate of (x, y, θ):

This is dead reckoning: starting from a known pose and adding up many small motion increments to track where you are now. (The name is nautical — sailors "deduced" position from speed and heading over time. "Ded." for "deduced," later "dead.")

From scratch in Python

python
import numpy as np

def counts_to_dist(dc, R=0.05, N=8000):
    # one revolution = circumference 2*pi*R; spread over N counts
    return dc * (2 * np.pi * R / N)

def odometry_step(pose, dcL, dcR, L=0.3):
    # pose = [x, y, theta]; dcL,dcR = left/right encoder counts this step
    dsL, dsR = counts_to_dist(dcL), counts_to_dist(dcR)
    ds  = (dsR + dsL) / 2.0          # forward travel
    dth = (dsR - dsL) / L                # heading change
    x, y, th = pose
    x  += ds * np.cos(th)
    y  += ds * np.sin(th)
    th += dth
    return np.array([x, y, th])

Why odometry drifts — the fatal flaw

Here is the catch, and it is a deep one. Odometry has no external reference. It only ever adds increments to its own previous estimate. So every error gets baked into the pose permanently and the next step builds on top of it. A wheel that slips on a wet floor, a tire that is 1% smaller than assumed, a tiny miscount — each adds a sliver of error that never gets corrected, only carried forward and amplified.

Make the accumulation concrete. Suppose the right wheel's encoder consistently reads 1% high (a slightly-wrong assumed radius — a pure systematic error). The robot drives what it believes are five straight 1.00 m segments. Because the right wheel "travels" a hair farther than the left each step, the robot actually curves slightly left while believing it went straight — and the heading error compounds:

The robot's belief is a perfectly straight line down the x-axis. Its true path is a gentle leftward arc, and after just 5 m it is already 13 cm off to the side and pointing 3° wrong — and it has no idea, because the only thing it can check is its own encoders, which are telling a consistent lie. Extend this to 100 m and the cross-track error is meters. The error grows because heading error feeds position error, which the next step inherits.

Drive the robot below and watch the gap grow. The teal robot follows the true path; the orange robot trusts only its encoders, which slip a little each step. Add wheel slip and the orange estimate peels away from the truth — and once it has drifted, it stays drifted, because there is nothing to pull it back.

Chapter 3: The IMU — Fast, Self-Contained, and Quietly Drifting

An Inertial Measurement Unit (IMU) is the little chip that knows you've rotated your phone. On a robot it is the workhorse of fast motion sensing — it can report hundreds or thousands of times a second, in the robot's own frame, needing nothing from the outside world. That speed and self-sufficiency make it indispensable. Its Achilles' heel is the same one we just met: it integrates, so it drifts.

An IMU bundles two kinds of sensor, both proprioceptive:

The accelerometer

An accelerometer measures the net external force per unit mass acting on it — that is, its acceleration (including gravity). The classic mechanical model is a tiny spring-mass-damper: a small "proof mass" on a spring inside the chip. When the chip accelerates, inertia lags the proof mass behind, stretching the spring; the deflection is proportional to acceleration. In steady state the applied force balances the spring force:

where m is the proof mass, c the damping, k the spring constant, and x the proof mass's displacement. (The dots are time derivatives: ẋ is velocity, ẍ is acceleration.) Real chips are MEMS — micro-machined cantilever beams whose deflection is read out as a tiny change in capacitance.

The gyroscope

A gyroscope measures angular velocity — how fast the robot is rotating, in radians (or degrees) per second. A mechanical gyro exploits the angular momentum of a fast-spinning rotor to keep its axis inertially stable; modern MEMS gyros use vibrating structures and the Coriolis effect. The key point for us: a gyro gives you a rate of turn, not an angle. To get an angle, you must integrate.

Integrating a biased gyro — watch drift grow, with numbers

Here is the single most important worked example in this chapter. Suppose a robot is sitting perfectly still — true angular velocity is zero, so the heading should never change. But the gyro has a tiny constant bias of b = 0.5 °/s (a very typical low-cost MEMS gyro figure). The robot doesn't know about the bias, so it does the obvious thing: it integrates the reading to update its heading estimate, θ:

Because the bias is constant, the heading error grows linearly with time. Let's tabulate it for b = 0.5 °/s while the robot sits motionless:

Read that last row again. After six motionless minutes, a 0.5°/s bias — far too small to notice in a single reading — has the robot convinced it has spun a half-turn. And it gets worse downstream: a wrong heading means gravity is subtracted in the wrong direction, leaking a phantom horizontal acceleration that integrates twice into position — so position error grows like t-squared. This is the drift Stanford flags as "a fundamental problem" with IMUs: to cancel drift, periodic references to external measurements are required.

From scratch in Python

python
import numpy as np

def integrate_gyro(gyro_readings, dt, bias=0.0):
    # gyro_readings: angular rates (rad/s). Robot is actually still,
    # but every reading carries a constant bias.
    theta = 0.0
    history = []
    for w in gyro_readings:
        theta += (w + bias) * dt    # integrate rate -> angle; bias leaks in
        history.append(theta)
    return np.array(history)

# still robot, 0.5 deg/s bias, sampled at 100 Hz for 60 s
dt   = 0.01
bias = np.deg2rad(0.5)
true = np.zeros(6000)               # truly no rotation
est  = integrate_gyro(true, dt, bias)
print(np.rad2deg(est[-1]))         # -> ~30 degrees of pure drift

Run the simulation below: the teal arrow is the robot's true heading (stationary, or turning if you drive it); the orange arrow is what the gyro believes, integrated with whatever bias you dial in. Even at a tiny bias, leave it running and watch the orange arrow creep away from teal — relentlessly, in one direction, forever.

Chapter 4: LiDAR — Painting the Room With Light

If the IMU is the robot's inner ear, LiDAR (Light Detection And Ranging) is its eyes for geometry. It is the absolute, exteroceptive reference that the drifting proprioceptive sensors so badly need. Where the IMU says "I think I turned," LiDAR says "there is a wall, exactly two meters away, at this bearing." It measures the world directly.

Time-of-flight ranging

LiDAR is an active, time-of-flight sensor. It fires a brief laser pulse, the pulse hits a surface and bounces back, and the sensor times the round trip. Since light travels at a known speed c, the distance follows from the simplest physics there is — speed times time — halved because the light made a round trip:

Light moves at about 0.3 m per nanosecond (c ≈ 3×10⁸ m/s). So a wall 3 m away returns its echo in a round trip of t = 2·3/c = 6/(3×10⁸) = 2×10⁻⁸ s = 20 nanoseconds. Sound, used by sonar, crawls at 0.3 m per millisecond — a million times slower — so the same 3 m takes 20 milliseconds for sonar. That million-fold gap is why timing a laser is so technologically hard (you must resolve nanoseconds) and why affordable, robust LiDAR is a relatively recent arrival. The whole engineering challenge is measuring that tiny t precisely; a 1 ns timing error becomes a 0.15 m range error.

Range quality degrades for a handful of physical reasons worth knowing: uncertainty in detecting the exact arrival time of the returning pulse; the beam's dispersal cone (it widens with distance, so far surfaces are sampled coarsely); interaction with the target (a black or mirrored surface absorbs or deflects the pulse, returning nothing); and relative motion of robot and target during the measurement. These shape the noise on ρ — the random part of our m = v + bias + noise model.

A scan is a set of (range, bearing) pairs

A 2D LiDAR doesn't fire one beam — it sweeps, firing equally-spaced beams in a full 360° circle (or a wide sector). Each beam i comes back as a range ρ_i (how far the echo travelled) paired with the bearing θ_i (the angle that beam was pointed). A whole sweep is a scan: a list of polar coordinates.

Polar to Cartesian — the conversion, by hand

Polar coordinates are how the sensor speaks, but maps and geometry live in Cartesian (x, y). The conversion is just basic trigonometry — a point at distance ρ and angle θ from the sensor sits at:

Let's do one concretely. Beam 17 reports ρ = 2.50 m at bearing θ = 30°. Convert:

So that echo came from the point (2.165, 1.250) in the robot's frame — 2.165 m ahead and 1.25 m to the left. Do this for all n beams and the scan becomes a cloud of (x, y) points tracing the walls and furniture the robot can see — a point cloud, the subject of Chapter 6.

From scratch in Python

python
import numpy as np

def scan_to_points(rho, theta):
    # rho: ranges (m), theta: bearings (rad). Returns Nx2 array of (x,y).
    x = rho * np.cos(theta)
    y = rho * np.sin(theta)
    return np.stack([x, y], axis=1)

# a tiny 4-beam scan, bearings 0, 30, 60, 90 degrees
theta = np.deg2rad([0, 30, 60, 90])
rho   = np.array([3.0, 2.5, 2.2, 2.0])
pts   = scan_to_points(rho, theta)
print(pts)   # [[3.0, 0.0], [2.165, 1.25], [1.1, 1.905], [0.0, 2.0]]

What LiDAR can't see: occlusion and max range

LiDAR is not magic. Two hard limits shape every scan. Occlusion: a beam stops at the first surface it hits, so anything behind that surface is invisible — the robot sees a "shadow" with no data. Max range: beyond some distance the returning pulse is too weak to detect, so far-away beams simply report "no return" (often a sentinel like the max range value). A scan is therefore always partial — it captures only the surfaces facing the sensor, within range, unoccluded.

Drive the robot around the room below and watch its LiDAR sweep. Each beam shoots out until it hits a wall (or runs out of range); the hit points accumulate as a point cloud. Notice the shadows behind obstacles — the robot is genuinely blind there — and the blank arc where walls exceed max range.

Chapter 5: Cameras & RGB-D — The Richest, Cheapest, Hardest Sensor

A camera is the most information-dense sensor a robot can carry — the human eye delivers millions of bits per second — and it is dirt cheap. It is also a passive, exteroceptive sensor: it collects whatever ambient light the world reflects, which makes it gloriously rich and frustratingly dependent on conditions. In the dark, in glare, against a blank white wall, a camera goes blind in ways a LiDAR does not.

We keep this chapter deliberately short, because Lesson 8 is devoted entirely to the camera model. Here we just place the camera on our two-axis map and name what it gives us.

How a camera captures the world: the pinhole idea

Point a bare photoreceptive surface at the world and every point on it receives light from every direction — the image is a uniform blur. The trick, thousands of years old and first clearly described by Leonardo, is to put a barrier with a tiny hole (a pinhole, or aperture) in front of the surface. Now each point on the surface receives light from essentially one direction, and a sharp (if inverted) image forms. That is the pinhole camera model, and it is the geometric heart of every camera.

The pinhole creates perspective projection: a 3D world point P projects to a 2D image point p, with farther things appearing smaller. By similar triangles, a point at depth Z and height Y projects to image height:

where f is the focal length (the pinhole-to-image distance). The depth Z sits in the denominator — that single fact is the whole story of perspective. Double an object's distance and it halves in the image; that's why far things look small, and why parallel rails appear to meet. Let's see it numerically: a person 1.8 m tall (Y = 1.8) photographed with f = 0.025 m (a 25 mm lens) projects to y = 0.025·1.8/Z. At Z = 5 m, y = 0.009 m (9 mm on the sensor); at Z = 10 m, y = 0.0045 m — exactly half, because depth doubled. Notice what is lost: the same 9 mm image height could come from a 1.8 m person at 5 m or a 3.6 m giant at 10 m. Dividing by Z destroys the scale — the camera cannot, from one image, tell which.

Lesson 8 turns this one relationship into the full camera matrix that maps world points all the way to pixel coordinates (u, v) — adding the conversion from metric image coordinates to pixels (via pixels-per-meter scale factors), the image-center offset, and the world-to-camera transform. It also covers calibration: recovering those internal parameters (focal length, center, distortion) so the pixels mean something metric. We flag it here so you know where the camera fits: it is a perception front-end whose geometry must be calibrated before its pixels carry distance information.

RGB-D: bolting depth onto color

A plain camera loses depth — the projection from 3D to 2D throws away how far each pixel was. An RGB-D camera adds a depth channel: alongside the usual red-green-blue value, every pixel carries an estimate of its distance from the camera. Most RGB-D sensors get that depth with an active trick — structured light (projecting a known dot pattern and reading how it deforms on surfaces) or a small built-in time-of-flight ranger. The famous Kinect-style "real-time human pose from a single depth image" demos are built on exactly this.

So RGB-D is a hybrid: passive color plus an active depth channel. Its output — a grid of pixels, each with (R, G, B, depth) — can be unprojected into a dense colored point cloud, which connects it straight to the next chapter.

Chapter 6: Point Clouds — Geometry as a Bag of Points

LiDAR and RGB-D both hand the robot the same kind of object: a point cloud — a set of points in space, each just a coordinate. It is the most honest representation of "what's out there" because it makes no assumptions: no surfaces, no objects, no labels, just measured locations. In 2D a point cloud is an N×2 array of (x, y); in 3D it's N×3.

That simplicity is also its burden. A single LiDAR sweep can return tens of thousands of points; a 3D scan, millions. And the points are unordered — there is no "point 5 is next to point 6" guarantee. Two operations tame this, and both reappear at the heart of ICP, so meet them now.

Downsampling: keep the shape, drop the bulk

Most of those points are redundant — a flat wall doesn't need a thousand samples. Downsampling keeps the geometry while slashing the count, so downstream algorithms run fast. The standard trick is the voxel grid filter: chop space into a grid of small cubes (voxels), and replace all the points inside each cube with a single representative (their centroid). A 0.05 m voxel grid turns a dense wall into a tidy lattice of one point every 5 cm — same shape, a fraction of the data.

The payoff is dramatic and worth quantifying, because the cost of the next stage depends on it. Say a 10×10 m room scan returns 20,000 points, and you apply a 0.1 m voxel grid. The room is 100 m², so there are at most 100/(0.1×0.1) = 10,000 voxels — but most are empty (interior, occluded). In practice the wall outline occupies only the boundary cells, perhaps ~400 of them, so you drop from 20,000 points to ~400: a 50× reduction. Since ICP's correspondence step costs roughly O(N×M) per iteration (every moving point searched against every model point), cutting N and M by 50× each cuts the per-iteration cost by ~2500×. Downsampling isn't tidying — it's what makes real-time scan matching possible.

python
import numpy as np

def voxel_downsample(pts, voxel=0.05):
    # pts: Nx2 (or Nx3). Snap each point to a voxel index, then
    # average the points sharing each voxel into one representative.
    keys = np.floor(pts / voxel).astype(int)          # which cell each point falls in
    uniq, inv = np.unique(keys, axis=0, return_inverse=True)
    out = np.zeros((len(uniq), pts.shape[1]))
    counts = np.zeros(len(uniq))
    for i, g in enumerate(inv):
        out[g] += pts[i]; counts[g] += 1          # sum points per cell
    return out / counts[:, None]                    # centroid of each cell

Nearest neighbor: the question every alignment asks

The second core operation: given a query point, which point in the other cloud is closest to it? This nearest-neighbor query is how a robot decides "this point I just measured probably corresponds to that point in my map." Naively you compare the query against all N points and keep the minimum distance — O(N) per query. Smarter structures (a k-d tree) get it down to roughly O(log N), which matters when you run the query for every one of thousands of points, every iteration of ICP.

python
def nearest_neighbor(q, cloud):
    # q: a 2-vector; cloud: Nx2. Return index + distance of closest point.
    d2 = ((cloud - q) ** 2).sum(axis=1)   # squared distances (no sqrt needed to compare)
    j  = int(np.argmin(d2))
    return j, float(np.sqrt(d2[j]))

Explore the operations below. Start with a raw cloud; downsample it (watch the count plummet while the shape survives); then click anywhere to fire a nearest-neighbor query and see which point lights up.

Chapter 7: Line Extraction — From a Cloud of Dots to "There's a Wall Here"

A point cloud is raw geometry, but a robot wants meaning: not "here are 300 dots," but "there is a wall running from here to there." Indoor environments are mostly straight edges — walls, doors, furniture — so the workhorse of 2D LiDAR perception is line extraction: fitting a series of straight segments to the scan. This is exactly Stanford's homework 3, and it is the perception module turning sensing into features.

Describing a line: polar parameters (r, α)

You might describe a line by slope and intercept (y = mx + b), but that blows up for vertical walls (infinite slope). Instead we use the elegant polar line form. Any line in the plane is pinned down by two numbers: the perpendicular distance r ≥ 0 from the origin to the line, and the angle α of that shortest connection. A point (x, y) lies on the line exactly when:

No singularities, every line representable. Our LiDAR points arrive in polar (ρ_i, θ_i); the perpendicular distance from such a point to the candidate line works out to d_i = ρ_i cos(θ_i − α) − r.

Fitting the best line: least squares

Given a clump of points we believe lie on one wall, the best-fit line is the one that minimizes the total squared perpendicular distance from the points to the line. We sum each point's squared distance and search for the (α, r) that makes the sum S smallest:

This is a least-squares problem (we assume Gaussian noise on the range ρ, none on the angle θ). Setting the derivatives of S to zero and solving gives closed-form formulas. The angle α comes first:

and then r is just the average perpendicular projection of the points onto that angle:

Don't memorize the α formula — understand its shape: it is a ratio of weighted sums of the data fed through arctan2, which is exactly what "the angle that best aligns with these points" should look like. The point is that there is a direct, non-iterative answer: feed in the points, get out the best line.

A worked fit, by hand

To build intuition, do the Cartesian version — it's the same idea, simpler arithmetic. Take five points that nearly lie on a horizontal line: (0, 0.1), (1, −0.1), (2, 0.0), (3, 0.1), (4, −0.1). The least-squares horizontal fit y = c is just the mean of the y-values:

So the best line is y = 0.00 — the noise cancelled. Now the perpendicular distances (here just |y_i − c|) are 0.1, 0.1, 0.0, 0.1, 0.1, and the residual sum of squares is S = 0.01+0.01+0+0.01+0.01 = 0.04. A small S means the points really do lie on a line; a large S means they don't, which is the signal we'll use to decide whether to split.

Split-and-Merge: where do the walls begin and end?

Least squares fits one line to a set of points — but a scan has many walls. We need to segment the scan into groups that each belong to one wall. The classic, fast algorithm is Split-and-Merge:

1. Fit a single line to all the points in the current segment.
2. Find the worst point — the one farthest from that line. If its distance exceeds a threshold, the points don't really form one line.
3. Split the segment at that worst point into two halves, and recurse on each half.
4. If no point is far enough (or the segment is too short to split), accept the line.
5. Finally, merge adjacent segments that turned out to be collinear (a wall accidentally cut in two).

From scratch in Python

python
import numpy as np

def fit_line(theta, rho):
    # closed-form least-squares polar line fit -> (alpha, r)
    n = len(rho)
    num = np.sum(rho**2 * np.sin(2*theta)) \
        - (2.0/n) * np.sum(np.outer(rho, rho) * np.outer(np.cos(theta), np.sin(theta)))
    den = np.sum(rho**2 * np.cos(2*theta)) \
        - (1.0/n) * np.sum(np.outer(rho, rho) * np.cos(theta[:, None] + theta[None, :]))
    alpha = 0.5 * np.arctan2(num, den) + np.pi/2
    r = (1.0/n) * np.sum(rho * np.cos(theta - alpha))
    return alpha, r

def find_split(theta, rho, alpha, r, thresh):
    # perpendicular distance of every point to the line; split at the worst
    d = np.abs(rho * np.cos(theta - alpha) - r)
    j = int(np.argmax(d))
    return j if d[j] > thresh else None

def split_lines(theta, rho, a, b, thresh, min_pts):
    alpha, r = fit_line(theta[a:b], rho[a:b])
    if b - a <= min_pts:
        return [(alpha, r, a, b)]
    s = find_split(theta[a:b], rho[a:b], alpha, r, thresh)
    if s is None:
        return [(alpha, r, a, b)]
    return split_lines(theta, rho, a, a+s, thresh, min_pts) \
         + split_lines(theta, rho, a+s, b, thresh, min_pts)

Run line extraction on a noisy scan below. Start with the raw points; press Extract to run Split-and-Merge and watch the algorithm fit lines, flag the worst point, split there, and recurse until clean segments snap onto the walls. Crank the noise and tighten the threshold to see the segment count explode — the classic precision/robustness tradeoff.

Chapter 8: ICP — Snapping Two Scans Together

Here is the problem that makes a robot's drift fixable. A robot took a LiDAR scan a moment ago and is holding a model point cloud of the room. It moves, takes a new scan, and gets a fresh cloud — but it doesn't know exactly how far it moved (odometry drifted!). The two clouds describe the same walls, but the new one is rotated and shifted by the robot's unknown motion. Find the rigid transform — a rotation R and translation t — that best lays the new scan back onto the model. That transform is the robot's true motion, recovered from geometry. Solve it and you have an exteroceptive correction for odometry's drift. This is the heart of scan-matching, and the algorithm is Iterative Closest Point (ICP).

The algorithm, four steps per iteration

1. Correspondence. For each point in the moving cloud, find its nearest neighbor in the model cloud (Chapter 6). These pairs are our current guess at "same physical point."
2. Center both clouds. Compute each cloud's centroid (mean point) and subtract it, so both clouds are centered at the origin. This separates the rotation from the translation — once centered, only rotation remains to solve.
3. Cross-covariance → SVD → rotation. Build a small cross-covariance matrix H from the centered pairs, take its singular value decomposition H = UΣV^⊤, and the optimal rotation is R = VU^⊤.
4. Translation. Recover t so the centroids line up: t = centroid_model − R·centroid_moving.

Apply (R, t) to the moving cloud and loop. Let's define the matrices, then grind one full iteration by hand with tiny numbers so nothing is mysterious.

The math

Let the model points be q_i and the (corresponded) moving points be p_i, for i = 1…n. Centroids:

Centered points: p'_i = p_i − p̄ and q'_i = q_i − q̄. The cross-covariance matrix is the sum of outer products (here a 2×2 matrix in 2D):

Take the SVD H = UΣV^⊤. The optimal rotation that aligns p onto q is:

and the translation drops out from the centroids:

One full iteration, by hand

Take three model points and three moving points that are the model rotated +90° about the origin (so the true answer is R = a 90° rotation, t = 0). We'll recover that from scratch. Assume the correspondences are already paired by index (in practice step 1 finds them):

Step 2a — centroids. Average each cloud's coordinates:

Step 2b — center both clouds (subtract the centroid from every point):

Step 3a — cross-covariance H = ∑ p'_i (q'_i)^⊤. Each term is a 2×2 outer product. The four entries accumulate as:

Step 3b — SVD and R = VU^⊤. This H is (up to scale) the matrix [0.5, 1; −1, −0.5], whose SVD factors are clean orthogonal rotations. Working it through, U^⊤ and V combine to give:

Check det(R) = (0)(0) − (−1)(1) = +1 — a proper rotation, not a reflection. And that matrix is exactly a +90° rotation (it sends (1,0)→(0,1)). We recovered the true rotation from raw points.

Step 4 — translation.

So R is +90° and t = 0 — precisely the transform we baked in. One iteration nailed it because the data was a perfect rigid copy; on noisy real scans, each iteration shrinks the error and you loop until it stops improving.

From scratch in Python

python
import numpy as np

def best_fit_transform(P, Q):
    # P, Q: Nx2 corresponded clouds. Return R (2x2), t (2,) aligning P onto Q.
    pbar, qbar = P.mean(0), Q.mean(0)       # centroids
    Pc, Qc = P - pbar, Q - qbar               # center both clouds
    H = Pc.T @ Qc                             # 2x2 cross-covariance
    U, S, Vt = np.linalg.svd(H)               # H = U S Vt
    R = Vt.T @ U.T                            # optimal rotation
    if np.linalg.det(R) < 0:                   # forbid a reflection
        Vt[-1] *= -1; R = Vt.T @ U.T
    t = qbar - R @ pbar
    return R, t

def icp(moving, model, iters=20):
    src = moving.copy()
    for _ in range(iters):
        # step 1: nearest-neighbor correspondences
        idx = [int(np.argmin(((model - p)**2).sum(1))) for p in src]
        R, t = best_fit_transform(src, model[idx])   # steps 2-4
        src = (R @ src.T).T + t                       # apply, then re-correspond
    return src

When does it stop? Convergence and the error metric

Each iteration applies a (R, t) and re-corresponds. How do we know it's done? We watch a single number: the mean alignment error — the average distance from each moving point to its corresponded model point after the transform:

where ∥·∥ is the length of a vector (the Euclidean distance). ICP is guaranteed to never increase E from one iteration to the next — each step is the optimal move given the current correspondences, so the error monotonically decreases. We stop when E falls below a tolerance, or when the change in E between iterations is negligible (it has flattened out), or after a maximum iteration count. Early iterations slash E with big moves; later ones polish with tiny ones.

A small footnote on flavors: the version above is point-to-point ICP (minimize distance between paired points). A common refinement is point-to-plane (or point-to-line in 2D), which minimizes distance from each point to the surface the model point sits on. It converges in far fewer iterations on flat structures like walls — exactly the indoor environments our line extraction (Chapter 7) was built for — because sliding a point along a wall costs nothing, which is geometrically true.

Chapter 9: Showcase — Watch ICP Snap a Scan Into Place

Everything from Chapter 8 is here, alive. Below sit two point clouds of the same room outline: the model (teal — what the robot already has) and a scan (orange — a fresh sweep, rotated and shifted by the robot's unknown motion, plus noise). They describe the same walls, but they don't line up. Your job — ICP's job — is to find the rigid transform that lays orange onto teal.

Watch the rhythm: correspondences, then motion, then smaller error, repeat. Early iterations make big jumps (the clouds are far apart); later ones make tiny adjustments (fine alignment). When the error stops dropping, ICP has converged — and the transform it accumulated, R and t composed over all the iterations, is the robot's recovered motion between the two scans.

Now the payoff for the whole lesson. That recovered (R, t) is an exteroceptive measurement of motion — built from the geometry of the world, not from integrating wheel ticks or gyro rates. Feed it back to the localization module and it does what odometry never could: it corrects the drift. The encoders and IMU give a fast, smooth, drifting guess; ICP scan-matching periodically yanks that guess back to ground truth. Fast-but-drifty plus slow-but-absolute — exactly the partnership Chapter 10 formalizes.

Chapter 10: Sensor Fusion — The Sum Beats Every Part

We've met sensors with opposite strengths. The IMU is fast and smooth but drifts. LiDAR scan-matching is slow and jittery but absolute — it never drifts, but each estimate carries noise. Neither alone gives a robot a good pose. Sensor fusion is the principled way to combine them so the result is better than any single sensor — you get the IMU's speed and the LiDAR's long-term truth.

Why combine at all? Complementary strengths

Stanford classifies fusion into three flavors. Competitive fusion combines redundant sensors measuring the same thing to cut uncertainty (two range sensors averaging out their noise). Complementary fusion combines sensors that each see part of the picture (a short-range and a long-range sensor covering different distances). Cooperative fusion combines sensors where the answer needs both (GPS plus stereo vision). The IMU-plus-LiDAR pairing is complementary in time: the IMU owns the short timescale, LiDAR owns the long one.

Fusing two Gaussian measurements, with numbers

Take the simplest case Stanford works out: two sensors measure the same distance, each Gaussian. Sensor 1 reports m₁ with variance σ₁²; sensor 2 reports m₂ with variance σ₂². The optimal fused estimate is a variance-weighted average:

Notice each measurement is weighted by the other sensor's variance — so the more-certain (smaller-variance) sensor gets the bigger weight. Let's plug in numbers. A precise LiDAR reads m₁ = 5.00 m with variance σ₁² = 0.01 (std 0.1 m); a coarse sonar reads m₂ = 5.40 m with variance σ₂² = 0.09 (std 0.3 m):

The fused estimate 5.04 m sits much closer to the precise LiDAR (5.00) than to the coarse sonar (5.40) — correctly, because the LiDAR is more trustworthy. And the fused variance 0.009 is smaller than either input (0.01 and 0.09): combining evidence makes you more certain than either sensor alone. Fusion both corrects and sharpens.

The complementary filter — fusion across timescales

For the IMU-plus-absolute-sensor pairing, the cheap classic is the complementary filter. It blends the two estimates with a single weight α close to 1:

Read it as two filters in one. The first term — the gyro integrated forward — is a high-pass: it follows fast motion crisply but lets slow drift through. The second term — the absolute reference (LiDAR heading, magnetometer) — is a low-pass: it's noisy moment-to-moment but correct on average. With α = 0.98, the estimate tracks the gyro 98% for snappy response, while the 2% pull toward the absolute reference quietly cancels drift over time. Worked: if the gyro says the heading is 30.0° (drifting) and the absolute sensor says 28.0° (noisy but unbiased), the blend is θ̂ = 0.98(30.0) + 0.02(28.0) = 29.4 + 0.56 = 29.96° — mostly following the gyro, gently corrected toward truth.

The complementary filter is the hand-tuned, fixed-weight version; the Kalman filter is the principled version that computes the optimal weight every step from the running uncertainties. Same idea, more rigor — which is exactly why Lesson 13 picks up here.

Chapter 11: The Sensor Toolkit — Connections & Cheat-Sheet

You now know how a robot's raw data is born — and how noisy, partial, and drifting it always is. Here is the whole sensor toolkit on one page, the way you'd want it on a real robot's design review.

Sensor cheat-sheet

The big ideas, distilled

Where this sits on the autonomy stack

Sensors are the bottom of the stack — the "See" beat of see-think-act. Their raw returns feed Perception (blue): line extraction and point clouds are perception turning numbers into features. The motion recovered from ICP scan-matching, and the pose from odometry, feed Localization & SLAM (purple), which fuses them into a drift-free pose and map. This lesson is the handoff from raw sensing to meaning.

Related lessons on Engineermaxxing

• Lesson 8: Camera Models & Calibration — the pinhole model fully built, with the camera matrix and calibration we forward-referenced in Chapter 5.
• Lesson 13: Kalman / EKF / UKF — the optimal, recursive version of the variance-weighted fusion from Chapter 10.
• Classical Visual-Inertial Odometry — fusing an IMU with a camera, exactly the fast-drifty-plus-absolute partnership, in depth.
• Modern SLAM — stacking many scan-matches and loop closures into a globally consistent map.
• The Kalman Filter & The Bayes Filter — the belief-update math under all sensor fusion.