Scan Registration with ICP

A LIDAR scan is a set of points on the surfaces around the robot. Two scans taken from nearby poses see the same walls, and the motion between the poses is the rigid transform that lays one scan on top of the other. Finding that transform is scan registration. It gives a motion estimate from the LIDAR alone, which LIDAR odometry chains from scan to scan, and it gives the relative pose between two visits to the same place, which a pose graph uses as a loop closure. The standard method is the iterative closest point algorithm (ICP) of Besl and McKay (1992). As the correspondence estimation section explains, its difficulty is the pairing: the scans do not say which point in one is the same surface point as a point in the other. ICP guesses the pairs from the current alignment, fits the transform to the guessed pairs, and repeats. The section builds ICP in steps. It starts with the fit for known pairs, which has a closed-form solution, and then adds the iteration over guessed pairs. It writes the same fit as a least-squares problem solved by Gauss-Newton, which allows two improvements: measuring the error to the local wall instead of to a single point, and weighting down pairs that do not belong to the same surface.

%config InlineBackend.figure_formats = ["svg"]

import matplotlib.pyplot as plt
import numpy as np

plt.rcParams.update({"figure.figsize": (6.5, 3.5), "font.size": 9, "axes.titlesize": 10, "legend.fontsize": 8,
                     "svg.fonttype": "path", "axes.grid": False})
BLUE, VERMILLION, GREEN, PINK, GOLD, GRAY = "#0072B2", "#D55E00", "#009E73", "#cb77a6", "#c28600", "#7f7f7f"

Two scans of a room

The code simulates a 2D LIDAR with 180 beams and 1 cm range noise in a 10 m by 6 m room with a box and a partition. The reference scan is taken at

(2, 2)

m with heading 0. The new scan is taken 1.2 m away and turned by 20°. Each scan is returned in the frame of the sensor that took it. A person stands in the room for the new scan only; the code leaves it out until the part on outliers.

WALLS = np.array([
    [[0, 0], [10, 0]], [[10, 0], [10, 6]], [[10, 6], [0, 6]], [[0, 6], [0, 0]],  # room
    [[6, 2], [7.5, 2]], [[7.5, 2], [7.5, 3.5]], [[7.5, 3.5], [6, 3.5]], [[6, 3.5], [6, 2]],  # box
    [[3, 6], [3, 4.2]],  # partition
], dtype=float)
PERSON = (np.array([4.6, 3.6]), 0.25)  # center and radius of a person, present in the new scan only


def rotation(theta):
    return np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])


def lidar_scan(pose, rng, n_beams=180, max_range=12.0, sigma=0.01, person=False):
    """Hit points in the sensor frame, from ray casting against the walls (and the person)."""
    angles = np.linspace(-np.pi, np.pi, n_beams, endpoint=False)
    d = np.column_stack([np.cos(pose[2] + angles), np.sin(pose[2] + angles)])
    o = pose[:2]
    r = np.full(n_beams, max_range)
    for p, q in WALLS:  # ray-segment intersection for every beam at once
        e = q - p
        den = d[:, 0] * e[1] - d[:, 1] * e[0]
        w = p - o
        with np.errstate(divide="ignore", invalid="ignore"):
            t = (w[0] * e[1] - w[1] * e[0]) / den
            u = (w[0] * d[:, 1] - w[1] * d[:, 0]) / den
        r = np.where((np.abs(den) > 1e-12) & (t > 0) & (u >= 0) & (u <= 1), np.minimum(r, t), r)
    if person:  # ray-circle intersection
        c, rad = PERSON
        f = o - c
        b = d @ f
        disc = b**2 - (f @ f - rad**2)
        t = -b - np.sqrt(np.maximum(disc, 0))
        r = np.where((disc > 0) & (t > 0), np.minimum(r, t), r)
    keep = r < max_range
    r = r[keep] + rng.normal(0, sigma, keep.sum())
    return np.column_stack([r * np.cos(angles[keep]), r * np.sin(angles[keep])])


def transform(pose, points):
    """Points from the frame of `pose` into the world frame."""
    return points @ rotation(pose[2]).T + pose[:2]


