Correspondence Estimation

In the occupancy grid section, scans placed at odometry poses produced a distorted map. Estimating the poses from the measurements requires knowing which part of the map each measurement comes from. EKF localization assumed this was known: the correspondence variable

c^i_t

was given, for example by a fiducial marker ID. A laser scanner or a camera does not provide it. Correspondence estimation assigns each measurement to a part of the map, or marks it as new. The filter then uses the assignment as if it were correct. A wrong assignment moves the pose estimate, and in SLAM it also moves the wrong landmark. Thrun, Burgard and Fox (2005, Section 7.6) note that a filter that keeps only the most likely correspondence tends to diverge when that correspondence is wrong. The form of the problem depends on the map:

Landmark maps: assign each detection to a landmark. This is data association, and it applies to any sensor that detects landmarks.
Point maps, as in lidar scan matching: pair points between two scans.
Image feature maps: match keypoints by appearance.

%config InlineBackend.figure_formats = ["svg"]

import matplotlib.pyplot as plt
import numpy as np
from cycler import cycler

plt.rcParams.update(
    {
        "figure.figsize": (6.5, 3.5),  # matches the 6.5 in text column
        "font.size": 9,
        "axes.titlesize": 10,
        "axes.labelsize": 9,
        "xtick.labelsize": 8,
        "ytick.labelsize": 8,
        "legend.fontsize": 8,
        "axes.prop_cycle": cycler(
            color=[
                "#0072B2",
                "#D55E00",
                "#009E73",
                "#cb77a6",
                "#c28600",
                "#1d99e0",
                "#7f7f7f",
            ]
        ),
        "axes.grid": True,
        "grid.alpha": 0.3,
        "svg.fonttype": "path",
    }
)

# named colors from the palette above
BLUE, VERMILLION, GREEN, PINK, GOLD, SKY, GRAY = (
    "#0072B2",
    "#D55E00",
    "#009E73",
    "#cb77a6",
    "#c28600",
    "#1d99e0",
    "#7f7f7f",
)

np.random.seed(5)

Landmark data association

Maximum likelihood correspondence

The robot has a predicted pose belief

\mathcal{N}(\bar\mu_t, \bar\Sigma_t)

and a map of landmarks

m_k

. For a measurement

z^i_t

and landmark

k

, the predicted measurement is

\hat z^k_t = h(\bar\mu_t, k, m)

, as on the EKF localization page. If

z^i_t

came from landmark

k

, the innovation

z^i_t - \hat z^k_t

is approximately Gaussian with zero mean and covariance

\Psi_k = H^k_t \, \bar\Sigma_t \, H^{k\top}_t + Q_t.

The first term is the pose uncertainty mapped into measurement space and

Q_t

is the sensor noise. The maximum likelihood correspondence is the landmark under which the measurement is most likely:

\hat c^i_t = \arg\max_k \; \det(2\pi\Psi_k)^{-1/2} \exp\!\left(-\tfrac{1}{2} D^2_{ik}\right), \qquad D^2_{ik} = (z^i_t - \hat z^k_t)^\top \Psi_k^{-1} (z^i_t - \hat z^k_t).

D^2_{ik}

is the Mahalanobis distance. Taking

-2\ln

of the likelihood and dropping the constant gives the equivalent rule

\hat c^i_t = \arg\min_k \; \left( D^2_{ik} + \ln\det\Psi_k \right).

The determinant term matters because

\Psi_k

differs from landmark to landmark: the Jacobian

H^k_t

depends on where the landmark is. A landmark with a large

\Psi_k

gets a small Mahalanobis distance for almost any measurement, and

\ln\det\Psi_k

charges it for that spread. Thrun, Burgard and Fox (2005, Section 7.6.2) derive the likelihood by integrating the measurement model over the predicted belief. Each measurement is decided separately, which assumes the measurements are conditionally independent given the pose.

Gating

The minimum always names a landmark, also for a measurement of something that is not in the map. A gate rejects these. For a correct correspondence,

D^2_{ik}

is chi-squared distributed with two degrees of freedom (range and bearing). A measurement is accepted for landmark

k

only if

D^2_{ik}

is below the 99th percentile of

\chi^2_2

, which is 9.21. A measurement that passes no gate is an outlier or a new landmark. EKF SLAM starts a new landmark when the distance to every landmark in the map exceeds a threshold

\alpha

