> ## Documentation Index > Fetch the complete documentation index at: https://aegean.ai/llms.txt > Use this file to discover all available pages before exploring further. # Correspondence Estimation > Data association, joint compatibility and ICP scan registration. [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/pantelis/eng-ai-agents/blob/main/notebooks/rse/correspondence-estimation/index.ipynb) In the [occupancy grid](https://aegean.ai/aiml-common/lectures/rse/occupancy-mapping/index.md) 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](https://aegean.ai/aiml-common/lectures/rse/ekf-localization/index.md) 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. ```python theme={null} %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. ```python theme={null} 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. ```python theme={null} 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) ``` ```output theme={null} 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. ```python theme={null} 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) ``` ```output theme={null} [[ 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] ``` ![Output from cell 5](https://mintcdn.com/aegeanaiinc/PgnPxOCukALFuZaK/aiml-common/lectures/rse/correspondence-estimation/images/cell_5_output_1.svg?fit=max\&auto=format\&n=PgnPxOCukALFuZaK\&q=85\&s=8ac278a32c74aae32a810b4edb8ed486) 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. ```python theme={null} 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)) ``` ```output theme={null} 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: 1. Transform the new scan with the current motion estimate. 2. Pair each point with its nearest point in the reference scan. 3. 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. 4. 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. ```python theme={null} 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") ``` ```output theme={null} reference scan: 180 points, new scan: 180 points ``` ```python theme={null} 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") ``` ```output theme={null} 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 ``` ![Output from cell 9](https://mintcdn.com/aegeanaiinc/PgnPxOCukALFuZaK/aiml-common/lectures/rse/correspondence-estimation/images/cell_9_output_1.svg?fit=max\&auto=format\&n=PgnPxOCukALFuZaK\&q=85\&s=e72986e445f81280366445dbf1cb72c2) 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](https://aegean.ai/aiml-common/lectures/3d-reconstruction/homographies/index.md). 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](https://aegean.ai/aiml-common/lectures/3d-reconstruction/stereo-vision/index.md) uses matching for depth. ## References * Besl, P. J., and McKay, N. D. (1992). [A method for registration of 3-D shapes](https://doi.org/10.1109/34.121791). 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](https://doi.org/10.1109/70.938382). 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](https://doi.org/10.1109/IROS.2018.8593953). 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](https://doi.org/10.1109/70.976019). 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](https://visionbook.mit.edu/multiview.html). MIT Press. * Zhang, J., and Singh, S. (2014). [LOAM: Lidar odometry and mapping in real-time](https://doi.org/10.15607/RSS.2014.X.007). Robotics: Science and Systems. *** Found an error on this page? [Report an issue](https://github.com/aegean-ai/eaia-feedback/issues/new?template=page-issue.yml\&page=/aiml-common/lectures/rse/correspondence-estimation). This documentation is built and hosted on [Mintlify](https://mintlify.com), a developer documentation platform.