rng = np.random.default_rng(0)
pose_ref = np.array([2.0, 2.0, 0.0])
pose_new = np.array([3.0, 2.6, np.deg2rad(20.0)])
reference = transform(pose_ref, lidar_scan(pose_ref, rng))  # the map: first scan, in the world frame
new_scan = lidar_scan(pose_new, rng)  # second scan, in its own frame
print(f"reference: {len(reference)} points, new scan: {len(new_scan)} points")

reference: 180 points, new scan: 180 points

The fit for known pairs

Suppose the pairs were known: point

p_i

of the new scan is the same surface point as

q_i

of the reference, for

i = 1, \ldots, n

. The rotation

R

and translation

t

that align them minimize

E(R, t) = \sum_{i=1}^{n} \lVert R p_i + t - q_i \rVert^2 .

The minimum has a closed form (Arun, Huang and Blostein, 1987). Setting the derivative with respect to

t

to zero gives

t = \bar q - R \bar p

, where

\bar p

and

\bar q

are the centroids of the two point sets. The best translation moves the rotated centroid of one set onto the centroid of the other. Substituting it back, with centered points

p_i' = p_i - \bar p

and

q_i' = q_i - \bar q

, leaves

E(R) = \sum_i \lVert R p_i' - q_i' \rVert^2 = \sum_i \left( \lVert p_i' \rVert^2 + \lVert q_i' \rVert^2 \right) - 2 \sum_i q_i'^\top R\, p_i' ,

because a rotation does not change lengths. Minimizing

E

is the same as maximizing

\sum_i q_i'^\top R\, p_i' = \operatorname{tr}(R H), \qquad H = \sum_i p_i' q_i'^\top ,

where

H

is the

2 \times 2

cross-covariance of the centered pairs. Write its singular value decomposition

H = U \Sigma V^\top

. Then

\operatorname{tr}(RH) = \operatorname{tr}(\Sigma\, V^\top R U)

. The matrix

V^\top R U

is orthogonal, so its diagonal entries are at most 1, and the trace is largest when

V^\top R U = I

R = V U^\top, \qquad t = \bar q - R \bar p .

If the points are nearly collinear or very noisy,

V U^\top

can come out as a reflection, with determinant

-1

. The rotation closest to it is obtained by flipping the sign of the last column of

V

. The code applies the fit to the new scan with the true pairs, which the simulation knows because both scans can be cast from the same beams. It recovers the pose up to the noise.

def best_rigid_fit(P, Q):
    """R, t minimizing sum ||R p_i + t - q_i||^2 for paired rows of P and Q."""
    p_bar, q_bar = P.mean(axis=0), Q.mean(axis=0)
    H = (P - p_bar).T @ (Q - q_bar)
    U, _, Vt = np.linalg.svd(H)
    V = Vt.T
    if np.linalg.det(V @ U.T) < 0:  # a reflection: flip the last column of V
        V[:, -1] *= -1
    R = V @ U.T
    return R, q_bar - R @ p_bar


# True pairs: the same beams, cast from the new pose, without noise, give the surface points p_i are measuring.
P_true = lidar_scan(pose_new, np.random.default_rng(1), sigma=0.0)
P_noisy = P_true + np.random.default_rng(2).normal(0, 0.01, P_true.shape)
R, t = best_rigid_fit(P_noisy, transform(pose_new, P_true))
print(f"recovered x={t[0]:.3f} y={t[1]:.3f} theta={np.rad2deg(np.arctan2(R[1, 0], R[0, 0])):.2f} deg, "
      f"true x={pose_new[0]:.3f} y={pose_new[1]:.3f} theta={np.rad2deg(pose_new[2]):.2f} deg")

recovered x=2.999 y=2.601 theta=19.99 deg, true x=3.000 y=2.600 theta=20.00 deg

Iterating over guessed pairs

In practice the pairs are unknown, and the two scans do not even hit the walls at the same points. ICP replaces the true pairs with a guess and refines the guess:

Transform the new scan with the current estimate of $(R, t)$ .
Pair each transformed point with its nearest point in the reference.
Fit $(R, t)$ to these pairs with the closed form above.
Repeat until the estimate stops changing.

