In Chapter 10 Lesson 1, we formulated scan registration as estimating a rigid-body transform that aligns two scans/point sets. Here we focus on ICP variants and the engineering decisions that make ICP usable on a mobile robot.

Learning objectives. By the end of this lesson you should be able to:

• Write ICP as a joint optimization over correspondences and rigid motion in \( SE(2) \) or \( SE(3) \).
• Derive the closed-form update for point-to-point ICP (weighted Procrustes / SVD).
• Derive the linearized normal equations for point-to-line (2D LiDAR) and point-to-plane (3D LiDAR).
• Explain why some variants converge faster locally and why some are more stable under corridor-like degeneracies.
• Implement baseline ICP variants and inspect convergence and failure cases.

flowchart TD
  A["Inputs: scan A (source), scan B (reference), initial guess from odometry"] --> B["Initialize pose T0"]
  B --> C["Correspondence search: nearest neighbors or projective matching"]
  C --> D["Reject outliers: distance gate, normal gate, mutual check"]
  D --> E["Choose metric: point-to-point or point-to-line/plane or probabilistic"]
  E --> F["Solve least-squares for delta pose"]
  F --> G["Compose update: Tk+1 = delta * Tk"]
  G --> H["Stop if improvement small or max iterations reached"]
  H --> I["Output: pose estimate and quality statistics"]
  

Let the source scan be a set of points \( \mathcal{P} = \{\mathbf{p}_i\}_{i=1}^N \) and the reference scan \( \mathcal{Q} = \{\mathbf{q}_j\}_{j=1}^M \). A rigid transform is parameterized by \( \mathbf{T}=(\mathbf{R},\mathbf{t}) \), with \( \mathbf{R}\in SO(d) \), \( \mathbf{t}\in\mathbb{R}^d \), and \( d\in\{2,3\} \).

ICP introduces a correspondence map \( c:\{1,\dots,N\}\to\{1,\dots,M\} \) that assigns each source point to a reference point (often nearest neighbor). The canonical point-to-point ICP objective is

\[ E(\mathbf{R},\mathbf{t},c) \;=\; \sum_{i=1}^{N} \left\lVert \mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_{c(i)} \right\rVert_2^2 . \]

ICP performs block coordinate descent (alternating minimization):

\[ c^{(k+1)} \;\in\; \arg\min_{c} E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c), \qquad (\mathbf{R}^{(k+1)},\mathbf{t}^{(k+1)}) \;\in\; \arg\min_{\mathbf{R},\mathbf{t} } E(\mathbf{R},\mathbf{t},c^{(k+1)}). \]

Monotonic descent property (core ICP guarantee). Define \( E^{(k)} = E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c^{(k)}) \). Then, ignoring degeneracies/ties in correspondence selection, \( E^{(k+1)} \le E^{(k)} \). The proof is a two-line inequality chain:

\[ \underbrace{E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c^{(k+1)})}_{\le E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c^{(k)})} \;\ge\; \underbrace{E(\mathbf{R}^{(k+1)},\mathbf{t}^{(k+1)},c^{(k+1)})}_{\le E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c^{(k+1)})}. \]

The first inequality holds because \( c^{(k+1)} \) is chosen to minimize the objective given the current pose; the second holds because \( (\mathbf{R}^{(k+1)},\mathbf{t}^{(k+1)}) \) minimizes the objective given the new correspondences. Therefore, ICP converges to a (generally local) fixed point where neither correspondences nor pose update improves the objective.

Key engineering implication: ICP is local. You must provide a reasonable initial guess (e.g., wheel odometry + IMU yaw integration) or use a global method (next lessons) to seed ICP.

Consider the transform step with correspondences fixed, i.e., pairs \( (\mathbf{p}_i, \mathbf{q}_i) \). We solve the weighted Procrustes problem:

\[ (\mathbf{R}^\star,\mathbf{t}^\star)\;\in\;\arg\min_{\mathbf{R}\in SO(d),\,\mathbf{t}\in\mathbb{R}^d} \sum_{i=1}^{N} w_i \left\lVert \mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i \right\rVert_2^2, \quad w_i \ge 0 . \]

Let weighted centroids be \( \bar{\mathbf{p} } = \frac{1}{W}\sum_i w_i \mathbf{p}_i \), \( \bar{\mathbf{q} } = \frac{1}{W}\sum_i w_i \mathbf{q}_i \), with \( W=\sum_i w_i \). Define centered points \( \mathbf{p}'_i=\mathbf{p}_i-\bar{\mathbf{p} } \), \( \mathbf{q}'_i=\mathbf{q}_i-\bar{\mathbf{q} } \). Then the optimal translation is \( \mathbf{t}^\star = \bar{\mathbf{q} } - \mathbf{R}^\star \bar{\mathbf{p} } \), and the remaining optimization reduces to maximizing a trace:

\[ \mathbf{R}^\star \;\in\; \arg\max_{\mathbf{R}\in SO(d)} \operatorname{tr}\!\left(\mathbf{R}\mathbf{H}\right), \qquad \mathbf{H} \;=\; \sum_{i=1}^{N} w_i\, \mathbf{p}'_i {\mathbf{q}'_i}^{\!\top}. \]

Proof sketch (trace maximization via SVD). Expand the centered objective: \( \sum_i w_i\|\mathbf{R}\mathbf{p}'_i-\mathbf{q}'_i\|^2 = \sum_i w_i\|\mathbf{p}'_i\|^2 + \sum_i w_i\|\mathbf{q}'_i\|^2 - 2\operatorname{tr}(\mathbf{R}\mathbf{H}) \). The first two terms are constant in \( \mathbf{R} \), so minimizing the error is equivalent to maximizing \( \operatorname{tr}(\mathbf{R}\mathbf{H}) \). Take the SVD \( \mathbf{H} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^{\top} \). Then \( \operatorname{tr}(\mathbf{R}\mathbf{H}) = \operatorname{tr}(\mathbf{V}^{\top}\mathbf{R}\mathbf{U}\boldsymbol{\Sigma}) \). Let \( \mathbf{S} = \mathbf{V}^{\top}\mathbf{R}\mathbf{U}\in SO(d) \). Since \( \boldsymbol{\Sigma} \) is diagonal nonnegative, the trace is maximized when \( \mathbf{S}=\mathbf{I} \) except possibly a reflection correction, yielding \( \mathbf{R}^\star = \mathbf{V}\mathbf{D}\mathbf{U}^{\top} \) with \( \mathbf{D}=\operatorname{diag}(1,\dots,1,\det(\mathbf{V}\mathbf{U}^{\top})) \).

Mobile-robot note (2D specialization). In \( d=2 \), the SVD step can be reduced to a single angle: if \( \mathbf{H}=\begin{bmatrix}h_{11}&h_{12}\\h_{21}&h_{22}\end{bmatrix} \), then the optimal rotation angle is \( \theta^\star = \operatorname{atan2}(h_{21}-h_{12},\,h_{11}+h_{22}) \).

Point-to-point ICP is easy but can converge slowly when surfaces are approximately planar: moving along the tangent direction barely changes the objective. The remedy is to measure error along the surface normal. In 3D this yields point-to-plane ICP; in 2D LiDAR scan matching it becomes point-to-line ICP.

Assume each correspondence includes a unit normal \( \mathbf{n}_i \) at the reference point. The point-to-plane/line objective is

\[ E_{\perp}(\mathbf{R},\mathbf{t}) \;=\; \sum_{i=1}^{N} w_i \left( \mathbf{n}_i^{\top}\left(\mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i\right) \right)^2 . \]

The cost is nonlinear because of \( \mathbf{R} \). ICP variants solve it by linearizing around the current estimate. Let the incremental motion be parameterized by a small twist \( \boldsymbol{\xi} \) (2D: \( \boldsymbol{\xi}=[\Delta x,\Delta y,\Delta\theta]^{\top} \); 3D: \( \boldsymbol{\xi}=[\Delta\boldsymbol{\omega},\Delta\mathbf{v}]^{\top}\in\mathbb{R}^6 \)).

