NDT Localization Against an HD Map

Localizing against a map is registration with an asymmetry: one cloud is live and small, the other is prebuilt and large, and the large one does not change. The normal distributions transform exploits that asymmetry directly — the map is reduced once to a grid of local Gaussians, and every subsequent frame is scored against that grid rather than against points.

This task implements it for localization and map matching, and pays particular attention to the output most implementations discard: the Hessian, which is where the honest covariance comes from.

What the offline precomputation buys, per frame:

Prerequisites #

• Python 3.10+, NumPy 1.24+, SciPy 1.11+, open3d 0.18+ for the reference implementation.
• Input: a map tile carrying precomputed voxel distributions, a motion-compensated live sweep, and a pose prior.
• Upstream stage: motion compensation, as in motion-compensating LiDAR scans with SLERP.
• Output: a 6-DOF pose with a covariance derived from the score surface.

Step-by-Step #

1. Precompute voxel distributions with the tile #

import numpy as np

def build_ndt(points: np.ndarray, voxel_m: float = 1.0, min_pts: int = 6):
    """Per-voxel mean and covariance, computed once and stored with the tile."""
    keys = np.floor(points / voxel_m).astype(np.int64)
    order = np.lexsort(keys.T)
    keys, points = keys[order], points[order]
    edges = np.flatnonzero(np.any(np.diff(keys, axis=0), axis=1)) + 1

    cells = {}
    for a, b in zip(np.r_[0, edges], np.r_[edges, len(points)]):
        if b - a < min_pts:
            continue                        # too few points for a stable covariance
        blk = points[a:b]
        mu = blk.mean(axis=0)
        cov = np.cov(blk.T) + 1e-4 * np.eye(3)   # regularize a planar cell
        cells[tuple(keys[a])] = (mu, np.linalg.inv(cov))
    return cells

Key parameters: voxel_m at 1 m is the usual outdoor choice — small enough to capture kerbs and poles, large enough that most cells have a stable covariance. The 1e-4 regularization keeps a perfectly planar cell, which is what a road surface produces, from having a singular covariance.

2. Seed from the propagated prior #

def seed_pose(prev_pose, odom_delta):
    """Start the optimiser from where odometry says the vehicle went."""
    return prev_pose @ odom_delta

NDT's basin is wider than ICP's but not unlimited. Seeding from odometry keeps the optimiser inside it; seeding from the previous pose without propagation costs a metre of error at 15 m/s and 100 ms, which at 1 m voxels is enough to score against the wrong cells.

3. Optimise the score #

from scipy.optimize import minimize

def ndt_score(pose_vec, cloud, cells, voxel_m):
    T = pose_from_vec(pose_vec)
    pts = (cloud @ T[:3, :3].T) + T[:3, 3]
    keys = np.floor(pts / voxel_m).astype(np.int64)
    total = 0.0
    for p, k in zip(pts, map(tuple, keys)):
        cell = cells.get(k)
        if cell is None:
            continue
        mu, inv = cell
        d = p - mu
        total -= np.exp(-0.5 * d @ inv @ d)      # negative: we minimise
    return total


def localize(cloud, cells, seed, voxel_m=1.0):
    res = minimize(ndt_score, vec_from_pose(seed), args=(cloud, cells, voxel_m),
                   method="Newton-CG", jac=True, hess=True)
    return pose_from_vec(res.x), res

Points falling in empty cells contribute nothing rather than a penalty, which is what makes NDT tolerant of a live sweep containing things the map does not — a lorry, a pedestrian, roadworks.

4. Derive the covariance from the Hessian #

def pose_covariance(res, floor_m=0.02, inflate=4.0) -> np.ndarray:
    """Covariance from the score Hessian, inflated along weak directions."""
    cov = np.linalg.inv(res.hess)
    w, V = np.linalg.eigh(cov)
    weak = w > (floor_m ** -2)              # small Hessian eigenvalue => large variance
    w[weak] *= inflate
    return V @ np.diag(w) @ V.T

This is the step that makes a tunnel behave correctly. The along-track eigenvalue collapses because every cross-section scores the same, the corresponding variance blows up, and the filter downstream weights the NDT observation accordingly instead of trusting a pose that is sliding.

What the Hessian eigenvalues look like across three scenes:

Verification & Acceptance Criteria #

def assert_ndt_localization(track, truth) -> None:
    lat = np.abs(lateral_errors(track, truth))
    assert np.percentile(lat, 95) <= 0.15, "lateral p95 over budget"

    inside = np.mean([e <= 3.0 * s for e, s in zip(lat, track.sigma_lat)])
    assert inside >= 0.99, "covariance is optimistic"

    tunnel = track.in_tunnel
    assert np.mean(track.sigma_along[tunnel]) > 5.0 * np.mean(track.sigma_along[~tunnel]), \
        "covariance did not grow in the tunnel — observability is not being read"

Acceptance gate: lateral p95 ≤0.15 m; ≥99% of errors inside 3σ; and along-track uncertainty growing by at least fivefold through an unobservable stretch, which is the check that proves the Hessian is being used rather than a constant.

What the voxel size trades, which is the one parameter worth sweeping on your own map:

Common Errors & Fixes #

Localization is fine and the frame budget is missed. Voxel distributions are being built per frame. Precompute them with the tile.

The pose slides in a tunnel and the covariance does not move. A fixed covariance is being reported. Derive it from the Hessian and inflate along weak eigenvectors.

Singular covariance in road-surface cells. A perfectly planar cell has a rank-2 covariance. Regularize with a small diagonal term, as in step 1.

The optimiser fails to converge after a stop. The seed used the previous pose without odometry propagation, and the vehicle moved. Propagate.

Score is dominated by a nearby lorry. It is not — points in cells the map does not have contribute nothing. If the score is being pulled, the lorry is being scored against cells that describe the road behind it; raise min_pts so sparse cells do not become distributions.

FAQ #

Why precompute the NDT map offline? #

Because the voxel means and covariances depend only on the map, which does not change between frames, and computing them per frame would dominate the cycle. Storing them with the tile turns per-frame localization into a scoring problem over a lookup table, and it also makes the representation compact: a voxel's distribution is nine numbers regardless of how many map points fell inside it, which is often a large compression over the raw cloud.

What does the Hessian tell you about observability? #

Its eigenvalues say how sharply the score falls off in each direction of pose space, so a small eigenvalue means the optimiser cannot tell where it is along that eigenvector. In a tunnel the along-track eigenvalue collapses while the lateral one stays healthy, which is exactly the geometric fact that every cross-section looks alike. Inverting the Hessian gives a covariance that carries that structure, which is why it is worth using rather than a fixed uncertainty.

How does NDT differ from ICP for this job? #

NDT scores a point against a local distribution rather than against a nearest neighbour, so it needs no correspondence search and its score surface is smooth. That makes it more tolerant of a poor initial guess and less sensitive to differing point densities between the live sweep and the map, both of which matter for localization. ICP is more accurate when it converges from a good seed, which is why some stacks run NDT to get close and a short ICP to finish.

Related #

• Benchmarking ICP vs NDT Registration Accuracy — measuring the trade this stage assumes.
• Fusing Odometry and Map Constraints with an EKF — the consumer of the covariance this stage produces.
• Motion-Compensating LiDAR Scans with SLERP — the preprocessing this stage assumes.

Up one level: Localization & Map Matching — the stage this localizer is the metric half of.