Each iteration cannot increase the error. Step 3 lowers it for fixed pairs, and step 2 lowers it for a fixed transform, because every point moves to a partner at least as close. The error therefore converges, but only to a local minimum, and which one depends on the starting estimate. In a robot the start comes from odometry or an IMU. The code starts from an odometry guess that is 0.36 m and 8° off and records the pairs of every iteration.

def nearest(moved, target):
    """Index of the nearest target point for each moved point, and the distance to it."""
    d2 = ((moved[:, None, :] - target[None, :, :]) ** 2).sum(axis=-1)
    j = d2.argmin(axis=1)
    return j, np.sqrt(d2[np.arange(len(moved)), j])


def pose_of(R, t):
    return np.array([t[0], t[1], np.arctan2(R[1, 0], R[0, 0])])


def icp_point_to_point(source, target, start, iterations=30, tol=1e-6):
    """ICP with the closed-form fit. Returns the pose after each iteration and the pairs used in it."""
    R, t = rotation(start[2]), start[:2].astype(float)
    history = [(pose_of(R, t), None, None)]
    for _ in range(iterations):
        moved = source @ R.T + t
        j, dist = nearest(moved, target)
        dR, dt = best_rigid_fit(moved, target[j])
        R, t = dR @ R, dR @ t + dt
        history.append((pose_of(R, t), j, dist.mean()))
        if np.linalg.norm(dt) < tol and abs(np.arctan2(dR[1, 0], dR[0, 0])) < tol:
            break
    return history


start = pose_new + np.array([0.3, -0.2, np.deg2rad(8.0)])
hist_pp = icp_point_to_point(new_scan, reference, start)
for k, (pose, _, err) in enumerate(hist_pp[:6]):
    print(f"iteration {k}: x={pose[0]:.3f} y={pose[1]:.3f} theta={np.rad2deg(pose[2]):6.2f} deg"
          + (f"  mean pair distance {err:.3f} m" if err is not None else ""))
print(f"stopped after {len(hist_pp) - 1} iterations at {hist_pp[-1][0].round(3)}; true pose {pose_new.round(3)}")

iteration 0: x=3.300 y=2.400 theta= 28.00 deg
iteration 1: x=3.209 y=2.445 theta= 26.40 deg  mean pair distance 0.285 m
iteration 2: x=3.142 y=2.475 theta= 25.04 deg  mean pair distance 0.227 m
iteration 3: x=3.094 y=2.508 theta= 23.91 deg  mean pair distance 0.184 m
iteration 4: x=3.062 y=2.533 theta= 22.92 deg  mean pair distance 0.148 m
iteration 5: x=3.042 y=2.547 theta= 22.17 deg  mean pair distance 0.119 m
stopped after 16 iterations at [2.99  2.604 0.352]; true pose [3.    2.6   0.349]

Four panels of the room: the reference scan in blue on the walls and the new scan in orange, with green lines joining each new point to its nearest reference point. At iteration 0 the new scan is rotated and shifted and many lines are long; by iteration 3 it lies almost on the walls; at the final iteration the two scans overlap.

ICP from the odometry guess. Green lines join every second point of the new scan (orange) to its nearest point in the reference scan (blue); these are the pairs the next fit uses. The walls of the map are drawn in gray for orientation and are not used. In the first iterations many pairs join points that are not the same surface point, some of them across a corner. Most still point roughly the right way, so each fit moves the scan closer, and the mean pair distance falls from 0.29 m to 0.12 m in five iterations. After 16 iterations the estimate is within 1.1 cm and 0.16° of the true pose. The mean pair distance stops at 6 cm, not zero, because the two scans hit the walls at different points. The starting estimate decides which minimum ICP reaches. The code repeats the run from the true position with the heading off by 70°.

bad_start = pose_new + np.array([0.0, 0.0, np.deg2rad(70.0)])
hist_bad = icp_point_to_point(new_scan, reference, bad_start, iterations=60)
final = hist_bad[-1]
print(f"from a 70 degree heading error: {len(hist_bad) - 1} iterations, heading {np.rad2deg(final[0][2]):.1f} deg "
      f"(true {np.rad2deg(pose_new[2]):.1f}), position error {np.hypot(*(final[0][:2] - pose_new[:2])):.2f} m, "
      f"mean pair distance {final[2]:.2f} m")