(Thrun, Burgard and Fox, 2005, Section 10.3.1). Spurious measurements would then create false landmarks, so practical systems keep new landmarks on a provisional list until they have been seen several times (Section 10.3.3). The code defines a map of six landmarks and the range-bearing model, and computes

D^2_{ik}

for four measurements.

def wrap(angle):
    """Wrap an angle to (-pi, pi]."""
    return (angle + np.pi) % (2 * np.pi) - np.pi


def h(s, landmark):
    """Predicted range and bearing from pose s = (x, y, theta) to a landmark."""
    dx, dy = landmark[0] - s[0], landmark[1] - s[1]
    return np.array([np.hypot(dx, dy), wrap(np.arctan2(dy, dx) - s[2])])


def H_jac(s, landmark):
    """Jacobian of h with respect to the pose."""
    dx, dy = landmark[0] - s[0], landmark[1] - s[1]
    q = dx * dx + dy * dy
    sq = np.sqrt(q)
    return np.array([[-sq * dx, -sq * dy, 0.0], [dy, -dx, -q]]) / q


def innovation(z, s, landmark):
    nu = z - h(s, landmark)
    nu[1] = wrap(nu[1])
    return nu


landmarks = np.array([[4.0, 1.0], [4.0, 2.2], [6.0, -1.5], [3.0, -3.0], [7.5, 2.5], [1.0, 4.0]])
Q = np.diag([0.05**2, np.deg2rad(1.0) ** 2])  # range and bearing noise
GATE = 9.21  # 99th percentile of chi-squared with 2 degrees of freedom

s_true = np.array([0.0, 0.0, 0.0])
seen = [0, 1, 2, 3]  # landmarks in view; the estimator does not know this
z = [h(s_true, landmarks[j]) + np.random.multivariate_normal([0, 0], Q) for j in seen]


def distance_matrix(z, mu_bar, Sigma_bar):
    """Mahalanobis distances D2[i, k] and log determinants logdet[k] of the innovation covariances."""
    D2 = np.zeros((len(z), len(landmarks)))
    logdet = np.zeros(len(landmarks))
    for k, lm in enumerate(landmarks):
        Hk = H_jac(mu_bar, lm)
        Psi = Hk @ Sigma_bar @ Hk.T + Q
        logdet[k] = np.linalg.slogdet(Psi)[1]
        for i, zi in enumerate(z):
            nu = innovation(zi, mu_bar, lm)
            D2[i, k] = nu @ np.linalg.solve(Psi, nu)
    return D2, logdet


def ml_with_gate(D2, logdet):
    """Most likely landmark among those that pass the gate, or -1 if none passes."""
    score = np.where(D2 < GATE, D2 + logdet, np.inf)  # -2 ln likelihood, up to a constant
    best = score.argmin(axis=1)
    return np.where(np.isfinite(score.min(axis=1)), best, -1)

Case 1: the predicted heading is off by 2 degrees, and the covariance reflects that. The landmarks are far apart compared to the uncertainty.

mu_good = np.array([0.05, -0.05, np.deg2rad(2.0)])
Sigma_good = np.diag([0.1**2, 0.1**2, np.deg2rad(3.0) ** 2])

D2_good, logdet_good = distance_matrix(z, mu_good, Sigma_good)
np.set_printoptions(precision=1, suppress=True, linewidth=120)
print("D^2, rows are measurements, columns are landmarks:")
print(D2_good)
print("assigned landmarks:", ml_with_gate(D2_good, logdet_good), " true landmarks:", seen)

D^2, rows are measurements, columns are landmarks:
[[   0.3   29.7  392.8  310.1 1113.5  313.7]
 [  49.3    1.7  345.4  501.5  825.8  201.9]
 [ 407.5  355.2    1.3  426.3  308.7  969.7]
 [ 286.8  472.8  379.2    0.2 1452.5 1218.4]]
assigned landmarks: [0 1 2 3]  true landmarks: [0, 1, 2, 3]

Each row has one entry below the gate, at the true landmark. The next smallest entry in any row is 29.7. All four measurements are assigned correctly. Case 2: the predicted heading is off by 15 degrees. The covariance is consistent with this error, with standard deviations of 10 degrees in heading and 0.5 m in position. Seen from the robot, landmarks 0 and 1 are about 15 degrees apart.