2D (SE(2)) point-to-line Jacobian. With current pose \( \mathbf{R}_0, \mathbf{t}_0 \), define \( \mathbf{s}_i = \mathbf{R}_0\mathbf{p}_i + \mathbf{t}_0 \). Residual is \( r_i = \mathbf{n}_i^{\top}(\mathbf{s}_i - \mathbf{q}_i) \). For small \( \Delta\theta \), \( \mathbf{R}(\Delta\theta)\approx \mathbf{I} + \Delta\theta\mathbf{J} \) where \( \mathbf{J}=\begin{bmatrix}0&-1\\1&0\end{bmatrix} \). The linearized residual is

\[ r_i(\boldsymbol{\xi}) \;\approx\; r_i(\mathbf{0}) \;+\; \mathbf{n}_i^{\top}\begin{bmatrix}1&0\end{bmatrix}\Delta x \;+\; \mathbf{n}_i^{\top}\begin{bmatrix}0&1\end{bmatrix}\Delta y \;+\; \mathbf{n}_i^{\top}\left(\mathbf{R}_0\mathbf{J}\mathbf{p}_i\right)\Delta\theta . \]

Stacking rows yields a weighted linear least squares problem \( \min_{\boldsymbol{\xi} }\|\mathbf{W}^{1/2}(\mathbf{A}\boldsymbol{\xi}+\mathbf{r})\|_2^2 \) with normal equations

\[ \left(\mathbf{A}^{\top}\mathbf{W}\mathbf{A}\right)\boldsymbol{\xi} \;=\; -\mathbf{A}^{\top}\mathbf{W}\mathbf{r}. \]

3D (SE(3)) point-to-plane Jacobian (compact form). For a point \( \mathbf{s}_i=\mathbf{R}_0\mathbf{p}_i+\mathbf{t}_0 \), the first-order perturbation by twist \( \boldsymbol{\xi}=[\Delta\boldsymbol{\omega},\Delta\mathbf{v}] \) gives \( \delta\mathbf{s}_i \approx -[\mathbf{s}_i]_{\times}\Delta\boldsymbol{\omega} + \Delta\mathbf{v} \). Therefore, \( \frac{\partial r_i}{\partial \Delta\boldsymbol{\omega} } = -(\mathbf{n}_i^{\top}[\mathbf{s}_i]_{\times}) \) and \( \frac{\partial r_i}{\partial \Delta\mathbf{v} } = \mathbf{n}_i^{\top} \).

Why this often converges faster: On locally planar surfaces, point-to-plane/line approximates the true distance to the surface better than Euclidean point-to-point matching, producing a better-conditioned Gauss–Newton step near the optimum.

Real mobile environments violate ICP assumptions: outliers occur due to moving objects, partial overlap, multi-path returns, and occlusions. Therefore practical ICP uses robustification.

Weighted ICP. Introduce per-correspondence weights \( w_i \) (sensor noise, incidence angle, range uncertainty). In point-to-point form: \( \sum_i w_i \|\mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i\|^2 \). In point-to-line/plane form, weights multiply squared normal residuals.

Robust ICP via M-estimators (IRLS). Replace squared residuals by a robust penalty \( \rho(r) \), e.g., Huber:

\[ \rho_{\text{Huber} }(r) \;=\; \begin{cases} \tfrac{1}{2}r^2, & |r| \le \delta \\ \delta|r| - \tfrac{1}{2}\delta^2, & |r| > \delta \end{cases} \]