print(f"from the odometry guess: mean pair distance {hist_pp[-1][2]:.3f} m")

from a 70 degree heading error: 50 iterations, heading 111.9 deg (true 20.0), position error 0.68 m, mean pair distance 0.67 m
from the odometry guess: mean pair distance 0.060 m

ICP stops at a heading of 112°, 92° from the true one, and 0.68 m from the true position. The scan has turned by about a quarter turn, so its walls lie along the other pair of walls of the room, and no small change of the pose lowers the error. The mean pair distance, 0.67 m, is more than ten times the 0.06 m of the converged run. A residual this large marks a failed registration. The converse does not hold: in a room with repeated structure, such as a corridor of identical doors, a wrong alignment can also have a small residual.

ICP as least squares

The closed form works only for the squared distance between paired points. Other errors need a general method. Parameterize the pose by

x = (t_x, t_y, \theta)

and stack the residuals

e_i(x) = R(\theta)\, p_i + t - q_i \in \mathbb{R}^2 .

They are linear in

t

and nonlinear in

\theta

. Gauss-Newton linearizes them around the current estimate. The Jacobian of

e_i

with respect to

x

J_i = \begin{bmatrix} I_2 & R'(\theta)\, p_i \end{bmatrix}, \qquad R'(\theta) = \begin{bmatrix} -\sin\theta & -\cos\theta \\ \cos\theta & -\sin\theta \end{bmatrix},

and the update solves the normal equations

\Delta x = -\left( \sum_i J_i^\top W_i J_i \right)^{-1} \sum_i J_i^\top W_i\, e_i , \qquad x \leftarrow x + \Delta x ,

with a weight matrix

W_i

for each pair, the identity for now. For point pairs, one Gauss-Newton step per iteration reaches the same pose as the closed form, here in the same 16 iterations. Its advantage is that any residual with a Jacobian fits the same loop, and the weights

W_i

can differ from pair to pair. The next two parts use both.

def dR(theta):
    return np.array([[-np.sin(theta), -np.cos(theta)], [np.cos(theta), -np.sin(theta)]])


def gauss_newton_step(x, P, Q, normals=None, weights=None):
    """One Gauss-Newton step for point-to-point residuals, or point-to-line residuals when normals are given."""
    R = rotation(x[2])
    e = P @ R.T + x[:2] - Q  # (n, 2) point-to-point residuals
    Jtheta = P @ dR(x[2]).T  # (n, 2) derivative of R p with respect to theta
    if normals is None:
        J = np.zeros((len(P), 2, 3))
        J[:, :, :2] = np.eye(2)
        J[:, :, 2] = Jtheta
        r = e
    else:  # project onto the normal of the local wall
        J = np.column_stack([normals, (normals * Jtheta).sum(axis=1)])[:, None, :]
        r = (normals * e).sum(axis=1)[:, None]
    w = np.ones(len(P)) if weights is None else weights
    A = np.einsum("n,nki,nkj->ij", w, J, J)
    b = np.einsum("n,nki,nk->i", w, J, r)
    return x - np.linalg.solve(A, b)


def icp_gauss_newton(source, target, start, iterations=30, point_to_line=False, robust=None, tol=1e-6):
    """ICP with a Gauss-Newton step per iteration. Returns the pose after each iteration and the mean pair distance."""
    x = start.astype(float)
    history = [(x.copy(), None)]
    normals_all = line_normals(target) if point_to_line else None
    for _ in range(iterations):
        moved = source @ rotation(x[2]).T + x[:2]
        j, dist = nearest(moved, target)
        n = normals_all[j] if point_to_line else None
        if point_to_line:
            res = np.abs((n * (moved - target[j])).sum(axis=1))
        else:
            res = dist
        weights = None if robust is None else robust(res)
        x_new = gauss_newton_step(x, source, target[j], n, weights)
        step = x_new - x
        x = x_new
        history.append((x.copy(), dist.mean()))
        if np.linalg.norm(step) < tol:
            break
    return history