mu_bad = np.array([0.0, 0.0, np.deg2rad(15.0)])
Sigma_bad = np.diag([0.5**2, 0.5**2, np.deg2rad(10.0) ** 2])

D2_bad, logdet_bad = distance_matrix(z, mu_bad, Sigma_bad)
print(D2_bad)
print("ln det Psi_k:", logdet_bad)
ml_bad = ml_with_gate(D2_bad, logdet_bad)
print("assigned landmarks:", ml_bad, " true landmarks:", seen)

[[ 1.4  0.7 31.4 37.1 56.9 15. ]
 [ 7.1  1.6 36.  54.3 46.6  8.3]
 [17.8 15.4  2.2 30.4 13.9 53. ]
 [13.4 25.7 17.9  1.4 75.5 76.1]]
ln det Psi_k: [-4.5 -4.5 -4.7 -4.5 -4.7 -4.5]
assigned landmarks: [1 1 2 3]  true landmarks: [0, 1, 2, 3]

Blue dots are the measurements placed in the world from the predicted pose. Lines join each measurement to its assigned landmark, green if correct and red if wrong. With the 15 degree error, all measurements are rotated about the robot by the same angle. The measurement of landmark 0 falls near landmark 1 and is assigned to it, so landmark 1 gets two measurements. The larger covariance also lowers every distance. For the first measurement both landmark 0 (1.4) and landmark 1 (0.7) pass the gate, so the gate does not catch the error. The six values of

\ln\det\Psi_k

lie between

-4.7

and

-4.5

, so the determinant term barely changes the scores, and the likelihood also picks landmark 1.

Joint compatibility

The measurements share one pose error. A heading error rotates all of them by the same angle, so a set of assignments is plausible only if one pose explains all of it. The joint compatibility test checks this. For a hypothesis that pairs measurements

i_1, \ldots, i_n

with landmarks

k_1, \ldots, k_n

, stack the innovations and Jacobians:

\nu = \begin{bmatrix} z^{i_1}_t - \hat z^{k_1}_t \\ \vdots \\ z^{i_n}_t - \hat z^{k_n}_t \end{bmatrix}, \qquad H = \begin{bmatrix} H^{k_1}_t \\ \vdots \\ H^{k_n}_t \end{bmatrix}, \qquad S = H \bar\Sigma_t H^\top + \operatorname{diag}(Q_t, \ldots, Q_t).

The off-diagonal blocks of

H \bar\Sigma_t H^\top

couple the measurements through the shared pose. The hypothesis is jointly compatible if

\nu^\top S^{-1} \nu

is below the

\chi^2

threshold for

2n

degrees of freedom. Neira and Tardós (2001) find the largest jointly compatible hypothesis with joint compatibility branch and bound (JCBB). With four measurements, the code enumerates all hypotheses instead. A measurement may stay unassigned. The code keeps the jointly compatible hypothesis with the most pairings and breaks ties by the smaller joint distance.

import itertools

from scipy.stats import chi2


def joint_distance(pairs, mu_bar, Sigma_bar):
    nu = np.concatenate([innovation(z[i], mu_bar, landmarks[k]) for i, k in pairs])
    H = np.vstack([H_jac(mu_bar, landmarks[k]) for _, k in pairs])
    S = H @ Sigma_bar @ H.T + np.kron(np.eye(len(pairs)), Q)
    return nu @ np.linalg.solve(S, nu)


def joint_compatibility(mu_bar, Sigma_bar):
    best_key, best_hyp = None, None
    for hyp in itertools.product(range(-1, len(landmarks)), repeat=len(z)):
        pairs = [(i, k) for i, k in enumerate(hyp) if k >= 0]
        used = [k for _, k in pairs]
        if not pairs or len(set(used)) < len(used):  # each landmark at most once
            continue
        d2 = joint_distance(pairs, mu_bar, Sigma_bar)
        if d2 > chi2.ppf(0.99, 2 * len(pairs)):
            continue
        key = (len(pairs), -d2)
        if best_key is None or key > best_key:
            best_key, best_hyp = key, hyp
    return np.array(best_hyp), -best_key[1]


jc_bad, jc_d2 = joint_compatibility(mu_bad, Sigma_bad)
print("joint compatibility:", jc_bad, f" joint D^2 = {jc_d2:.1f}")
print("individual ML:      ", ml_bad)
print("true landmarks:     ", np.array(seen))

