ProjekinAja. Robotics Stage 5 · Perception SLAM Lesson 5.8
● Stage 5 · Lesson 5.8

Scan Matching and ICP: LIDAR-Based Pose Estimation

ICP is a local optimizer — give it a bad initial guess and it will converge confidently to the wrong answer; ICP degeneracy in corridors is not an outlier scenario, it is the default environment for indoor robots.

⏱ ~22 min read 📐 Geometry + Linear Algebra 🤖 SLAM foundation

🎯 By the end you'll understand

How ICP iteratively finds the rigid transformation between two LIDAR scans. The difference between point-to-point and point-to-plane error metrics and why it matters for indoor walls. Why ICP can converge confidently to the wrong answer from a bad initial guess. How NDT scan matching avoids the correspondence problem entirely. How to extract the covariance from a scan match result and use it as observation noise R in FollowBot's EKF. How to detect ICP degeneracy in a featureless corridor and reject the measurement rather than corrupting the filter.

KEY INSIGHTICP is a local optimizer, not a global solver

ICP will always converge. Whether it converges to the right answer depends entirely on your initial guess — and in a long corridor, no amount of iterations will save you from a bad one.

Scan matching is the problem of finding the rigid body transformation — a rotation and translation — that best aligns one LIDAR scan to another (or to a reference map). ICP (Iterative Closest Point) is the workhorse algorithm: simple, fast, and deeply misunderstood. The misunderstanding is this: practitioners treat ICP as a robust estimator. It is not. It is a local optimizer. It descends a non-convex landscape and stops at the nearest local minimum. If you initialize it well, that local minimum is the correct alignment. If you do not, it converges confidently to a wrong rigid transformation — and the residual metric can look just as good either way.

🔑 The practical implication

ICP's job in FollowBot's localization pipeline is not to solve the global place-recognition problem — that belongs to the map and the EKF's initial state. ICP's job is to refine a pose estimate that is already close to correct, typically within 0.3–0.5 m and 10–15° for a 2D LIDAR setup. The EKF's predicted pose (from odometry + IMU propagation) provides this initial guess every scan cycle. When the prediction is good, ICP is excellent. When the prediction fails — after a wheel slip, an IMU saturation, or a long featureless corridor where odometry drifts freely — ICP can be actively harmful.

HOW IT WORKSICP: three steps, iterated to convergence

ICP operates on two sets of 2D (or 3D) points: a source scan (the current LIDAR reading, to be aligned) and a target (the reference — either the previous scan or a map). It finds the rigid transformation T = (R, t) that minimizes the sum of squared distances between corresponding point pairs.

1

Correspondence estimation