hist_gn = icp_gauss_newton(new_scan, reference, start)
print(f"Gauss-Newton point-to-point: {len(hist_gn) - 1} iterations, pose {hist_gn[-1][0].round(3)}")
print(f"closed-form point-to-point:  {len(hist_pp) - 1} iterations, pose {hist_pp[-1][0].round(3)}")

Gauss-Newton point-to-point: 16 iterations, pose [2.99  2.604 0.352]
closed-form point-to-point:  16 iterations, pose [2.99  2.604 0.352]

Point-to-line error

The two scans sample a wall at different points. Even at the true pose a point of the new scan lies between two reference points, a few centimeters from either, so the point-to-point error never reaches zero, and it pulls the scan along the wall toward the sampled points. The error that matters is the distance across the wall. Chen and Medioni (1992) measured it from each point to the plane through its partner; in 2D the plane is a line. With

n_i

the unit normal of the reference scan at

q_i

, the residual becomes a scalar,

e_i(x) = n_i^\top \left( R(\theta)\, p_i + t - q_i \right), \qquad J_i = n_i^\top \begin{bmatrix} I_2 & R'(\theta)\, p_i \end{bmatrix} .

A point can slide along the wall without cost, and only its distance from the wall counts. The normals come from the reference scan: for each point, fit a line to it and its four nearest neighbors, and take the direction of least spread as the normal.

def line_normals(points, k=5):
    """Unit normal at each point: the direction of least spread of its k nearest neighbors."""
    d2 = ((points[:, None, :] - points[None, :, :]) ** 2).sum(axis=-1)
    idx = np.argsort(d2, axis=1)[:, :k]
    normals = np.empty_like(points)
    for i, nb in enumerate(idx):
        c = points[nb] - points[nb].mean(axis=0)
        _, vecs = np.linalg.eigh(c.T @ c)
        normals[i] = vecs[:, 0]  # eigenvector of the smallest eigenvalue
    return normals


hist_pl = icp_gauss_newton(new_scan, reference, start, point_to_line=True)


def pose_error(pose):
    return np.hypot(*(pose[:2] - pose_new[:2])), abs(np.rad2deg(pose[2] - pose_new[2]))


for name, hist in (("point-to-point", hist_gn), ("point-to-line", hist_pl)):
    pos, ang = pose_error(hist[-1][0])
    print(f"{name:15s} {len(hist) - 1:2d} iterations, position error {100 * pos:.1f} cm, heading error {ang:.2f} deg")

point-to-point  16 iterations, position error 1.1 cm, heading error 0.16 deg
point-to-line    6 iterations, position error 0.8 cm, heading error 0.41 deg

Position error in centimeters against iteration on a log scale for point-to-point and point-to-line ICP, both starting from the same odometry guess.

Position error of the estimate at each iteration, for the point-to-point and point-to-line residuals, from the same odometry guess. Point-to-line ICP reaches its final estimate after three iterations and stops after six. Point-to-point needs 16, and both end within about 1 cm of the true position. Point-to-point closes in slowly because each pair pulls a point toward a sampled point, which can lie along the wall in the wrong direction. Near the end its error even rises again, from 0.5 cm to 1.1 cm, as the pairs change. Point-to-line ignores motion along a wall, so it converges in a few steps as long as the scan contains walls in at least two directions. In this run its final heading error, 0.41°, is larger than the 0.16° of point-to-point. A likely cause is the noise in normals estimated from only five neighbors.

Outliers

Not every point of the new scan has a partner in the reference. A person who walked in between the scans, a door that moved, or a part of the room that only one scan sees all produce points whose nearest neighbor lies on a different surface. Least squares gives each of them a weight that grows with the square of its residual, so a few of them can pull the whole alignment. A robust error replaces the square with a function that grows more slowly for large residuals. In the Gauss-Newton loop this only changes the weights: each pair gets a weight

w_i

computed from its current residual

r_i

, and the weighted problem is solved as before. The Huber weight with threshold

\delta

w_i = \begin{cases} 1 & |r_i| \le \delta \\ \delta / |r_i| & |r_i| > \delta \end{cases}