joint compatibility: [0 1 2 3]  joint D^2 = 6.9
individual ML:       [1 1 2 3]
true landmarks:      [0 1 2 3]

Joint compatibility assigns all four measurements correctly, with a joint distance of 6.9 against a threshold of 20.1 for 8 degrees of freedom. The individual assignment uses landmark 1 twice and is not a valid hypothesis. Enumeration grows as

(K+1)^n

for

n

measurements and

K

landmarks; JCBB prunes this search.

Scan registration with ICP

A lidar map can also be a set of raw points. The motion between two scans is then estimated by aligning them, which again needs correspondences: which point in one scan is the same surface point as a given point in the other. The scans do not provide this, and from two poses the beams hit the walls at different points anyway. The iterative closest point algorithm (ICP) of Besl and McKay (1992) repeats four steps:

Transform the new scan with the current motion estimate.
Pair each point with its nearest point in the reference scan.
Compute the rotation and translation that minimize the sum of squared distances between the pairs. For point pairs, the solution comes in closed form from the singular value decomposition of their cross-covariance matrix.
Update the estimate. Stop when it no longer changes.

The correspondences are recomputed in every iteration from the current alignment. From a good initial estimate most nearest neighbors are correct pairs and the alignment improves. From a poor one most pairs are wrong, and ICP can converge to a wrong alignment, a local minimum of the error. The initial estimate usually comes from odometry or an IMU. Variants of ICP measure a different error between pairs, such as the distance from a point to the local surface, but form the pairs the same way. The code simulates a 2D lidar in a room with a box and a partition, takes two scans 1.2 m and 20 degrees apart, and aligns the second scan to the first.

walls = np.array(
    [
        [[0, 0], [10, 0]], [[10, 0], [10, 6]], [[10, 6], [0, 6]], [[0, 6], [0, 0]],
        [[6, 2], [7.5, 2]], [[7.5, 2], [7.5, 3.5]], [[7.5, 3.5], [6, 3.5]], [[6, 3.5], [6, 2]],
        [[3, 6], [3, 4.2]],
    ],
    dtype=float,
)


def lidar_scan(pose, n_beams=180, max_range=12.0, sigma=0.01):
    """Ray-cast the walls from pose; return the hit points in the sensor frame."""
    x, y, th = pose
    points = []
    for angle in np.linspace(-np.pi, np.pi, n_beams, endpoint=False):
        d = np.array([np.cos(th + angle), np.sin(th + angle)])
        nearest = max_range
        for p, q in walls:
            A = np.array([d, p - q]).T
            if abs(np.linalg.det(A)) < 1e-12:
                continue
            t, u = np.linalg.solve(A, p - np.array([x, y]))
            if t > 0 and 0 <= u <= 1:
                nearest = min(nearest, t)
        if nearest < max_range:
            r = nearest + np.random.normal(0, sigma)
            points.append([r * np.cos(angle), r * np.sin(angle)])
    return np.array(points)


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


def to_world(pose, points):
    return points @ rotation(pose[2]).T + pose[:2]


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

reference scan: 180 points, new scan: 180 points

def best_rigid_fit(A, B):
    """Rotation R and translation t minimizing the squared distances between R a + t and b."""
    ca, cb = A.mean(axis=0), B.mean(axis=0)
    U, _, Vt = np.linalg.svd((A - ca).T @ (B - cb))
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:  # a reflection, not a rotation
        Vt[1] *= -1
        R = Vt.T @ U.T
    return R, cb - R @ ca


def icp(source, target, initial_pose, iterations=40):
    R, t = rotation(initial_pose[2]), initial_pose[:2].astype(float)
    mean_error = []
    for _ in range(iterations):
        moved = source @ R.T + t
        sq_dist = ((moved[:, None, :] - target[None, :, :]) ** 2).sum(axis=-1)
        nearest = sq_dist.argmin(axis=1)  # the correspondence guess
        mean_error.append(np.sqrt(sq_dist.min(axis=1)).mean())
        dR, dt = best_rigid_fit(moved, target[nearest])
        R, t = dR @ R, dR @ t + dt
    return np.array([t[0], t[1], np.arctan2(R[1, 0], R[0, 0])]), mean_error