For each point in the source scan, find its nearest neighbor in the target scan. This is the critical and expensive step. In the basic ICP variant, "nearest neighbor" means smallest Euclidean distance. The resulting pairs are the correspondences — and they are almost certainly imperfect, especially early in the iteration when the two scans are misaligned. Outlier correspondences (where a source point genuinely has no match in the target because it hit a moving object or a surface outside the target's field of view) are optionally rejected by distance thresholding.

2

Transformation update via SVD

Given a fixed set of correspondences (pi, qi), the optimal rigid transformation is closed-form. Compute the cross-covariance matrix H = Σ (pi − p̄)(qi − q̄)T, where p̄ and q̄ are the centroid-subtracted means. Decompose H = UΣVT via SVD. The optimal rotation is R* = VUT, and the optimal translation is t* = q̄ − R*p̄. This closed-form update is exact for the given correspondences — the iteration is needed only because the correspondences themselves change as the transformation improves.

3

Iterate until convergence

Apply the transformation update, recompute correspondences with the new alignment, compute the mean squared residual error. Terminate when: (a) the change in transformation between iterations falls below a threshold (e.g., Δt < 0.001 m, Δθ < 0.01°), or (b) the mean squared residual error changes by less than ε, or (c) a maximum iteration count is reached. Convergence is typically reached in 10–50 iterations for 2D LIDAR scans in structured environments. In pathological cases (corridors, open fields) the algorithm may hit the iteration cap without meeting the pose-change criterion.

Source scan (current LIDAR) Target scan (or map) SVD solve R*, t* → update T Residual mean sq. dist. iter 0 iter N evaluate
ICP loop: source points find their nearest neighbors in the target (dashed correspondence arrows), the SVD solves for the optimal rigid transform, the source is updated, residual is evaluated. Repeat until convergence. The residual curve shows the characteristic rapid initial drop followed by diminishing returns.

CORE CONCEPTPoint-to-point vs. point-to-plane error

The standard ICP formulation above minimizes point-to-point distance: the straight-line distance from each source point to its nearest target point. This is geometrically simple but converges slowly on planar surfaces — which are exactly what indoor environments are made of.

The reason is geometric. If the robot is near a flat wall, all LIDAR returns on that wall segment should map to that plane. The point-to-point distance between a source wall point and its target wall nearest neighbor has a large component along the wall (because small misalignments shift points along the surface) and a small component perpendicular to it (the true error). Minimizing the former aggressively distorts the solution; minimizing only the perpendicular component would be ideal. That is exactly what point-to-plane ICP does.

MetricError definitionConvergence speedWhen to prefer
Point-to-point ‖pi − qi‖² Slow on planes Sparse, non-planar geometry (e.g., vegetation)
Point-to-plane ((pi − qi) · n̂i)² 3–5× faster on indoor walls Structured indoor environments (rooms, corridors)
Point-to-distribution (NDT) Mahalanobis dist. to voxel Gaussian Very fast; no correspondence step Dense point clouds; GPU-available pipelines

Point-to-plane ICP requires estimating the surface normal n̂ at each target point — typically computed from the local neighborhood of 5–10 nearest neighbors. In 2D LIDAR, normals are estimated from scan segment directions, which is reliable on walls and unreliable on isolated points or corners. This is an important edge case: corners are precisely where you want ICP to lock in (they have strong constraints in all directions), and they are exactly where normal estimation is noisiest. SLAM Toolbox — which FollowBot uses — implements point-to-plane as its default scan matcher, with adaptive normal estimation that falls back to point-to-point on geometric singularities.

💡 Why convergence speed matters more than accuracy per iteration

In a 10 Hz LIDAR pipeline, you have 100 ms per scan cycle. ICP must complete within a fraction of that — typically 10–20 ms — to leave budget for the EKF update, occupancy grid update, and network overhead. Point-to-plane's 3–5× convergence speedup often means the difference between 8 and 40 iterations in a fixed time budget, which translates directly to a better pose estimate under the same wall-clock constraint.

HOW IT WORKSConvergence conditions and when they mislead you

ICP terminates on one of three conditions. Understanding which one triggered is diagnostically important:

Production scan matching code (including SLAM Toolbox and Cartographer) logs which condition triggered. Most robotics teams ignore these logs until something breaks. Do not be those teams.

📖 The information matrix: what ICP actually knows about uncertainty

After ICP converges, the information matrix (inverse of the pose covariance) can be approximated from the Hessian of the residual at the converged point: Λ ≈ JTJ, where J is the Jacobian of residuals with respect to the pose parameters. The eigenvalues of Λ encode constraint strength: a large eigenvalue means the scan strongly constrains motion along that direction; a small eigenvalue (near zero) means the scan provides almost no constraint along that direction. This is the degeneracy detector. In a corridor parallel to the x-axis, one eigenvalue will be large (the robot is well-constrained perpendicular to the corridor) and one will be near zero (almost unconstrained along the corridor). This eigenvalue structure is exactly what the widget below visualizes.

SEE ITICP convergence demo + degeneracy detector

Choose an environment geometry and step through ICP iterations. Watch the correspondence arrows, the residual curve, and — critically — the eigenvalues of the information matrix. Degenerate directions are highlighted in red. The goal: understand when to trust the scan match and when to reject it.

⚙ Interactive · ICP convergence + degeneracy detector

Select environment geometry. Click Step to advance one ICP iteration, or Run to animate. Watch the eigenvalues — red means degenerate.

Environment:
source   target
ICP State
Iteration: 0
Residual: —
ΔPose (m): —
Status: ready
Information matrix eigenvalues
λ₁ (strong axis)—
λ₂ (weak axis)—
Degeneracy: —

EKF decision

Run ICP to see the EKF update decision.

Residual vs. iteration

Select environment and press Step or Run. In the corridor, watch λ₂ → 0 and the EKF reject the x-axis measurement.

The key observation is what happens in the corridor environment: λ₁ remains large (the robot is well-constrained perpendicular to the corridor walls) but λ₂ collapses toward zero (the robot could slide any distance along the corridor and the scan residual would barely change). This is degeneracy. The correct response is to reject — or heavily down-weight — the measurement along the degenerate axis in the EKF update, rather than accepting a spurious position correction that will corrupt the filter state.

CORE CONCEPTNDT: eliminating the correspondence problem

Normal Distributions Transform (NDT) scan matching takes a fundamentally different approach. Instead of finding point-to-point correspondences, it models the target scan as a field of Gaussian distributions — one per voxel (grid cell) of the environment. Each voxel contains the mean and covariance of the target points that fall within it. To align a source scan, NDT evaluates the probability of each source point under the Gaussian distribution of the voxel it falls into, and maximizes this log-likelihood using Newton's method.

The practical consequences:

NDT does not eliminate the degeneracy problem — a Gaussian model of a corridor still has one unconstrained direction. But it does eliminate the sensitivity to correspondence noise, which makes it more robust than ICP when the initial guess is off by more than one scan segment.

💡 SLAM Toolbox's scan matcher uses a hybrid approach

SLAM Toolbox, which FollowBot runs for mapping, uses a correlative scan matcher for global localization (exhaustive search over a discretized pose space — slow but global) followed by ICP or a gradient-descent refinement step. The correlative matcher solves the initialization problem that ICP cannot; ICP provides the sub-cell accuracy that the correlative scan is too coarse to deliver. This is the standard two-stage architecture: global → local, coarse → fine.

IN PRACTICEScan matching covariance as observation noise R for the EKF

The EKF update step requires an observation noise covariance R — the 3×3 (or 2×2 for position-only) matrix encoding how much to trust the scan match result relative to the prediction. Most tutorials hardcode R. That is wrong. The correct approach is to use the covariance that comes out of the scan matcher itself.

After ICP or NDT converges, the pose covariance C is estimated as the inverse of the information matrix Λ:

scan_match_covariance.pyrclpy · scan match → EKF R matrix
import numpy as np

# After ICP converges: Jacobian J of residuals wrt pose (3D: x, y, theta)
# J shape: (N_correspondences, 3)
def scan_match_covariance(J, sigma_r=0.02):
    """Approximate pose covariance from ICP Jacobian.
    sigma_r: expected point residual std dev (metres)."""
    # information matrix: Lambda ≈ (1/sigma_r²) * J^T J
    Lambda = (1.0 / sigma_r**2) * J.T @ J         # shape (3, 3)

    # degeneracy check: smallest eigenvalue
    eigvals = np.linalg.eigvalsh(Lambda)
    min_eig = np.min(eigvals)

    if min_eig < 10.0:   # threshold: tune per environment
        # degenerate: do not update EKF along weak axis
        return None, "DEGENERATE — measurement rejected"

    # covariance = Lambda^{-1}; this is R for the EKF
    R = np.linalg.inv(Lambda)
    return R, "OK"

# In the EKF update node (rclpy callback):
def scan_match_callback(self, msg):
    J = self.get_icp_jacobian(msg)          # from scan matcher
    R, status = scan_match_covariance(J)
    if R is None:
        self.get_logger().warn(f'{status}')
        return                              # reject: skip EKF update
    self.ekf.update(msg.pose, R)

Two things to observe here. First, the degeneracy check is explicit and happens before the EKF update — a degenerate measurement is rejected outright, not down-weighted by a large R. This is the conservative approach and is correct: a very large R would still allow the EKF to move the state in the degenerate direction by a small amount per scan cycle, and over 100 scan cycles this accumulates into a real error. Rejection is cleaner. Second, sigma_r = 0.02 m (2 cm) is a reasonable point residual for a well-calibrated 2D LIDAR at typical indoor ranges — but it should be validated against actual residual distributions from your specific sensor.

🔑 The covariance connects Stage 4 to Stage 5

In Lesson 4.9 (Multi-Sensor Fusion on FollowBot) you designed the EKF architecture and chose placeholder values for the observation noise R. The scan matching covariance derived above is the real R for the LIDAR-based localization measurement. The IMU and odometry noise models from Stage 4 go into the process noise Q. The occupancy map from Lesson 5.7 provides the target scan. Everything is now connected: sensor → scan match → covariance → EKF → pose estimate → occupancy map update.

FOLLOWBOTIntegrating scan matching into FollowBot's localization pipeline

FollowBot's localization uses SLAM Toolbox in localization mode (map already built), which wraps a scan matcher internally. But understanding the scan match output is essential for two reasons: detecting when localization is unreliable, and extracting pose covariance for the EKF.

The SLAM Toolbox node publishes a /slam_toolbox/pose topic (a geometry_msgs/PoseWithCovarianceStamped) that includes a 6×6 covariance matrix. For a 2D robot, the relevant subblock is the 3×3 covering (x, y, θ). This is the R matrix for the EKF's LIDAR update. Here is how to extract and validate it:

slam_covariance_check.pyrclpy · extract R from SLAM Toolbox
import numpy as np
from geometry_msgs.msg import PoseWithCovarianceStamped
import rclpy
from rclpy.node import Node

class SlamCovCheck(Node):
    DEGEN_THRESH = 1e-3       # min eigenvalue of 3x3 cov subblock
    MAX_POS_SIGMA = 0.5       # max acceptable position std dev (m)

    def __init__(self):
        super().__init__('slam_cov_check')
        self.create_subscription(
            PoseWithCovarianceStamped,
            '/slam_toolbox/pose', self.cb, 10
        )

    def cb(self, msg):
        cov36 = np.array(msg.pose.covariance).reshape(6, 6)
        # extract x, y, theta block (indices 0,1,5)
        idx = [0, 1, 5]
        R = cov36[np.ix_(idx, idx)]               # 3x3 pose covariance

        eigvals = np.linalg.eigvalsh(R)
        min_eig = np.min(eigvals)
        pos_sigma = np.sqrt(np.max([R[0,0], R[1,1]]))

        if min_eig < self.DEGEN_THRESH:
            self.get_logger().warn(
                f'SLAM pose degenerate: min_eig={min_eig:.2e} — skipping EKF update'
            )
            return
        if pos_sigma > self.MAX_POS_SIGMA:
            self.get_logger().warn(
                f'SLAM uncertainty too high: σ={pos_sigma:.2f} m — skipping'
            )
            return

        # covariance is valid — pass R to EKF update
        self.ekf_update(msg.pose.pose, R)
🔑 FollowBot stage milestone progress

At this point in Stage 5, FollowBot has: camera intrinsics calibrated (< 0.5 px reprojection error); LIDAR-camera extrinsics calibrated; YOLOv8 person detection integrated in ROS 2; LIDAR cluster + camera bbox fusion producing stable 3D person position at 10 Hz; SLAM Toolbox building an occupancy map of the test environment. Lesson 5.8 adds the scan matching validation layer — the covariance check that prevents degenerate poses from corrupting the EKF. The occupancy grid inflated and ready for navigation milestone follows in Lesson 5.9 (SLAM integration).

One practical note on testing the degeneracy detector: build a simple test environment from cardboard boxes that creates a 3 m corridor section. Drive FollowBot along it and log the eigenvalue ratio λ₁/λ₂. You should see the ratio climb above 50:1 in the corridor section. If you do not, your corridor is not long or featureless enough to trigger the degeneracy, and you may need a longer hallway for validation. Real hospital corridors and warehouse aisles reliably produce λ₁/λ₂ > 100:1 in testing.

WEAKNESSWhere scan matching bites in practice

✗ Using ICP without a good initial guess and calling it "robust"

ICP has no global search capability. If the initial guess (from odometry or IMU propagation) is more than roughly 0.3 m or 15° from the true transformation, ICP will converge to a local minimum that looks convincing in terms of residual but is geometrically wrong. This failure is completely silent — the ICP node reports success, the residual is small (the wrong alignment also has small point distances for nearby geometry), and the EKF incorporates a wrong pose observation. The robot's map then starts accumulating phantom walls. The fix is not better ICP — it is better initial guesses (tighter EKF prediction through better process modeling) and global relocalization fallback when prediction uncertainty exceeds an ICP-safe threshold.

✗ Ignoring the degeneracy in corridors and trusting the x-axis pose update

This is the most common field failure in indoor service robotics. The robot enters a long corridor. ICP converges with λ₂ near zero along the corridor axis. The scan matcher reports a pose update that includes a small corridor-axis correction — the value is essentially noise. The EKF accepts it because no one implemented a degeneracy check. Over 30 seconds of corridor traversal, these noise measurements accumulate. The robot's x-position estimate drifts 0.4 m off-map, the occupancy grid updates start carving phantom obstacles into the map, and navigation fails when the robot tries to localize against a corrupted map. The fix is ten lines of code — the degeneracy check above. It is never written until after the first field failure teaches the lesson the hard way.

✗ Computing scan match covariance once and hardcoding it as R

Some teams run ICP in a test environment, measure the covariance, find it's approximately diag(0.01, 0.01, 0.001), and hardcode R = diag(0.01, 0.01, 0.001) for all future operation. This is wrong for two reasons. First, the covariance is environment-dependent: a room full of furniture produces tight covariances; a corridor produces a covariance that is large in one direction by orders of magnitude. Second, the covariance is range-dependent: LIDAR point noise grows with range, so a scan matching against a large, sparse target has higher covariance than one against a nearby, dense target. Hardcoding R means the EKF is operating with a wrong noise model for most of its operational life. Use the per-scan covariance extracted from the information matrix.

KEY INSIGHTHow this connects

  • 5.7 · Occupancy Grid Mapping: The Map Representation That Works — the occupancy grid from Lesson 5.7 is the target scan for ICP in localization mode. Every scan match aligns the current LIDAR reading against this map. The map quality directly determines whether ICP can find valid correspondences — a poorly built map produces degenerate scan matches even in non-degenerate environments.
  • 4.9 · Multi-Sensor Fusion on FollowBot: Architecture and Covariance Design — the EKF architecture designed in Lesson 4.9 is where the scan match covariance (R matrix) is consumed. The degeneracy detector output determines whether the scan match update is accepted or skipped. Degeneracy-aware rejection is the missing piece that makes the Stage 4 multi-sensor fusion actually work in real corridor environments.
  • 5.9 · SLAM: Simultaneous Localization and Mapping — SLAM Toolbox uses scan matching as its inner loop: every new scan is matched against the current map to estimate pose, and then the map is updated with the new scan. Understanding ICP degeneracy is prerequisite to understanding why SLAM diverges in corridors and how loop-closure recovery works.

Scan matching is the bridge between raw LIDAR data and the pose estimate that everything else in FollowBot depends on. Get it wrong — by trusting a degenerate match or using a hardcoded covariance — and every downstream component (occupancy grid, person tracker, EKF, path planner) degrades. Get it right, with degeneracy detection and per-scan covariance propagation, and the localization pipeline becomes robustly honest about what it knows and what it does not.

GLOSSARYKey terms introduced

ICP (Iterative Closest Point)
A local optimization algorithm that finds the rigid body transformation (rotation + translation) aligning a source point cloud to a target by alternating between correspondence estimation (nearest-neighbor matching) and transformation update (SVD-based closed-form solve). Requires a good initial guess; converges to the nearest local minimum, not the global minimum.
Point-to-plane ICP
A variant of ICP that minimizes the distance from each source point to the tangent plane of its target correspondence, rather than the Euclidean point-to-point distance. Converges 3–5× faster than point-to-point ICP on planar indoor surfaces by correctly handling motion along smooth surfaces.
NDT (Normal Distributions Transform)
A scan matching algorithm that models the target scan as a field of voxel-wise Gaussian distributions and maximizes the log-likelihood of source points under those distributions. Avoids the nearest-neighbor correspondence step, enabling O(1) voxel lookup per iteration and quadratic convergence via Newton's method.
Information matrix
The inverse of the pose covariance matrix, computed from the Hessian of the scan match residual at convergence (Λ ≈ JTJ). Its eigenvalues encode constraint strength: large eigenvalue = strong constraint; near-zero eigenvalue = degenerate, unconstrained direction.
ICP degeneracy
The condition in which the scan match information matrix has one or more near-zero eigenvalues, indicating that the environment provides insufficient geometric constraints to determine pose along one or more axes. Characteristic of featureless corridors (unconstrained along the corridor axis), open fields, and circular rooms.
Observation noise covariance (R)
The matrix in the EKF update step that encodes how much to trust a sensor measurement relative to the filter's prediction. For scan matching, R should be computed per-scan from the information matrix inverse, not hardcoded — environment and range both affect the actual uncertainty.
Scan-to-map matching
Aligning the current LIDAR scan against a pre-built map (the target), used in localization mode when the map is known. Contrasts with scan-to-scan matching (consecutive scans, used in odometry estimation), which accumulates drift without map corrections.

ACTIVE RECALLCheck your understanding

Answer in your own words first, then reveal.

1Why is ICP called a local optimizer and what does this mean for FollowBot's localization pipeline?›

ICP descends the gradient of the scan-match residual from its initial pose guess. It will always find a local minimum of that residual — but the residual landscape is non-convex, with many local minima corresponding to wrong alignments that happen to have small point distances in the current scan geometry. If the initial guess is within the basin of attraction of the correct global minimum (roughly: within 0.3 m and 15° for typical 2D LIDAR indoor scans), ICP converges correctly. If the guess is outside that basin — because odometry drifted after a wheel slip, or an IMU saturated during a sharp turn — ICP converges confidently to the wrong transformation. For FollowBot, this means the EKF's predicted pose (from odometry + IMU propagation) must be used as the ICP initial guess, and the prediction uncertainty must stay below the ICP convergence basin width. When it exceeds that width (e.g., after loss of wheel contact or long odometry-only stretches), a global relocalization step is needed before resuming scan matching.

2What is ICP degeneracy, which environments cause it for indoor robots, and how do eigenvalues of the information matrix detect it?›

ICP degeneracy occurs when the scan provides insufficient geometric constraints to determine pose along one or more axes. The information matrix Λ ≈ JTJ encodes constraint strength: its eigenvalues represent constraint along the eigenvector directions. A large eigenvalue means the scan strongly constrains motion along that axis; a near-zero eigenvalue means almost any displacement along that axis produces the same residual — the scan provides no information. In a long featureless corridor, the environment constrains the robot perpendicular to the corridor well (one large eigenvalue) but provides almost no constraint along the corridor axis (one near-zero eigenvalue). The eigenvalue ratio λ₁/λ₂ is the degeneracy indicator: ratios above 20–50:1 indicate degeneracy. The fix is to check min(eigenvalue) against a threshold before accepting the scan match result, and to reject (skip the EKF update) when the check fails rather than accepting a noise-dominated pose correction.

3Why is point-to-plane ICP faster than point-to-point on indoor walls, and what is the tradeoff?›

Point-to-point ICP minimizes the full Euclidean distance between a source point and its target correspondent. On a flat wall, a source point misaligned along the wall has a large point-to-point distance despite small perpendicular misalignment. The optimizer must work hard to disentangle the along-wall noise from the true perpendicular error. Point-to-plane ICP measures only the perpendicular distance to the target surface (using the surface normal at the target point), which is the geometrically meaningful error for planar surfaces. This focuses the gradient on the true alignment signal, leading to 3–5× faster convergence on structured indoor environments. The tradeoff is that point-to-plane requires accurate surface normal estimation at each target point — reliable on flat walls, noisy at corners and isolated points. Corners are geometrically important (they constrain motion in two directions), so poor normal estimation at corners can degrade convergence quality. Production implementations (SLAM Toolbox) detect geometric singularities and fall back to point-to-point at corners.

4How should the scan match covariance be used as the EKF's observation noise R, and why is hardcoding R wrong?›

After ICP or NDT converges, the pose covariance is approximated as the inverse of the information matrix: C ≈ Λ⁻¹ = (JTJ/σ²)⁻¹, where σ is the expected point residual noise. This per-scan covariance is the correct R for the EKF update — it tells the filter how uncertain the scan match result is for this specific scan in this specific environment. Hardcoding R is wrong for two independent reasons. First, the covariance is environment-dependent: a room with furniture produces tight covariances (small R) while a corridor produces a covariance that is large along the corridor axis by orders of magnitude (large R in one direction). Second, it is range-dependent: LIDAR point noise grows with target distance, increasing scan match uncertainty at long range. A hardcoded R uses a wrong noise model for most of the robot's operational life, causing the EKF to either over-trust or under-trust the scan match relative to odometry, distorting the fusion. The practical fix is a one-time addition of ~15 lines of code to extract the covariance from the scan matcher and pass it to the EKF update callback, with a degeneracy check as a guard.

Next: SLAM — closing the loop between mapping and localization →