which keeps the square for small residuals and makes the cost linear beyond

\delta

. A cruder alternative drops every pair with a residual above a threshold. The code repeats the point-to-line run with a new scan that also sees the person, standing between the two poses, once with plain least squares and once with Huber weights and

\delta = 5

cm.

def huber(delta):
    return lambda r: np.where(r <= delta, 1.0, delta / np.maximum(r, 1e-12))


new_with_person = lidar_scan(pose_new, np.random.default_rng(3), person=True)
on_person = np.linalg.norm(transform(pose_new, new_with_person) - PERSON[0], axis=1) < PERSON[1] + 0.05
print(f"new scan: {len(new_with_person)} points, {on_person.sum()} of them on the person")

runs = {"least squares": icp_gauss_newton(new_with_person, reference, start, point_to_line=True),
        "Huber, 5 cm": icp_gauss_newton(new_with_person, reference, start, point_to_line=True, robust=huber(0.05))}
for name, hist in runs.items():
    pos, ang = pose_error(hist[-1][0])
    print(f"{name:14s} {len(hist) - 1:2d} iterations, position error {100 * pos:.1f} cm, heading error {ang:.2f} deg")

new scan: 180 points, 7 of them on the person

least squares   7 iterations, position error 15.2 cm, heading error 0.63 deg
Huber, 5 cm     9 iterations, position error 0.1 cm, heading error 0.16 deg

Two panels of the room after point-to-line ICP with a person present in the new scan only. Points on the person are marked with black crosses. The titles give the final position and heading error for plain least squares and for Huber weights.

The new scan after point-to-line ICP when it also sees a person (pink, crosses mark the points on the person). Left: plain least squares. Right: Huber weights. The titles give the remaining position and heading error. Seven of the 180 points fall on the person. With plain least squares they pair with points on the box about 1.4 m away and drag the scan toward it, and the estimate ends 15.2 cm from the true position. Huber weights cap the influence of each of those pairs, and the estimate ends within 0.1 cm and 0.16°.

ICP in practice

Real implementations add a few things that the code above leaves out. They find nearest neighbors with a k-d tree or a voxel grid instead of comparing every pair of points. They subsample dense scans, often to one point per voxel. They reject pairs farther apart than a threshold that shrinks as the alignment improves, which combines the trimming above with the robust weights. And they check the result: a large final residual, or a Hessian

\sum_i J_i^\top W_i J_i

that is nearly singular, as in a long featureless corridor where the scan can slide along the walls, marks an alignment that should not be trusted. KISS-ICP (Vizzo et al., 2023) is a recent LIDAR odometry system built almost entirely from these parts: point-to-point ICP on a voxel map, a motion prediction from the previous scans as the starting estimate, and a robust kernel whose threshold adapts to how well recent scans have aligned. Not every 2D SLAM system uses ICP. slam_toolbox, the default SLAM package of Nav2, matches scans with a correlative matcher that evaluates a grid of candidate poses against a likelihood map, which does not depend on a good starting estimate in the same way.

References

Arun, K. S., Huang, T. S., and Blostein, S. D. (1987). Least-squares fitting of two 3-D point sets. IEEE Transactions on Pattern Analysis and Machine Intelligence.
Besl, P. J., and McKay, N. D. (1992). A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence.
Chen, Y., and Medioni, G. (1992). Object modelling by registration of multiple range images. Image and Vision Computing.
Vizzo, I., Guadagnino, T., Mersch, B., Wiesmann, L., Behley, J., and Stachniss, C. (2023). KISS-ICP: In defense of point-to-point ICP, simple, accurate, and robust registration if done the right way. IEEE Robotics and Automation Letters.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Scan Registration with ICP

Two scans of a room

The fit for known pairs

Iterating over guessed pairs

ICP as least squares

Point-to-line error

Outliers

ICP in practice

References

​Two scans of a room

​The fit for known pairs

​Iterating over guessed pairs

​ICP as least squares

​Point-to-line error

​Outliers

​ICP in practice

​References

Two scans of a room

The fit for known pairs

Iterating over guessed pairs

ICP as least squares

Point-to-line error

Outliers

ICP in practice

References