IRLS converts this to a sequence of weighted least-squares problems with weights \( w_i = \psi(r_i)/r_i \), where \( \psi(r)=\rho'(r) \). For Huber, \( w_i = 1 \) when \( |r_i| \le \delta \) and \( w_i = \delta/|r_i| \) when \( |r_i| > \delta \).

Trimmed ICP. If only a fraction of points overlap (common when turning around corners), select the \( \alpha \in (0,1] \) fraction of correspondences with smallest residual and discard the rest:

\[ E_{\text{trim} }(\mathbf{R},\mathbf{t}) \;=\; \sum_{i \in \mathcal{I}_{\alpha}(\mathbf{R},\mathbf{t})} \left\lVert \mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_{c(i)} \right\rVert_2^2, \quad |\mathcal{I}_{\alpha}| = \lfloor \alpha N \rfloor, \]

where \( \mathcal{I}_{\alpha} \) keeps the smallest residuals under the current pose. This can be seen as minimizing a truncated objective that approximates partial overlap.

Gating and mutual checks. Even before robust costs, simple geometric checks help: distance gate \( \|\mathbf{R}\mathbf{p}_i+\mathbf{t}-\mathbf{q}_{c(i)}\| \le d_{\max} \), and normal compatibility \( \mathbf{n}_i^{\top}\mathbf{n}'_i \ge \cos(\theta_{\max}) \) (when both scans have normals).

A principled probabilistic view treats each correspondence not as an exact point match but as a match between local surface distributions. This yields Generalized ICP (GICP), which interpolates between point-to-point and point-to-plane behavior depending on local covariance.

Model each point as a Gaussian with covariance estimated from neighbors: \( \mathbf{p}_i \sim \mathcal{N}(\bar{\mathbf{p} }_i, \mathbf{C}^{P}_i) \), \( \mathbf{q}_i \sim \mathcal{N}(\bar{\mathbf{q} }_i, \mathbf{C}^{Q}_i) \). Under independence, the residual distribution for correspondence \( i \) has covariance \( \mathbf{C}_i = \mathbf{C}^{Q}_i + \mathbf{R}\mathbf{C}^{P}_i\mathbf{R}^{\top} \). The negative log-likelihood yields the weighted quadratic form:

\[ E_{\text{G} }(\mathbf{R},\mathbf{t}) \;=\; \sum_{i=1}^{N} \left(\mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i\right)^{\top} \mathbf{C}_i^{-1} \left(\mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i\right). \]

Interpretation. If \( \mathbf{C}^{Q}_i=\sigma^2\mathbf{I} \) and \( \mathbf{C}^{P}_i=\mathbf{0} \), then \( \mathbf{C}_i^{-1}\propto \mathbf{I} \) and we recover point-to-point ICP. If covariances are highly anisotropic with small variance along the normal and large variance in tangential directions, then the quadratic form emphasizes normal error, approximating point-to-plane behavior.

Mobile-robot relevance. With a spinning 3D LiDAR, local neighborhoods are often near-planar on walls and the ground; GICP can adapt to this structure and improve convergence and stability compared to pure point-to-point.

ICP is only as good as its correspondences. Mobile robots face strict compute budgets and must make correspondence search efficient and stable.

Nearest-neighbor (NN) ICP. The standard choice is NN in Euclidean space: \( c(i)=\arg\min_j \|\mathbf{R}\mathbf{p}_i+\mathbf{t}-\mathbf{q}_j\| \). Use a k-d tree on the reference scan for approximately \( \mathcal{O}(N\log M) \) queries (average case).

Projective correspondences (structured LiDAR / range images). For sensors with an angular image structure (many 3D LiDARs), you can project points into the range image and search correspondences in a small window around the projected pixel. This yields approximate correspondences in \( \mathcal{O}(N) \) time and is common in real-time odometry.

Multi-resolution ICP. Build a pyramid by downsampling (voxel grid in 3D or stride subsampling in 2D), solve coarse-to-fine:

\[ \mathbf{T}^{(0)} \text{ (coarse)} \;\; ←\;\; \text{ICP}(\mathcal{P}^{(0)},\mathcal{Q}^{(0)}), \qquad \mathbf{T}^{(\ell+1)} \;\; ←\;\; \text{ICP}(\mathbf{T}^{(\ell)}\!\circ\mathcal{P}^{(\ell+1)},\mathcal{Q}^{(\ell+1)}). \]

This expands the basin of convergence and reduces sensitivity to noise.

Degeneracy detection via normal equations. In point-to-line/plane ICP, the approximate Hessian is \( \mathbf{H}\approx \mathbf{A}^{\top}\mathbf{W}\mathbf{A} \). If \( \mathbf{H} \) has a near-zero eigenvalue, motion is weakly observable (e.g., a long straight corridor cannot constrain translation along the corridor). A common remedy is to add a weak motion prior:

\[ \min_{\boldsymbol{\xi} } \;\; \|\mathbf{W}^{1/2}(\mathbf{A}\boldsymbol{\xi}+\mathbf{r})\|_2^2 \;+\; \lambda \|\boldsymbol{\xi}\|_2^2, \quad \lambda > 0, \]

which corresponds to Tikhonov regularization and improves numerical stability.

flowchart TD
  S["Sensor type?"] --> L2["2D LiDAR (planar scans)"]
  S --> L3["3D LiDAR (point clouds)"]
  L2 --> G1["Dominant geometry: lines/walls?"]
  G1 -->|yes| P2L["Use point-to-line ICP \n+ trimming"]
  G1 -->|no| P2P2["Use point-to-point ICP \n+ robust weights"]
  L3 --> G2["Local planar structure available?"]
  G2 -->|yes| P2PL["Use point-to-plane \nor probabilistic ICP"]
  G2 -->|no| P2P3["Use point-to-point \n+ downsampling"]
  P2L --> D1["Check degeneracy from normal equations eigenvalues"]
  P2PL --> D1
  P2P2 --> D1
  P2P3 --> D1
  D1 --> OUT["Output pose + quality metrics"]
  

In a navigation stack, ICP is often used to estimate incremental motion between consecutive scans. You also want a quality estimate to decide whether to trust ICP (e.g., to fuse with odometry in later chapters).

For a linearized least-squares step \( \mathbf{A}\boldsymbol{\xi}\approx -\mathbf{r} \) with residual noise variance \( \sigma^2 \), the covariance of the estimated increment is approximately

\[ \operatorname{Cov}(\hat{\boldsymbol{\xi} }) \;\approx\; \sigma^2\left(\mathbf{A}^{\top}\mathbf{W}\mathbf{A}\right)^{-1}. \]

The diagonal entries indicate uncertainty in \( \Delta x,\Delta y,\Delta\theta \) (2D) or twist components (3D). Large variances correspond to poor constraints (e.g., featureless environments).

Practical quality gates. Typical mobile-robot heuristics include:

• Minimum number of inliers \( N_{\text{in} } \ge N_{\min} \).
• Residual RMS below threshold: \( \sqrt{\frac{1}{N_{\text{in} } }\sum r_i^2} < \epsilon_r \).
• Condition number of \( \mathbf{A}^{\top}\mathbf{W}\mathbf{A} \) below a limit.
• Increment magnitude bounded (reject unrealistic jumps): \( \|\Delta\mathbf{t}\| < d_{\max} \), \( |\Delta\theta| < \theta_{\max} \).

The following reference implementations focus on 2D scan matching (common in indoor AMR) and implement both point-to-point and point-to-line ICP. For 3D, the same structure applies; only the residual and Jacobians change.

Python libraries. Typical robotics workflows use numpy, scipy and (for 3D) open3d. In ROS environments, you often wrap ICP as a node subscribing to laser scans / point clouds and publishing tf.

Chapter10_Lesson2.py

import numpy as np
import pandas as pd
np.random.seed(0)

# Design: A (fixed, a=3), B (random, b=4), r=5 replicates per cell
a, b, r = 3, 4, 5
A_levels = [f"A{i+1}" for i in range(a)]
B_levels = [f"B{j+1}" for j in range(b)]

# True parameters
alpha = np.array([0.0, 0.6, 1.0])  # fixed A effects (sum-to-zero not enforced here)
sigma_b  = 0.5
sigma_ab = 0.4
sigma    = 1.0

rows = []
for i, Ai in enumerate(A_levels):
    for j, Bj in enumerate(B_levels):
        b_j = np.random.normal(0, sigma_b)
        ab_ij = np.random.normal(0, sigma_ab)
        mu_ij = 2.0 + alpha[i] + b_j + ab_ij
        y = mu_ij + np.random.normal(0, sigma, size=r)
        for k in range(r):
            rows.append((Ai, Bj, k+1, y[k]))
df = pd.DataFrame(rows, columns=["A","B","rep","y"])

# Design matrices (treatment coding for X; random effects Z via group indicators)
# (In practice, use patsy or statsmodels to build X and Z.)
import patsy
y_vec, X = patsy.dmatrices('y ~ C(A)', data=df, return_type='dataframe')
# Mixed model with random intercepts for B and interaction
import statsmodels.formula.api as smf
md = smf.mixedlm("y ~ C(A)", df, groups=df["B"], re_formula="1")
mfit = md.fit(reml=True, method='lbfgs')

print(mfit.summary())

# Naive fixed-effects ANOVA (incorrect denominator for A in mixed setting)
import statsmodels.api as sm
anova = sm.stats.anova_lm(smf.ols("y ~ C(A)*C(B)", data=df).fit(), typ=2)
print(anova)

# Simple (approximate) power for A main effect using pooled sigma from residuals
sigma_hat = np.sqrt(mfit.scale)
n_per_A = b*r  # observations per level of A
Delta = np.max(df.groupby("A")["y"].mean()) - np.min(df.groupby("A")["y"].mean())
from math import sqrt
def approx_power_two_group(delta, sigma, n):
    import scipy.stats as st
    z_alpha = st.norm.ppf(1-0.05/2)
    z = abs(delta)/sigma * sqrt(n/2)
    return st.norm.cdf(z - z_alpha)
print("Approx power (coarse, pairwise A levels):", approx_power_two_group(Delta, sigma_hat, n_per_A))

C++ libraries. In performance-critical stacks, C++ ICP is commonly built with Eigen for linear algebra and PCL for point-cloud utilities (k-d trees, voxel grids, ICP implementations). The demo below is educational and uses a brute-force NN for clarity (replace with a k-d tree for large scans).

Chapter10_Lesson2.cpp

// Chapter10_Lesson2.cpp
// ICP Variants for Mobile Robots (2D SE(2) focus)
//
// Implements point-to-point ICP (SVD/Procrustes) and point-to-line ICP (linearized LS).
// Notes:
//  - For production, prefer kd-tree (nanoflann) or PCL (pcl::IterativeClosestPoint).
//  - This file is self-contained except for Eigen.
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter10_Lesson2.cpp -I /path/to/eigen -o icp_demo
//
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <limits>
#include <cmath>
#include <algorithm>

using Vec2 = Eigen::Vector2d;
using Mat2 = Eigen::Matrix2d;

struct SE2 {
  Mat2 R;
  Vec2 t;
};

static inline Mat2 rot2(double theta) {
  double c = std::cos(theta), s = std::sin(theta);
  Mat2 R;
  R << c, -s,
       s,  c;
  return R;
}

static inline SE2 compose(const SE2& a, const SE2& b) {
  SE2 out;
  out.R = a.R * b.R;
  out.t = a.R * b.t + a.t;
  return out;
}

static inline double angleOf(const Mat2& R) {
  return std::atan2(R(1,0), R(0,0));
}

// Brute-force nearest neighbor (O(N*M)) for clarity
static inline void nearestNeighbor(
    const std::vector<Vec2>& src,
    const std::vector<Vec2>& dst,
    std::vector<int>& idx,
    std::vector<double>& dist)
{
  idx.assign(src.size(), -1);
  dist.assign(src.size(), std::numeric_limits<double>::infinity());
  for (size_t i=0; i<src.size(); ++i) {
    double best = std::numeric_limits<double>::infinity();
    int bestj = -1;
    for (size_t j=0; j<dst.size(); ++j) {
      double d2 = (src[i] - dst[j]).squaredNorm();
      if (d2 < best) { best = d2; bestj = (int)j; }
    }
    idx[i] = bestj;
    dist[i] = std::sqrt(best);
  }
}

// Estimate 2D normals for ordered scan points (polyline assumption)
static inline std::vector<Vec2> estimateNormals2D(const std::vector<Vec2>& q) {
  size_t M = q.size();
  std::vector<Vec2> n(M);
  for (size_t i=0; i<M; ++i) {
    const Vec2& prev = q[(i + M - 1) % M];
    const Vec2& next = q[(i + 1) % M];
    Vec2 tang = next - prev;
    Vec2 nn(-tang.y(), tang.x());
    double norm = nn.norm();
    if (norm < 1e-12) nn = Vec2(1,0);
    else nn /= norm;
    n[i] = nn;
  }
  return n;
}

// Weighted Procrustes (point-to-point) in 2D using SVD
static inline SE2 solvePointToPoint(
    const std::vector<Vec2>& P,
    const std::vector<Vec2>& Q,
    const std::vector<double>& w)
{
  double W = 0.0;
  Vec2 pbar(0,0), qbar(0,0);
  for (size_t i=0; i<P.size(); ++i) {
    double wi = w.empty() ? 1.0 : w[i];
    W += wi;
    pbar += wi * P[i];
    qbar += wi * Q[i];
  }
  if (W < 1e-12) W = 1.0;
  pbar /= W; qbar /= W;

  Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
  for (size_t i=0; i<P.size(); ++i) {
    double wi = w.empty() ? 1.0 : w[i];
    Vec2 pc = P[i] - pbar;
    Vec2 qc = Q[i] - qbar;
    H += wi * pc * qc.transpose();
  }

  Eigen::JacobiSVD<Eigen::Matrix2d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Mat2 U = svd.matrixU();
  Mat2 V = svd.matrixV();
  Mat2 R = V * U.transpose();
  if (R.determinant() < 0) {
    // reflection fix
    V.col(1) *= -1.0;
    R = V * U.transpose();
  }
  Vec2 t = qbar - R * pbar;
  return SE2{R, t};
}

// One linearized point-to-line step in SE(2)
static inline void solvePointToLineStep(
    const std::vector<Vec2>& Psrc,         // source points (untransformed)
    const std::vector<Vec2>& Qref,         // matched reference points
    const std::vector<Vec2>& Nref,         // matched normals at reference
    const SE2& cur,
    Eigen::Vector3d& delta,
    double& mean_r2)
{
  const size_t N = Psrc.size();
  Eigen::MatrixXd A(N, 3);
  Eigen::VectorXd r(N);

  mean_r2 = 0.0;
  for (size_t i=0; i<N; ++i) {
    Vec2 p = cur.R * Psrc[i] + cur.t;      // current transformed
    Vec2 e = p - Qref[i];
    double ri = Nref[i].dot(e);
    r((int)i) = ri;
    mean_r2 += ri*ri;

    // Jacobian: [n_x, n_y, n^T R J psrc]
    A((int)i, 0) = Nref[i].x();
    A((int)i, 1) = Nref[i].y();
    // J * psrc = [-y, x]
    Vec2 perp(-Psrc[i].y(), Psrc[i].x());
    Vec2 d = cur.R * perp;
    A((int)i, 2) = Nref[i].dot(d);
  }
  mean_r2 /= std::max<size_t>(1, N);

  // Solve least squares: minimize ||A*delta + r||^2
  // Normal equations for small N: (A^T A) delta = -A^T r
  Eigen::Matrix3d H = A.transpose() * A;
  Eigen::Vector3d g = A.transpose() * r;
  delta = -H.ldlt().solve(g);
}

static inline std::vector<Vec2> applyTransform(const std::vector<Vec2>& pts, const SE2& T) {
  std::vector<Vec2> out(pts.size());
  for (size_t i=0; i<pts.size(); ++i) out[i] = T.R * pts[i] + T.t;
  return out;
}

// ICP loop in SE(2)
static SE2 icpSE2(
    const std::vector<Vec2>& src,
    const std::vector<Vec2>& dst,
    const std::string& variant,
    int maxIter,
    double tol,
    double rejectDist,
    double trimRatio)
{
  SE2 T{rot2(0.0), Vec2(0,0)};
  std::vector<Vec2> dstNormals;
  if (variant == "p2l") dstNormals = estimateNormals2D(dst);

  double prevCost = std::numeric_limits<double>::infinity();

  for (int it=0; it<maxIter; ++it) {
    auto srcT = applyTransform(src, T);

    // correspondences
    std::vector<int> idx;
    std::vector<double> d;
    nearestNeighbor(srcT, dst, idx, d);

    // filter by rejectDist
    std::vector<int> kept;
    kept.reserve(src.size());
    for (size_t i=0; i<src.size(); ++i) {
      if (idx[i] < 0) continue;
      if (rejectDist > 0.0 && d[i] > rejectDist) continue;
      kept.push_back((int)i);
    }

    // trimming by distance
    if (trimRatio > 0.0 && trimRatio <= 1.0 && kept.size() > 3) {
      size_t k = std::max<size_t>(3, (size_t)std::floor(trimRatio * kept.size()));
      std::nth_element(
        kept.begin(), kept.begin() + (int)k, kept.end(),
        [&](int a, int b) { return d[a] < d[b]; }
      );
      kept.resize(k);
    }

    if (kept.size() < 3) break;

    std::vector<Vec2> P, Q, N;
    P.reserve(kept.size());
    Q.reserve(kept.size());
    N.reserve(kept.size());

    for (int i : kept) {
      P.push_back(src[i]);
      Q.push_back(dst[idx[i]]);
      if (variant == "p2l") N.push_back(dstNormals[idx[i]]);
    }

    double cost = 0.0;

    if (variant == "p2p") {
      std::vector<double> w; // uniform weights
      SE2 Tnew = solvePointToPoint(P, Q, w);
      T = Tnew;

      auto srcAligned = applyTransform(P, T);
      double sum = 0.0;
      for (size_t i=0; i<P.size(); ++i) sum += (srcAligned[i] - Q[i]).squaredNorm();
      cost = sum / std::max<size_t>(1, P.size());
    } else {
      Eigen::Vector3d delta;
      double mean_r2;
      solvePointToLineStep(P, Q, N, T, delta, mean_r2);

      double dx = delta(0), dy = delta(1), dth = delta(2);
      SE2 dT{rot2(dth), Vec2(dx, dy)};
      T = compose(dT, T);
      cost = mean_r2;
    }

    if (std::abs(prevCost - cost) < tol) break;
    prevCost = cost;
  }

  return T;
}

int main() {
  // Synthetic 2D "scan"
  std::vector<Vec2> dst;
  const int M = 300;
  dst.reserve(M);
  for (int i=0; i<M; ++i) {
    double t = 2.0 * M_PI * (double)i / (double)M;
    double x = 2.0 * std::cos(t) + 0.3 * std::cos(5*t);
    double y = 1.0 * std::sin(t) + 0.2 * std::sin(3*t);
    dst.emplace_back(x, y);
  }

  // Ground-truth transform to generate src
  double trueTheta = 0.25;
  Vec2 trueT(0.8, -0.4);
  SE2 Tgt{rot2(trueTheta), trueT};

  std::vector<Vec2> src = applyTransform(dst, Tgt);
  // Add small noise
  for (auto& p : src) p += 0.01 * Vec2::Random();

  // Recover pose with point-to-line ICP
  SE2 est = icpSE2(src, dst, "p2l", /*maxIter*/60, /*tol*/1e-8, /*rejectDist*/0.5, /*trimRatio*/0.9);

  std::cout << "Estimated theta: " << angleOf(est.R) << "\n";
  std::cout << "Estimated t: [" << est.t.x() << ", " << est.t.y() << "]\n";
  std::cout << "True theta: " << trueTheta << "\n";
  std::cout << "True t: [" << trueT.x() << ", " << trueT.y() << "]\n";
  return 0;
}

Java libraries. Java robotics stacks often pair with EJML or Apache Commons Math for numerical linear algebra. The educational demo below uses only core Java and solves small normal equations directly.

Chapter10_Lesson2.java

// Chapter10_Lesson2.java
// ICP Variants for Mobile Robots (2D SE(2) focus)
//
// Implements:
//   - Point-to-Point ICP (SVD via closed-form 2x2) using a simple Procrustes derivation
//   - Point-to-Line ICP (linearized least squares) using normal equations
//
// This is intentionally lightweight and uses only core Java.
// For production robotics stacks, see:
//   - ROS 2 Java bindings, or
//   - libraries such as EJML for robust linear algebra.
//
// Compile:
//   javac Chapter10_Lesson2.java
// Run:
//   java Chapter10_Lesson2

import java.util.*;
import static java.lang.Math.*;

public class Chapter10_Lesson2 {

    // --------- small linear algebra helpers (2D / 3D) ---------
    static class Vec2 {
        double x, y;
        Vec2(double x, double y) { this.x = x; this.y = y; }
        Vec2 add(Vec2 o) { return new Vec2(x + o.x, y + o.y); }
        Vec2 sub(Vec2 o) { return new Vec2(x - o.x, y - o.y); }
        Vec2 mul(double s) { return new Vec2(s * x, s * y); }
        double dot(Vec2 o) { return x * o.x + y * o.y; }
        double norm2() { return x*x + y*y; }
        double norm() { return sqrt(norm2()); }
    }

    static class Mat2 {
        double a11, a12, a21, a22; // row-major
        Mat2(double a11, double a12, double a21, double a22) {
            this.a11=a11; this.a12=a12; this.a21=a21; this.a22=a22;
        }
        static Mat2 rot(double th) {
            double c=cos(th), s=sin(th);
            return new Mat2(c, -s, s, c);
        }
        Vec2 mul(Vec2 v) {
            return new Vec2(a11*v.x + a12*v.y, a21*v.x + a22*v.y);
        }
        Mat2 mul(Mat2 B) {
            return new Mat2(
                a11*B.a11 + a12*B.a21, a11*B.a12 + a12*B.a22,
                a21*B.a11 + a22*B.a21, a21*B.a12 + a22*B.a22
            );
        }
        double det() { return a11*a22 - a12*a21; }
        Mat2 transpose() { return new Mat2(a11, a21, a12, a22); }
    }

    static class SE2 {
        Mat2 R;
        Vec2 t;
        SE2(Mat2 R, Vec2 t) { this.R=R; this.t=t; }
        Vec2 transform(Vec2 p) { return R.mul(p).add(t); }
        static SE2 compose(SE2 A, SE2 B) {
            Mat2 R = A.R.mul(B.R);
            Vec2 t = A.R.mul(B.t).add(A.t);
            return new SE2(R, t);
        }
        double theta() { return atan2(R.a21, R.a11); }
    }

    // --------- brute-force NN ---------
    static int[] nearestNeighbor(List<Vec2> src, List<Vec2> dst, double[] outDist) {
        int[] idx = new int[src.size()];
        Arrays.fill(idx, -1);
        for (int i=0; i<src.size(); i++) {
            double best = Double.POSITIVE_INFINITY;
            int bestj = -1;
            Vec2 p = src.get(i);
            for (int j=0; j<dst.size(); j++) {
                Vec2 q = dst.get(j);
                double d2 = p.sub(q).norm2();
                if (d2 < best) { best = d2; bestj = j; }
            }
            idx[i] = bestj;
            outDist[i] = sqrt(best);
        }
        return idx;
    }

    // --------- normals for ordered scans ---------
    static List<Vec2> estimateNormals2D(List<Vec2> q) {
        int M = q.size();
        List<Vec2> n = new ArrayList<>(M);
        for (int i=0; i<M; i++) n.add(new Vec2(1,0));
        for (int i=0; i<M; i++) {
            Vec2 prev = q.get((i-1+M)%M);
            Vec2 next = q.get((i+1)%M);
            Vec2 tang = next.sub(prev);
            Vec2 nn = new Vec2(-tang.y, tang.x);
            double norm = nn.norm();
            if (norm < 1e-12) nn = new Vec2(1,0);
            else nn = nn.mul(1.0/norm);
            n.set(i, nn);
        }
        return n;
    }

    // --------- point-to-point closed-form (2D) ---------
    // Uses 2D trace maximization: theta = atan2(h21 - h12, h11 + h22)
    static SE2 solvePointToPoint(List<Vec2> P, List<Vec2> Q) {
        int N = P.size();
        Vec2 pbar = new Vec2(0,0), qbar = new Vec2(0,0);
        for (int i=0; i<N; i++) { pbar = pbar.add(P.get(i)); qbar = qbar.add(Q.get(i)); }
        pbar = pbar.mul(1.0/N);
        qbar = qbar.mul(1.0/N);

        // H = sum (p' q'^T)
        double h11=0,h12=0,h21=0,h22=0;
        for (int i=0; i<N; i++) {
            Vec2 pc = P.get(i).sub(pbar);
            Vec2 qc = Q.get(i).sub(qbar);
            h11 += pc.x * qc.x; h12 += pc.x * qc.y;
            h21 += pc.y * qc.x; h22 += pc.y * qc.y;
        }
        double th = atan2(h21 - h12, h11 + h22);
        Mat2 R = Mat2.rot(th);
        Vec2 t = qbar.sub(R.mul(pbar));
        return new SE2(R, t);
    }

    // --------- point-to-line linearized LS (SE(2)) ---------
    // Solve (A^T A) delta = -A^T r, delta=[dx,dy,dtheta]
    static SE2 solvePointToLineStep(List<Vec2> Psrc, List<Vec2> Qref, List<Vec2> Nref, SE2 cur) {
        int N = Psrc.size();
        double[][] A = new double[N][3];
        double[] r = new double[N];

        for (int i=0; i<N; i++) {
            Vec2 p = cur.transform(Psrc.get(i));
            Vec2 e = p.sub(Qref.get(i));
            Vec2 n = Nref.get(i);
            double ri = n.dot(e);
            r[i] = ri;

            A[i][0] = n.x;
            A[i][1] = n.y;

            Vec2 ps = Psrc.get(i);
            Vec2 perp = new Vec2(-ps.y, ps.x);
            Vec2 d = cur.R.mul(perp);
            A[i][2] = n.dot(d);
        }

        // H = A^T A (3x3), g = A^T r (3)
        double[][] H = new double[3][3];
        double[] g = new double[3];
        for (int i=0; i<N; i++) {
            for (int c=0; c<3; c++) {
                g[c] += A[i][c] * r[i];
                for (int d=0; d<3; d++) H[c][d] += A[i][c] * A[i][d];
            }
        }
        // Solve H delta = -g (small 3x3) via Gaussian elimination
        double[] b = new double[]{ -g[0], -g[1], -g[2] };
        double[] delta = solve3x3(H, b);

        double dx = delta[0], dy = delta[1], dth = delta[2];
        SE2 dT = new SE2(Mat2.rot(dth), new Vec2(dx, dy));
        return SE2.compose(dT, cur);
    }

    static double[] solve3x3(double[][] A, double[] b) {
        // Simple Gaussian elimination with partial pivoting for 3x3
        double[][] M = new double[3][4];
        for (int i=0; i<3; i++) {
            System.arraycopy(A[i], 0, M[i], 0, 3);
            M[i][3] = b[i];
        }

        for (int col=0; col<3; col++) {
            int piv = col;
            for (int r=col+1; r<3; r++) if (abs(M[r][col]) > abs(M[piv][col])) piv = r;
            double[] tmp = M[col]; M[col] = M[piv]; M[piv] = tmp;

            double diag = M[col][col];
            if (abs(diag) < 1e-12) diag = (diag >= 0 ? 1e-12 : -1e-12);
            for (int c=col; c<4; c++) M[col][c] /= diag;

            for (int r=0; r<3; r++) if (r != col) {
                double f = M[r][col];
                for (int c=col; c<4; c++) M[r][c] -= f * M[col][c];
            }
        }
        return new double[]{ M[0][3], M[1][3], M[2][3] };
    }

    static List<Vec2> apply(List<Vec2> pts, SE2 T) {
        List<Vec2> out = new ArrayList<>(pts.size());
        for (Vec2 p : pts) out.add(T.transform(p));
        return out;
    }

    static SE2 icpSE2(List<Vec2> src, List<Vec2> dst, String variant,
                      int maxIter, double tol, double rejectDist, double trimRatio) {
        SE2 T = new SE2(Mat2.rot(0.0), new Vec2(0,0));
        List<Vec2> dstNormals = variant.equals("p2l") ? estimateNormals2D(dst) : null;

        double prevCost = Double.POSITIVE_INFINITY;
        for (int it=0; it<maxIter; it++) {
            List<Vec2> srcT = apply(src, T);

            double[] dist = new double[srcT.size()];
            int[] idx = nearestNeighbor(srcT, dst, dist);

            ArrayList<Integer> kept = new ArrayList<>();
            for (int i=0; i<src.size(); i++) {
                if (idx[i] < 0) continue;
                if (rejectDist > 0 && dist[i] > rejectDist) continue;
                kept.add(i);
            }
            if (kept.size() < 3) break;

            // trimming by distance
            if (trimRatio > 0.0 && trimRatio <= 1.0 && kept.size() > 3) {
                kept.sort(Comparator.comparingDouble(i -> dist[i]));
                int k = max(3, (int)floor(trimRatio * kept.size()));
                while (kept.size() > k) kept.remove(kept.size()-1);
            }

            List<Vec2> P = new ArrayList<>(kept.size());
            List<Vec2> Q = new ArrayList<>(kept.size());
            List<Vec2> N = new ArrayList<>(kept.size());
            for (int i : kept) {
                P.add(src.get(i));
                Q.add(dst.get(idx[i]));
                if (dstNormals != null) N.add(dstNormals.get(idx[i]));
            }

            double cost;
            if (variant.equals("p2p")) {
                T = solvePointToPoint(P, Q);
                List<Vec2> aligned = apply(P, T);
                double sum = 0.0;
                for (int i=0; i<aligned.size(); i++) sum += aligned.get(i).sub(Q.get(i)).norm2();
                cost = sum / max(1, aligned.size());
            } else {
                // one Gauss-Newton step for point-to-line
                SE2 Tnew = solvePointToLineStep(P, Q, N, T);

                // cost: mean squared normal residual at current step
                double s = 0.0;
                List<Vec2> aligned = apply(P, Tnew);
                for (int i=0; i<aligned.size(); i++) {
                    double ri = N.get(i).dot(aligned.get(i).sub(Q.get(i)));
                    s += ri*ri;
                }
                cost = s / max(1, aligned.size());
                T = Tnew;
            }

            if (abs(prevCost - cost) < tol) break;
            prevCost = cost;
        }
        return T;
    }

    public static void main(String[] args) {
        // Synthetic "scan"
        int M = 300;
        ArrayList<Vec2> dst = new ArrayList<>(M);
        for (int i=0; i<M; i++) {
            double t = 2.0 * PI * i / (double)M;
            double x = 2.0 * cos(t) + 0.3 * cos(5*t);
            double y = 1.0 * sin(t) + 0.2 * sin(3*t);
            dst.add(new Vec2(x, y));
        }

        // Ground truth transform
        double trueTheta = 0.25;
        Vec2 trueT = new Vec2(0.8, -0.4);
        SE2 Tgt = new SE2(Mat2.rot(trueTheta), trueT);

        ArrayList<Vec2> src = new ArrayList<>(M);
        Random rnd = new Random(2);
        for (Vec2 p : dst) {
            Vec2 pp = Tgt.transform(p);
            // tiny noise
            pp = pp.add(new Vec2(0.01*(2*rnd.nextDouble()-1), 0.01*(2*rnd.nextDouble()-1)));
            src.add(pp);
        }

        // Estimate with point-to-line ICP
        SE2 est = icpSE2(src, dst, "p2l", 60, 1e-8, 0.5, 0.9);

        System.out.println("Estimated theta: " + est.theta());
        System.out.println("Estimated t: [" + est.t.x + ", " + est.t.y + "]");
        System.out.println("True theta: " + trueTheta);
        System.out.println("True t: [" + trueT.x + ", " + trueT.y + "]");
    }
}

MATLAB/Simulink. MATLAB is commonly used for prototyping; Robotics System Toolbox provides pcregistericp for 3D. The script below implements 2D variants from scratch and is compatible with basic MATLAB. In Simulink-based pipelines, ICP is typically wrapped as a MATLAB Function block operating on the latest scan pair.

Chapter10_Lesson2.m

% Chapter10_Lesson2.m
% ICP Variants for Mobile Robots (2D SE(2) focus)
%
% Implements:
%   - Point-to-Point ICP (closed-form 2D Procrustes)
%   - Point-to-Line ICP (linearized least squares)
%   - Optional robust Huber weighting and trimming
%
% Notes:
%   - For 3D point clouds, Robotics System Toolbox offers pcregistericp.
%   - This script is self-contained (no toolboxes required), but uses O(N*M)
%     nearest-neighbor search for clarity.

function Chapter10_Lesson2()
    rng(2);

    % Synthetic "scan": deformed ellipse
    M = 300;
    t = linspace(0, 2*pi, M)';
    dst = [2*cos(t) + 0.3*cos(5*t), 1.0*sin(t) + 0.2*sin(3*t)];

    % Ground truth transform to generate src
    trueTheta = 0.25;
    trueT = [0.8; -0.4];
    Rgt = rot2(trueTheta);
    src = (Rgt*dst')' + trueT';
    src = src + 0.01*randn(size(src));

    % Run point-to-line ICP (faster locally)
    variant = "p2l";  % "p2p" or "p2l"
    opts.maxIter = 60;
    opts.tol = 1e-8;
    opts.rejectDist = 0.5;
    opts.trimRatio = 0.9;
    opts.huberDelta = 0.05;

    [R, t_est, hist] = icpSE2(src, dst, variant, opts);

    estTheta = atan2(R(2,1), R(1,1));
    fprintf("Estimated theta: %.6f\n", estTheta);
    fprintf("Estimated t: [%.6f, %.6f]\n", t_est(1), t_est(2));
    fprintf("True theta: %.6f\n", trueTheta);
    fprintf("True t: [%.6f, %.6f]\n", trueT(1), trueT(2));

    % Plot
    srcAligned = (R*src')' + t_est';
    figure;
    plot(dst(:,1), dst(:,2), '.', 'DisplayName', 'dst (reference)'); hold on;
    plot(src(:,1), src(:,2), '.', 'DisplayName', 'src (raw)');
    plot(srcAligned(:,1), srcAligned(:,2), '.', 'DisplayName', 'src aligned');
    axis equal; legend; title('ICP alignment (2D)');
end

% --------------------------- helpers ---------------------------

function R = rot2(theta)
    c = cos(theta); s = sin(theta);
    R = [c, -s; s, c];
end

function n = estimateNormals2D(q)
    % Ordered scan assumption: normal is perpendicular to chord tangent
    M = size(q,1);
    qprev = q([M, 1:M-1], :);
    qnext = q([2:M, 1], :);
    tang = qnext - qprev;
    n = [-tang(:,2), tang(:,1)];
    nn = sqrt(sum(n.^2,2)) + 1e-12;
    n = n ./ nn;
end

function [idx, dist] = nearestNeighborBrute(src, dst)
    % O(N*M) brute force
    N = size(src,1); M = size(dst,1);
    idx = zeros(N,1); dist = zeros(N,1);
    for i=1:N
        d2 = sum((dst - src(i,:)).^2, 2);
        [best, j] = min(d2);
        idx(i) = j;
        dist(i) = sqrt(best);
    end
end

function w = huberWeights(r, delta)
    a = abs(r);
    w = ones(size(r));
    mask = a > delta;
    w(mask) = delta ./ (a(mask) + 1e-12);
end

function [R, t, history] = solvePointToPoint(P, Q, w)
    if nargin < 3 || isempty(w)
        w = ones(size(P,1),1);
    end
    W = sum(w) + 1e-12;
    pbar = sum(P .* w, 1) / W;
    qbar = sum(Q .* w, 1) / W;
    Pc = P - pbar;
    Qc = Q - qbar;
    H = (Pc .* w)' * Qc;
    [U, ~, V] = svd(H);
    R = V * U';
    if det(R) < 0
        V(:,2) = -V(:,2);
        R = V * U';
    end
    t = (qbar' - R*pbar');
end

function [R, t_est, history] = icpSE2(src, dst, variant, opts)
    R = rot2(0.0);
    t_est = [0;0];

    history.cost = [];
    history.inliers = [];

    if variant == "p2l"
        dstNormals = estimateNormals2D(dst);
    else
        dstNormals = [];
    end

    prevCost = inf;

    for it = 1:opts.maxIter
        srcT = (R*src')' + t_est';
        [idx, dist] = nearestNeighborBrute(srcT, dst);
        mask = true(size(dist));
        if isfield(opts, 'rejectDist') && opts.rejectDist > 0
            mask = mask & (dist <= opts.rejectDist);
        end

        % trimming
        if isfield(opts, 'trimRatio') && ~isempty(opts.trimRatio) && opts.trimRatio > 0 && opts.trimRatio <= 1
            cand = find(mask);
            if ~isempty(cand)
                k = max(3, floor(opts.trimRatio * numel(cand)));
                [~, order] = sort(dist(cand), 'ascend');
                keep = cand(order(1:k));
                mask = false(size(mask));
                mask(keep) = true;
            end
        end

        if sum(mask) < 3
            break;
        end

        P = src(mask,:);
        Q = dst(idx(mask),:);

        if variant == "p2p"
            w = [];
            if isfield(opts, 'huberDelta') && ~isempty(opts.huberDelta)
                e = (R*P')' + t_est' - Q;
                rnorm = sqrt(sum(e.^2,2));
                w = huberWeights(rnorm, opts.huberDelta);
            end
            [R, t_est, ~] = solvePointToPoint(P, Q, w);
            e = (R*P')' + t_est' - Q;
            cost = mean(sum(e.^2,2));
        else
            n = dstNormals(idx(mask), :);
            % linearized point-to-line solve for delta=[dx,dy,dtheta]
            pcur = (R*P')' + t_est';
            r = sum(n .* (pcur - Q), 2);
            perp = [-P(:,2), P(:,1)];
            Jtheta = sum(n .* (R*perp')', 2);
            A = [n, Jtheta];

            w = ones(size(r));
            if isfield(opts, 'huberDelta') && ~isempty(opts.huberDelta)
                w = huberWeights(r, opts.huberDelta);
            end

            W = sqrt(w);
            Aw = A .* W;
            bw = r .* W;
            delta = - (Aw \ bw);  % least squares

            dx = delta(1); dy = delta(2); dtheta = delta(3);
            Rdelta = rot2(dtheta);
            tdelta = [dx; dy];

            % compose update
            t_est = Rdelta*t_est + tdelta;
            R = Rdelta*R;

            % compute cost
            pnew = (R*P')' + t_est';
            r2 = sum(n .* (pnew - Q), 2);
            cost = mean(r2.^2);
        end

        history.cost(end+1) = cost;
        history.inliers(end+1) = sum(mask);

        if abs(prevCost - cost) < opts.tol
            break;
        end
        prevCost = cost;
    end
end

Wolfram Mathematica. Mathematica provides fast nearest-neighbor queries and symbolic/numeric linear algebra, making it a good platform for experimenting with alternative metrics and analyzing conditioning. The notebook below contains a compact implementation.

Chapter10_Lesson2.nb

(* ::Package:: *)

(* Chapter10_Lesson2.nb *)
(* ICP Variants for Mobile Robots (2D SE(2) focus) *)

(* This notebook uses:
     - Nearest[] for correspondences
     - A 2D point-to-point closed-form rotation estimate
     - A 2D point-to-line linearized least-squares step
*)

ClearAll["Global`*"];

rot2[th_] := { {Cos[th], -Sin[th]}, {Sin[th], Cos[th]} };

applySE2[R_, t_, pts_] := (R.# + t) & /@ pts;

(* Estimate 2D normals for ordered polyline points *)
estimateNormals2D[q_List] := Module[{M = Length[q], prev, next, tang, n},
  prev = RotateLeft[q, 1];
  next = RotateRight[q, 1];
  tang = next - prev;
  n = ({-#[[2]], #[[1]]} & /@ tang);
  n = Normalize /@ n;
  n
];

(* Point-to-point closed-form in 2D: theta = atan2(h21-h12, h11+h22) *)
solveP2P2D[P_List, Q_List] := Module[{pbar, qbar, Pc, Qc, h11, h12, h21, h22, th, R, t},
  pbar = Mean[P];
  qbar = Mean[Q];
  Pc = P - pbar;
  Qc = Q - qbar;

  h11 = Total[Pc[[All, 1]]*Qc[[All, 1]]];
  h12 = Total[Pc[[All, 1]]*Qc[[All, 2]]];
  h21 = Total[Pc[[All, 2]]*Qc[[All, 1]]];
  h22 = Total[Pc[[All, 2]]*Qc[[All, 2]]];

  th = ArcTan[h11 + h22, h21 - h12];
  R = rot2[th];
  t = qbar - R.pbar;
  {R, t, th}
];

(* One linearized point-to-line step in SE(2) *)
solveP2LStep2D[Psrc_List, Qref_List, Nref_List, R0_, t0_] := Module[
  {N = Length[Psrc], pcur, r, A, perp, Jtheta, delta, dx, dy, dth, Rdelta, tdelta, R1, t1},
  pcur = applySE2[R0, t0, Psrc];
  r = MapThread[#3.(#1 - #2) &, {pcur, Qref, Nref}];

  perp = ({-#[[2]], #[[1]]} & /@ Psrc);
  Jtheta = MapThread[#2.(R0.#1) &, {perp, Nref}];

  A = Transpose[{Nref[[All, 1]], Nref[[All, 2]], Jtheta}];

  (* Least squares: minimize ||A delta + r||^2  *)
  delta = -LinearSolve[Transpose[A].A, Transpose[A].r];
  {dx, dy, dth} = delta;

  Rdelta = rot2[dth];
  tdelta = {dx, dy};

  (* Compose update *)
  R1 = Rdelta.R0;
  t1 = Rdelta.t0 + tdelta;
  {R1, t1, delta}
];

(* ICP loop (brute-force NN via Nearest[]) *)
icpSE2[src_List, dst_List, variant_: "p2l", maxIter_: 60, tol_: 1*^-8, rejectDist_: 0.5, trimRatio_: 0.9] := Module[
  {R = rot2[0.0], t = {0.0, 0.0}, prevCost = Infinity, it,
   nn, dstNormals, srcT, pairs, dist, idx, keep, P, Q, N, cost, out},

  nn = Nearest[dst -> "Index"];
  dstNormals = If[variant == "p2l", estimateNormals2D[dst], {}];

  For[it = 1, it <= maxIter, it++,
    srcT = applySE2[R, t, src];

    idx = nn /@ srcT; idx = Flatten[idx];
    dist = MapThread[Norm[dst[[#2]] - #1] &, {srcT, idx}];

    keep = Flatten@Position[dist, _?(# <= rejectDist &)];

    If[Length[keep] < 3, Break[]];

    (* trimming *)
    If[trimRatio < 1 && Length[keep] > 3,
      keep = Take[keep, UpTo[Max[3, Floor[trimRatio*Length[keep]]]]] /. 
        Thread[keep -> keep[[Ordering[dist[[keep]]]]]];
    ];

    P = src[[keep]];
    Q = dst[[idx[[keep]]]];

    If[variant == "p2p",
      out = solveP2P2D[P, Q];
      R = out[[1]]; t = out[[2]];
      cost = Mean[Norm /@ (applySE2[R, t, P] - Q)]^2;,
      N = dstNormals[[idx[[keep]]]];
      out = solveP2LStep2D[P, Q, N, R, t];
      R = out[[1]]; t = out[[2]];
      cost = Mean[(MapThread[#3.(#1 - #2) &, {applySE2[R, t, P], Q, N}])^2];
    ];

    If[Abs[prevCost - cost] < tol, Break[]];
    prevCost = cost;
  ];

  {R, t, ArcTan[R[[1, 1]], R[[2, 1]]]}
];

(* Demo *)
SeedRandom[2];
M = 300;
tt = Subdivide[0, 2 Pi, M - 1];
dst = Transpose[{2 Cos[tt] + 0.3 Cos[5 tt], 1.0 Sin[tt] + 0.2 Sin[3 tt]}];

trueTheta = 0.25;
trueT = {0.8, -0.4};
Rgt = rot2[trueTheta];

src = applySE2[Rgt, trueT, dst] + 0.01 RandomVariate[NormalDistribution[0, 1], {M, 2}];

{Rest, test, thetaEst} = icpSE2[src, dst, "p2l", 60, 1*^-8, 0.5, 0.9];

Print["Estimated theta: ", thetaEst];
Print["Estimated t: ", test];
Print["True theta: ", trueTheta];
Print["True t: ", trueT];

The following problems reinforce the mathematical structure of ICP variants and the failure modes relevant to AMR scan matching.

Problem 1 (Monotone Descent): Show that the standard ICP iterations produce a non-increasing sequence of objective values \( E^{(k)} \) for the point-to-point objective, assuming the correspondence step chooses exact nearest neighbors.

Solution: Define \( E(\mathbf{R},\mathbf{t},c)=\sum_i \|\mathbf{R}\mathbf{p}_i+\mathbf{t}-\mathbf{q}_{c(i)}\|^2 \). By definition of nearest-neighbor assignment, \( E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c^{(k+1)}) \le E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c^{(k)}) \). By definition of the rigid transform minimizer with fixed correspondences, \( E(\mathbf{R}^{(k+1)},\mathbf{t}^{(k+1)},c^{(k+1)}) \le E(\mathbf{R}^{(k)},\mathbf{t}^{(k)},c^{(k+1)}) \). Chaining gives \( E^{(k+1)} \le E^{(k)} \).

Problem 2 (2D Closed-Form Rotation): Let \( \mathbf{H}=\sum_i \mathbf{p}'_i{\mathbf{q}'_i}^{\top}=\begin{bmatrix}h_{11}&h_{12}\\h_{21}&h_{22}\end{bmatrix} \) be the 2D cross-covariance matrix for centered correspondences. Prove that the optimal rotation angle for point-to-point ICP is \( \theta^\star = \operatorname{atan2}(h_{21}-h_{12},\,h_{11}+h_{22}) \).

Solution: In 2D, any rotation is \( \mathbf{R}(\theta)=\begin{bmatrix}\cos\theta&-\sin\theta\\\sin\theta&\cos\theta\end{bmatrix} \). The trace objective is \( \operatorname{tr}(\mathbf{R}(\theta)\mathbf{H})=\cos\theta(h_{11}+h_{22}) + \sin\theta(h_{21}-h_{12}) \). Maximizing over \( \theta \) is maximizing \( a\cos\theta + b\sin\theta \) with \( a=h_{11}+h_{22} \), \( b=h_{21}-h_{12} \). The maximum occurs at angle \( \theta^\star=\operatorname{atan2}(b,a) \), giving the stated result.

Problem 3 (Point-to-Line Normal Equations in SE(2)): Starting from the point-to-line residual \( r_i=\mathbf{n}_i^{\top}(\mathbf{R}\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i) \), derive the linearized system \( (\mathbf{A}^{\top}\mathbf{A})\boldsymbol{\xi}=-\mathbf{A}^{\top}\mathbf{r} \) for \( \boldsymbol{\xi}=[\Delta x,\Delta y,\Delta\theta]^{\top} \).

Solution: Linearize around \( (\mathbf{R}_0,\mathbf{t}_0) \). Let \( \mathbf{s}_i=\mathbf{R}_0\mathbf{p}_i+\mathbf{t}_0 \). For small \( \Delta\theta \), \( \mathbf{R}(\Delta\theta)\approx \mathbf{I}+\Delta\theta\mathbf{J} \), with \( \mathbf{J}=\begin{bmatrix}0&-1\\1&0\end{bmatrix} \). Thus \( \mathbf{R}\mathbf{p}_i+\mathbf{t}\approx \mathbf{s}_i + [\Delta x,\Delta y]^{\top} + \Delta\theta(\mathbf{R}_0\mathbf{J}\mathbf{p}_i) \). Substitute into \( r_i \):

\[ r_i(\boldsymbol{\xi}) \;\approx\; r_i(\mathbf{0}) \;+\; n_{ix}\Delta x \;+\; n_{iy}\Delta y \;+\; \left(\mathbf{n}_i^{\top}\mathbf{R}_0\mathbf{J}\mathbf{p}_i\right)\Delta\theta . \]

Therefore each row of \( \mathbf{A} \) is \( [n_{ix},\; n_{iy},\; \mathbf{n}_i^{\top}\mathbf{R}_0\mathbf{J}\mathbf{p}_i] \). Stacking gives \( \mathbf{r}(\boldsymbol{\xi})\approx \mathbf{r}+\mathbf{A}\boldsymbol{\xi} \), and minimizing \( \|\mathbf{r}+\mathbf{A}\boldsymbol{\xi}\|^2 \) yields the normal equations.

Problem 4 (Corridor Degeneracy): Consider a 2D environment where all matched points lie on a single straight wall. Explain why translation along the wall is weakly constrained for point-to-line ICP, and relate this to the eigenstructure of \( \mathbf{H}=\mathbf{A}^{\top}\mathbf{A} \).

Solution: If all normals \( \mathbf{n}_i \) are (approximately) identical (wall normal), the residual \( r_i=\mathbf{n}^{\top}(\mathbf{R}\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i) \) depends only on the component of translation along \( \mathbf{n} \) (perpendicular to the wall). Translation parallel to the wall changes \( \mathbf{t} \) in a direction orthogonal to \( \mathbf{n} \), producing almost no change in \( r_i \). In the linear system, this means columns of \( \mathbf{A} \) corresponding to that translation direction contribute little, so \( \mathbf{A}^{\top}\mathbf{A} \) has a small eigenvalue: the corresponding eigenvector indicates the unobservable motion direction. Regularization or fusing with odometry can stabilize the estimate.

Problem 5 (Complexity Budget): Assume NN ICP uses a k-d tree and \( N \) source points, \( M \) reference points. Give the typical per-iteration time complexity and explain why downsampling helps.

Solution: Building the k-d tree is typically \( \mathcal{O}(M\log M) \) and queries are approximately \( \mathcal{O}(N\log M) \) (average case). Solving the transform step is \( \mathcal{O}(N) \) for accumulating matrices and a small fixed-cost solve (SVD in 2D/3D or 3×3 / 6×6 normal equations). Thus per iteration is dominated by correspondence search. Downsampling reduces \( N \) and \( M \), often with minimal loss in accuracy because redundant nearby points add little new constraint.

We presented ICP as alternating minimization over correspondences and pose, proved monotonic descent of the objective, and derived the key transform solvers that define major ICP variants. For AMR, the most consequential practical choices are: (i) point-to-line (2D) / point-to-plane (3D) for faster local convergence, (ii) robust weighting and trimming for outliers and partial overlap, and (iii) correspondence strategies and degeneracy detection to keep estimates stable in real-time pipelines.

1. Besl, P.J., & McKay, N.D. (1992). A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 14(2), 239–256.
2. Chen, Y., & Medioni, G. (1992). Object modeling by registration of multiple range images. Image and Vision Computing, 10(3), 145–155.
3. Rusinkiewicz, S., & Levoy, M. (2001). Efficient variants of the ICP algorithm. Proceedings of the 3rd International Conference on 3-D Digital Imaging and Modeling (3DIM), 145–152.
4. Censi, A. (2008). An ICP variant using a point-to-line metric. IEEE International Conference on Robotics and Automation (ICRA), 19–25.
5. Segal, A., Hähnel, D., & Thrun, S. (2009). Generalized-ICP. Robotics: Science and Systems (RSS).
6. Bouaziz, S., Tagliasacchi, A., & Pauly, M. (2013). Sparse iterative closest point. Computer Graphics Forum, 32(5), 113–123.
7. Pomerleau, F., Colas, F., Siegwart, R., & Magnenat, S. (2013). Comparing ICP variants on real-world data sets. Autonomous Robots, 34, 133–148.