M5-T4 — Scan Registration

Learning objectives

After this lesson, you should be able to:

• Compare point-to-point, point-to-line, point-to-plane, GICP and NDT in terms of cost, accuracy and robustness.
• Derive the small-angle linear system of point-to-plane registration and read its Jacobian row [n, p × n].
• Detect degeneracy from the eigenvalues of JᵀJ and identify the unconstrained direction.
• Choose a registration method for a given sensor and environment.

The problem and the intuition

Sliding a sheet of paper along a table does not change how well it lies on the table: the distance from every point to the table surface stays zero. A point-to-point matcher feels a difference (the nearest table point changes), so it slowly slides the paper in many small steps until points happen to coincide. A point-to-plane matcher knows the table is a plane and feels no difference along it: its cost is flat in the sliding directions, which is both a strength (it converges fast, without caring about sampling) and a warning (those directions are unconstrained and need another surface).

Using the surface normal says: I care how far I am from the surface, not where on the surface I am.

The algorithm: choosing and linearising the distance

Problem. Given a source cloud P (the new scan, thinned) and a target Q (the previous scan or the local map), find the rigid transform (R, t) minimising a sum of squared point-to-surface distances. This extends the ICP of M3-T5.

Assumptions. The target has local surface structure (normals can be estimated); an initial transform within the convergence basin; mostly static scene.

Point-to-plane linearisation

For a source point p, its nearest target point q and the target normal n at q, the residual is r = n · (R p + t − q). For a small rotation, R ≈ I + [ω]_×, so R p ≈ p + ω × p, and

n

unit normal of the target surface at the matched point.

p × n

cross product: the lever arm of the normal about the origin, which couples rotation to the residual.

x = (t, ω)

the six unknowns of the small motion (3 for translation, 3 for rotation).

H = Σ JᵀJ

the 6×6 information matrix of the registration: its eigenvalues tell what is constrained.

The step is applied, the correspondences are re-computed, and the loop repeats (usually 5–15 iterations). In 2D it is a 3×3 system.

Other distance measures

Degeneracy

If H has an eigenvalue near zero, the corresponding eigenvector is a direction (a translation or rotation) in which the scan puts no constraint. In a corridor the along-axis translation has no surface normal component, so H_xx = 0. Zhang et al. (2016) propose to solve only in the well-constrained subspace and to take the missing direction from another source (the prediction or an IMU).

A worked example: which directions does a scene constrain?

Each matched point with normal n contributes nnᵀ to the translation block of H (the 3×3 top-left part of JᵀJ). Take 100 points on each plane.

1. A room: floor, one wall in front, one wall at the side. The normals are z, x and y. The translation block is 100 (zzᵀ + xxᵀ + yyᵀ) = 100 I: all three translations are constrained equally. (For three orthogonal planes the rotations are constrained as well: the lever arms p × n are non-zero.)
2. A corridor: floor, left wall, right wall. The normals are z, y and −y. The translation block is 100 (zzᵀ + yyᵀ + yyᵀ) = diag(0, 200, 100). The eigenvalue along x is 0: a motion along the corridor changes no residual. The registration cannot say whether the robot moved 0 or 5 m.
3. Add one door frame (a surface with normal x, 20 points). The x eigenvalue becomes 20: nonzero, but small relative to 200 and 100. The condition number is 10; the along-corridor position is now observable, though weakly. The uncertainty in x grows as 1/√20 relative to the others.
4. A ground-only scene (an open parking lot): the normals are all z. The translation block is diag(0, 0, 100·N): only height, roll and pitch are observed. This is why LiDAR odometry on flat open ground drifts in x, y and yaw unless there are poles, cars or kerbs.
5. Reading the result. The ratio of the smallest to the largest eigenvalue of H is the diagnostic: below, say, 10⁻² the direction is weak; at 0 it is lost. A system that reports a covariance from H lets the back end (M4) weight the constraint correctly.

The lab’s stage on diagnosing registration failure uses this same computation.

Engineering implications and limitations

Engineering implications

• Use point-to-plane (or GICP) in structured scenes and point-to-point with adaptive thresholds and a good motion prior (KISS-ICP) in unstructured ones. Point-to-point is simpler and, done right, surprisingly robust.
• Estimate normals from enough neighbours (10–20) and reject those with a poor planarity (ratio of eigenvalues). Noisy normals are the main source of point-to-plane error.
• Reject bad correspondences: distance gates that shrink over the iterations, normal-compatibility checks, and trimmed or robust losses (M3-T5).
• Provide a prior. Registration needs an initial guess: IMU, constant velocity, or the last motion. Compute the match inside the prior’s uncertainty.
• Compute and publish the information matrix H, its smallest eigenvalue and the inlier ratio. Pass them to the back end.
• Speed. A k-d tree (nanoflann, FLANN) or an incremental structure (ikd-Tree, voxel hash map) keeps each iteration at O(N log M).

Limits

All local registration methods need a start in the right basin and a static scene, and all fail silently in degenerate geometry. Global registration (FPFH with RANSAC, TEASER++, learned descriptors) removes the dependence on the initial guess but costs more and is used for relocalisation and loop closure rather than odometry. A match with a low residual is not proof of a correct pose; check the inlier ratio, the conditioning and the consistency with the prior.

Common misconceptions

Engineering takeaways

1. Point-to-plane distance uses the surface normal and makes each iteration a linear least-squares problem with Jacobian row [n, p × n].
2. Point-to-line is the right model for edges; GICP and NDT smooth the problem and widen the basin.
3. The 6×6 matrix JᵀJ shows what is constrained: a zero eigenvalue means a degenerate direction (the corridor).
4. Registration needs an initial guess, rejects outliers, and should report its information matrix and inlier ratio.
5. Choose by scene: planar structure favours point-to-plane; unstructured scenes favour well-built point-to-point or direct methods.

References and further reading

Established fundamentals. Global registration and learned matching (Module 8) are active research.

• Chen, Y. and Medioni, G. (1992). Object modelling by registration of multiple range images. Image and Vision Computing 10(3): point-to-plane ICP.
• Segal, A., Haehnel, D. and Thrun, S. (2009). Generalized-ICP. Robotics: Science and Systems.
• Biber, P. and Straßer, W. (2003). The normal distributions transform: a new approach to laser scan matching. IEEE/RSJ IROS.
• Zhang, J., Kaess, M. and Singh, S. (2016). On degeneracy of optimization-based state estimation problems. IEEE International Conference on Robotics and Automation.
• Koide, K., Yokozuka, M., Oishi, S. and Banno, A. (2021). Voxelized GICP for fast and accurate 3D point cloud registration. IEEE International Conference on Robotics and Automation.
• Vizzo, I. et al. (2023). KISS-ICP: In defense of point-to-point ICP. IEEE Robotics and Automation Letters 8(2).

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Next: how LOAM puts feature extraction and registration into a real-time pipeline that runs at two different rates.

Related content

Nothing below is required to finish this lesson.

• Before
  M5-T3: Feature Extraction

  Compute LOAM-style smoothness to select edge and planar features and understand why selection helps.

• Lab
  M5-L2: Diagnose Poor Scan Registration

  Registration fails in a corridor and with a bad start: diagnose degeneracy and choose the right cost.