starts = {
    "odometry guess": pose_new + np.array([0.3, -0.2, np.deg2rad(8.0)]),
    "70 degree heading error": pose_new + np.array([0.0, 0.0, np.deg2rad(70.0)]),
}
results = {}
for name, start in starts.items():
    estimate, errors = icp(new_scan, reference, start)
    results[name] = (start, estimate, errors)
    print(
        f"{name:<24s} estimate x={estimate[0]:.2f} y={estimate[1]:.2f} theta={np.rad2deg(estimate[2]):6.1f} deg"
        f"   mean pair distance {errors[0]:.2f} m -> {errors[-1]:.2f} m"
    )
print(f"{'true pose':<24s} x={pose_new[0]:.2f} y={pose_new[1]:.2f} theta={np.rad2deg(pose_new[2]):6.1f} deg")

odometry guess           estimate x=2.99 y=2.60 theta=  20.2 deg   mean pair distance 0.29 m -> 0.06 m
70 degree heading error  estimate x=3.39 y=2.06 theta= 111.0 deg   mean pair distance 0.74 m -> 0.67 m
true pose                x=3.00 y=2.60 theta=  20.0 deg

From the odometry guess, 0.36 m and 8 degrees off, ICP ends within 0.01 m and 0.2 degrees of the true pose. The mean distance between paired points drops from 0.29 m to 0.06 m. It stays above zero because the two scans sample the walls at different points. From the 70 degree heading error, ICP stops at 111 degrees, 91 degrees from the true heading, with a mean pair distance of 0.67 m. A large residual like this points to a failed registration. In a room with repeated structure, a wrong alignment can also have a small residual.

Lidar features and loop closure

LOAM (Zhang and Singh, 2014) does not pair every point. It selects points on sharp edges and points on flat surfaces, using the curvature of the scan line around each point, and matches edge points to edge lines and surface points to planar patches in the map. The features are defined by local geometry. Loop closure is a correspondence at the scale of a place. A robot that returns to a mapped area has to recognize it so that the loop in the map can be closed. Scan Context (Kim and Kim, 2018) summarizes a lidar scan in a descriptor that is compared with stored scans. A false loop closure merges two different places and can distort the whole map. Landmark data association also applies to lidar. The Victoria Park dataset of Guivant and Nebot (2001), recorded from a vehicle in a park in Sydney, uses tree trunks detected in laser scans as landmarks. It is widely used to compare landmark-based SLAM algorithms.

Image feature matching

A detector finds keypoints, such as corners, that are likely to be detected again from another viewpoint. A descriptor encodes the image patch around each keypoint as a vector. Keypoints in two images are matched by descriptor distance, and the matches are checked with a geometric model, for example with RANSAC as in the homographies section. The descriptor has the role of the signature

s

in the landmark measurement

z = (r, \phi, s)

of Probabilistic Robotics. Distinct signatures make landmarks easier to tell apart. Torralba, Isola and Freeman (2024, Section 44.3.3) cover hand-designed detectors and descriptors such as SIFT and ORB, and learned ones such as SuperPoint with the SuperGlue matcher. The stereo vision section uses matching for depth.

References

Besl, P. J., and McKay, N. D. (1992). A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 14(2), 239-256.
Guivant, J. E., and Nebot, E. M. (2001). Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Transactions on Robotics and Automation, 17(3), 242-257.
Kim, G., and Kim, A. (2018). Scan Context: Egocentric spatial descriptor for place recognition within 3D point cloud map. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 4802-4809.
Neira, J., and Tardós, J. D. (2001). Data association in stochastic mapping using the joint compatibility test. IEEE Transactions on Robotics and Automation, 17(6), 890-897.
Thrun, S., Burgard, W., and Fox, D. (2005). Probabilistic Robotics. MIT Press.
Torralba, A., Isola, P., and Freeman, W. T. (2024). Foundations of Computer Vision. MIT Press.
Zhang, J., and Singh, S. (2014). LOAM: Lidar odometry and mapping in real-time. Robotics: Science and Systems.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Correspondence Estimation

Landmark data association

Maximum likelihood correspondence

Gating

Joint compatibility

Scan registration with ICP

Lidar features and loop closure

Image feature matching

References

​Landmark data association

​Maximum likelihood correspondence

​Gating

​Joint compatibility

​Scan registration with ICP

​Lidar features and loop closure

​Image feature matching

​References

Landmark data association

Maximum likelihood correspondence

Gating

Joint compatibility

Scan registration with ICP

Lidar features and loop closure

Image feature matching

References