Week 12 — Simultaneous Localization and Mapping (SLAM) Day 4 of 7

Why This Matters

A GPS-denied warehouse robot must know where it is to within 5 cm. A 2D visual SLAM system (ORB-SLAM from Day 3) fails here for two reasons. First, warehouse aisles are textureless: a row of identical gray pallets gives ORB almost no keypoints. Second, the lighting is uneven and often dim. LiDAR does not care. A 16-beam spinning LiDAR returns 300,000 points per second whether the floor is concrete or carpet, bright or dark.

The problem is that raw LiDAR points arrive in the sensor frame, 10 Hz, distorted by motion. Between two scans the robot may have moved 0.5 m and rotated 5°. If you simply stack scans without registering them, walls smear into double lines within 3 seconds. Every downstream task (path planning, obstacle avoidance, map building) fails.

LOAM (Lidar Odometry and Mapping, Zhang and Singh, 2014) solved this with a two-stage approach that became the template for a decade of LiDAR SLAM. LeGO-LOAM (Shan and Englot, 2018) made it run on a Jetson-class CPU by exploiting ground segmentation. This lesson covers both, with the actual math and the numbers you need to tune them.

Core Concepts

LOAM: Feature-Based Registration in Two Stages

LOAM does not match raw point clouds. It extracts two feature types per scan, then registers them.

Edge points have high curvature. For each point $i$, compute smoothness $c$ over its $k$ neighbors (LOAM uses $k=10$, half on each side along the scan ring):

$$c_i = \frac{1}{k |\mathbf{p}i|} \left| \sum{j \in S, j \neq i} (\mathbf{p}_j - \mathbf{p}_i) \right|$$

Points with $c_i$ in the top 10% are edge candidates; the bottom 10% are planar candidates. On a 16-beam Velodyne HDL-16, one scan yields roughly 1,800 edge features and 1,400 planar features after filtering. That is a 100:1 reduction from ~30,000 raw points. Registration then runs on these sparse sets.

The two-stage split is the key idea. Odometry runs at 10 Hz and matches each scan to the previous one. It is fast but drifts. Mapping runs at 1 Hz and matches the current scan to a growing voxel map (LOAM uses 1 m cubes). It is slow but accurate. The mapping stage refines the pose the odometry stage produced, and its output feeds back as the odometry prior. This is why LOAM holds drift under 1% of distance traveled on the KITTI benchmark, versus 3–5% for single-stage scan matching.

LeGO-LOAM: Ground Segmentation and Two-Step Optimization

LeGO-LOAM targets embedded hardware. It first separates ground from non-ground points using the vertical angle of each point and a local plane fit. On a typical indoor floor, 40–60% of points are ground. Those points constrain only roll, pitch, and Z. LeGO-LOAM uses them in a cheap first optimization, then uses the remaining edge and planar features for the full 6-DOF second optimization.

The second change is label matching. LeGO-LOAM keeps the segmentation labels from the ground split. When matching feature $i$ to the map, it only considers map points with the same label. A planar feature on the ground never matches an edge feature on a shelf. This cuts the correspondence search from $O(n \cdot m)$ to a much smaller set and rejects outliers before the optimizer sees them. The result runs at 10 Hz on a Jetson TX2 at ~50% CPU, where LOAM needs a desktop i7.

Mathematical Foundation

Both systems solve the same core problem: find the rigid transform $\mathbf{T} \in SE(3)$ that minimizes feature-to-feature distance.

For an edge point $\mathbf{p}_i$ matched to a line through map points $\mathbf{a}, \mathbf{b}$, the residual is the point-to-line distance:

$$d_e = \frac{\left| (\mathbf{p}_i’ - \mathbf{a}) \times (\mathbf{p}_i’ - \mathbf{b}) \right|}{\left| \mathbf{a} - \mathbf{b} \right|}$$

For a planar point matched to a plane with normal $\mathbf{n}$ through point $\mathbf{q}$, the residual is point-to-plane:

$$d_p = \mathbf{n}^T (\mathbf{p}_i’ - \mathbf{q})$$

Here $\mathbf{p}_i’ = \mathbf{R}\mathbf{p}_i + \mathbf{t}$ is the point transformed by the current pose estimate, with rotation $\mathbf{R}$ and translation $\mathbf{t}$. $\mathbf{a}, \mathbf{b}, \mathbf{q}, \mathbf{n}$ come from the map. The optimizer minimizes the sum over all correspondences:

$$\min_{\mathbf{T}} \sum_i d_{e,i}^2 + \sum_j d_{p,j}^2$$

Worked example. Suppose one edge point sits at $\mathbf{p} = (5.00, 0.00, 0.10)$ m in sensor frame. The map line runs through $\mathbf{a} = (5.02, -0.50, 0.00)$ and $\mathbf{b} = (5.02, 0.50, 0.00)$. The current pose estimate is identity. Then:

So $d_e \approx 0.102$ m. That is a large residual. A well-converged match should be under 0.05 m, ideally under 0.02 m. The optimizer will pull the pose 10 cm toward the line. The Jacobian of $d_e$ with respect to the 6-DOF pose increment gives the gradient direction. LOAM solves this with Levenberg-Marquardt, typically converging in 4–6 iterations per scan.

Practical Example

Scenario: A differential-drive AGV in a 40 m × 25 m cold-storage warehouse. Payload: 500 kg pallets. Sensor: Ouster OS1-16 mounted 1.8 m high, tilted 5° down. Required localization error: under 5 cm lateral, under 1° heading. Speed: 1.2 m/s.

Step 1 — Sensor choice. The OS1-16 gives 16 rings, vertical FOV 33°. At 1.8 m height and 5° down-tilt, the lowest ring hits the floor 4.1 m ahead. That is enough floor coverage for LeGO-LOAM’s ground segmentation. A 64-beam unit would be overkill and cost 3× more.

Step 2 — Feature counts. At 10 Hz and 1.2 m/s, the robot moves 0.12 m per scan. With a 0.5 m minimum feature-separation radius, expect ~1,200 edge and ~900 planar features per scan. This is within LeGO-LOAM’s comfort zone (it degrades below ~500 features).

Step 3 — Ground split. Cold storage floors are flat concrete. Roughly 55% of points are ground. LeGO-LOAM’s first optimization uses these to lock Z and roll/pitch. The AGV only needs X, Y, and yaw from the second stage, which reduces the effective problem size by about 40%.

Step 4 — Map resolution. Use 0.4 m voxels, not the default 1 m. Aisles are 3 m wide. With 1 m voxels, a pallet edge and the wall behind it merge into one cell, and planar matching picks the wrong plane. At 0.4 m, the pallet face and wall stay separate.

Step 5 — Verification. Log the per-scan residual RMS. A healthy run shows 0.02–0.04 m. A rising trend above 0.08 m means the odometry prior is drifting. Check wheel odometry calibration and IMU bias before blaming the SLAM.

Common Pitfalls

1. Motion distortion ignored. A LiDAR scan takes ~100 ms to complete. At 1.2 m/s and 30°/s yaw, the first and last points of one scan are 0.12 m and 3° apart. If you do not de-skew using the IMU or constant-velocity model, walls curve and edge residuals never drop below 0.08 m. Detect: residual RMS stays high even with a good initial guess. Fix: interpolate poses across the scan using IMU angular rates; LOAM does this with a linear motion model per scan.

2. Feature extraction on dynamic objects. A forklift crossing the aisle contributes edge features that move between scans. The optimizer fits the pose to the forklift, not the walls. Detect: sudden pose jumps of 5–10 cm with no wheel odometry change. Fix: LeGO-LOAM’s label matching helps, but add a range-image outlier filter: drop features whose nearest neighbor in the previous scan is more than 0.3 m away.

3. Voxel size too coarse for the structure. Covered above in the warehouse example. The general rule: voxel size should be no more than 1/5 of the smallest gap between distinct surfaces you need to resolve. For a 3 m aisle with 0.3 m clearance to pallets, that means 0.3–0.4 m voxels.

4. Ground segmentation failing on ramps. LeGO-LOAM assumes a locally flat ground plane. On a 10° loading ramp, the plane fit fails, ground points get labeled as obstacles, and the first optimization diverges. Detect: the ground-point count drops sharply while total points stay constant. Fix: use a robust plane fit (RANSAC with 100 iterations) and relax the angle threshold from 10° to 15°.

5. No loop closure in vanilla LOAM. LOAM has no loop closure. Over a 200 m patrol, expect 1–2 m drift. If the robot returns to start, the map has a visible seam. Detect: compare the pose at the start of a lap to the pose at the end. Fix: this is exactly what Day 5 covers. Until then, run LOAM in a bounded area under 50 m or accept the drift.

Key Takeaways

  1. LOAM splits the problem: fast 10 Hz odometry on scan-to-scan, slow 1 Hz mapping on scan-to-map. The split is what keeps drift near 1% of distance traveled.
  2. Feature extraction cuts ~30,000 raw points to ~3,000 edge and planar features. Register features, not points. This is 10× faster with no accuracy loss.
  3. LeGO-LOAM’s ground segmentation and label matching make LiDAR SLAM runnable on a Jetson-class CPU at 10 Hz. Use it for embedded systems; use LOAM when you have desktop compute.
  4. Point-to-line and point-to-plane residuals are the workhorses. A converged match gives residuals under 0.05 m; anything above 0.08 m means something is wrong upstream.
  5. Voxel size must be smaller than the smallest gap between distinct surfaces you need to resolve. For warehouse aisles, 0.4 m, not the default 1 m.

Tomorrow

Tomorrow we tackle Loop Closure Detection: how to recognize that the robot has returned to a place it has seen before, and how to correct the accumulated drift without breaking the map.