By the end of this lab, you will be able to:

• Define the scan-to-scan motion estimation problem in 2D as a rigid alignment: find \( \mathbf{R}\in SO(2) \) and \( \mathbf{t}\in\mathbb{R}^2 \) such that \( \mathbf{q}_i \approx \mathbf{R}\mathbf{p}_i + \mathbf{t} \).
• Derive and implement the closed-form least-squares transform for fixed correspondences, including the 2D angle formula \( \theta^\star = \operatorname{atan2}(b,a) \).
• Implement a robust ICP loop with (i) nearest-neighbor correspondences, (ii) distance gating, (iii) trimming, (iv) robust weighting (Huber), and (v) convergence tests.
• Convert the estimated transform into a planar motion increment \( \Delta \mathbf{x}_k = (\Delta x, \Delta y, \Delta \psi) \) and integrate to produce scan-matching odometry.

Deliverables:

1. A working ICP-based scan-to-scan motion estimator in Python, C++, Java, MATLAB, and Wolfram Mathematica.
2. A short report (1–2 pages) that includes your parameter choices (gating threshold, trimming fraction, iteration budget), and failure cases (e.g., pure corridors, strong dynamics).
3. A quantitative evaluation: alignment error proxies (final RMSE, inlier ratio) and motion consistency checks.

Let scan \( k \) be a set of 2D points in the LiDAR frame at time \( k \): \( \mathcal{Q}_k = \{\mathbf{q}_i\}_{i=1}^{N_q} \), and scan \( k+1 \) be \( \mathcal{P}_{k+1} = \{\mathbf{p}_i\}_{i=1}^{N_p} \). We want the rigid transform that maps points from the newer scan into the older scan:

\[ \mathbf{q} \approx \mathbf{R}\mathbf{p} + \mathbf{t}, \quad \mathbf{R}= \begin{bmatrix} \cos\Delta\psi & -\sin\Delta\psi \\ \sin\Delta\psi & \cos\Delta\psi \end{bmatrix}, \quad \mathbf{t}= \begin{bmatrix}\Delta x \\ \Delta y\end{bmatrix}. \]

This transform corresponds to the robot’s relative planar motion between scans. In homogeneous form:

\[ \mathbf{T}_{k+1\rightarrow k} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

The main difficulty is that correspondences between \( \mathcal{P}_{k+1} \) and \( \mathcal{Q}_k \) are unknown and must be inferred from geometry (nearest-neighbor, projective association, or other strategies introduced in earlier lessons). ICP alternates between: (i) assigning correspondences and (ii) optimizing \( (\mathbf{R},\mathbf{t}) \).

graph TD
  A["Input: scan k, scan k+1"] --> B["Preprocess: range filter, downsample, remove isolated points"]
  B --> C["Initial guess: wheel/IMU odometry (optional)"]
  C --> D["Associate points: nearest neighbor in scan k"]
  D --> E["Reject bad pairs: distance gate + trimming"]
  E --> F["Solve rigid transform: R,t from matched pairs"]
  F --> G["Apply update to scan k+1 points"]
  G --> H["Convergence check: delta small OR cost stops improving"]
  H -->|not converged| D
  H -->|converged| I["Output: T(k+1 to k) + quality metrics"]
  

Assume we have a set of matched pairs \( (\mathbf{p}_i,\mathbf{q}_i) \). The classic point-to-point objective is:

\[ \min_{\mathbf{R}\in SO(2),\,\mathbf{t}\in\mathbb{R}^2} J(\mathbf{R},\mathbf{t}) := \sum_{i=1}^{m}\left\lVert \mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i \right\rVert_2^2. \]

Step 1 (optimal translation for fixed rotation). Let \( \bar{\mathbf{p}}=\frac{1}{m}\sum_i \mathbf{p}_i \) and \( \bar{\mathbf{q}}=\frac{1}{m}\sum_i \mathbf{q}_i \). Differentiate w.r.t. \( \mathbf{t} \):

\[ \frac{\partial J}{\partial \mathbf{t}} = 2\sum_{i=1}^{m}\left(\mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i\right)=\mathbf{0} \;\Rightarrow\; \mathbf{t}^\star = \bar{\mathbf{q}} - \mathbf{R}\bar{\mathbf{p}}. \]

Substitute \( \mathbf{t}^\star \) and define centered points \( \mathbf{x}_i=\mathbf{p}_i-\bar{\mathbf{p}} \), \( \mathbf{y}_i=\mathbf{q}_i-\bar{\mathbf{q}} \). Then:

\[ J(\mathbf{R},\mathbf{t}^\star)= \sum_{i=1}^{m}\left\lVert \mathbf{R}\mathbf{x}_i - \mathbf{y}_i \right\rVert_2^2 = \sum_{i=1}^{m}\left(\lVert \mathbf{x}_i\rVert_2^2 + \lVert \mathbf{y}_i\rVert_2^2\right) -2\,\operatorname{tr}(\mathbf{R}\mathbf{H}), \]

where the (2×2) cross-covariance is \( \mathbf{H}=\sum_{i=1}^{m}\mathbf{x}_i\mathbf{y}_i^\top \). Since the norm terms do not depend on \( \mathbf{R} \), minimizing \( J \) is equivalent to maximizing \( \operatorname{tr}(\mathbf{R}\mathbf{H}) \):

\[ \mathbf{R}^\star = \arg\max_{\mathbf{R}\in SO(2)} \operatorname{tr}(\mathbf{R}\mathbf{H}). \]

Step 2 (SVD/Kabsch solution). Let \( \mathbf{H}=\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^\top \) be the SVD. Using the trace cyclic property and orthogonality constraints, a standard proof based on the (von Neumann) trace inequality yields:

\[ \mathbf{R}^\star = \mathbf{V} \begin{bmatrix} 1 & 0\\ 0 & \det(\mathbf{V}\mathbf{U}^\top) \end{bmatrix} \mathbf{U}^\top. \]

Proof sketch (key idea). Write \( \operatorname{tr}(\mathbf{R}\mathbf{H})=\operatorname{tr}(\mathbf{R}\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^\top) =\operatorname{tr}(\mathbf{M}\boldsymbol{\Sigma}) \) with \( \mathbf{M}=\mathbf{V}^\top\mathbf{R}\mathbf{U} \in O(2) \). Since \( \boldsymbol{\Sigma} \) is diagonal and nonnegative, the trace is maximized when \( \mathbf{M} \) is as close as possible to identity, subject to the determinant constraint for \( SO(2) \), giving the sign-correction term.

2D closed-form (no SVD required). In 2D, the optimal rotation can be computed by a scalar angle. With centered pairs \( \mathbf{x}_i=[x_i,y_i]^\top \) and \( \mathbf{y}_i=[u_i,v_i]^\top \), define:

\[ a=\sum_{i=1}^{m}(x_i u_i + y_i v_i), \qquad b=\sum_{i=1}^{m}(x_i v_i - y_i u_i), \qquad \Delta\psi^\star=\operatorname{atan2}(b,a). \]

Then \( \mathbf{R}^\star=\mathbf{R}(\Delta\psi^\star) \) and \( \mathbf{t}^\star=\bar{\mathbf{q}}-\mathbf{R}^\star\bar{\mathbf{p}} \). This is especially convenient for embedded implementations (Java/MATLAB) and lab-scale coding.

Real AMR scans violate ideal ICP assumptions due to partial overlap (limited field-of-view), dynamic objects (people, doors), and geometry degeneracy (corridors). We therefore add robustification components that you can tune and evaluate:

(A) Distance gating. Reject pairs whose residual norm exceeds a threshold \( d_i > d_{\max} \).

(B) Trimming. Keep only the best fraction \( \rho \in (0,1] \) of correspondences with smallest residuals (trimmed ICP). If \( m=\lfloor \rho N \rfloor \), define an inlier set \( \mathcal{I} \) of size \( m \) and optimize only over \( i\in\mathcal{I} \).

(C) Robust M-estimation (Huber). With residual magnitudes \( r_i=\left\lVert \mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i \right\rVert_2 \), Huber weights are:

\[ w_i = \begin{cases} 1, & r_i \le \delta \\ \frac{\delta}{r_i}, & r_i > \delta \end{cases} \qquad\Rightarrow\qquad \min_{\mathbf{R},\mathbf{t}} \sum_i w_i\, r_i^2. \]

Weighted least squares changes only the means and cross-covariance: use weighted centroids \( \bar{\mathbf{p}}_w=\frac{\sum_i w_i\mathbf{p}_i}{\sum_i w_i} \) and \( \bar{\mathbf{q}}_w=\frac{\sum_i w_i\mathbf{q}_i}{\sum_i w_i} \), then \( \mathbf{H}_w=\sum_i w_i(\mathbf{p}_i-\bar{\mathbf{p}}_w)(\mathbf{q}_i-\bar{\mathbf{q}}_w)^\top \).

Convergence criteria. In practice, stop if any of the following holds:

\[ \lVert \Delta \mathbf{t} \rVert_2 < \varepsilon_t, \qquad |\Delta\psi| < \varepsilon_\psi, \qquad |J_{k}-J_{k-1}| < \varepsilon_J, \qquad k \ge k_{\max}. \]

Nearest-neighbor complexity. If you use brute-force association, complexity is \( \mathcal{O}(N_p N_q) \). With a KD-tree on the target scan, nearest-neighbor queries become approximately \( \mathcal{O}(N_p \log N_q) \). In the provided Python code, SciPy’s \( \texttt{cKDTree} \) is used when available.

Let the global robot pose at scan \( k \) be \( \mathbf{x}_k = (x_k, y_k, \psi_k) \). Suppose ICP returns \( (\Delta x, \Delta y, \Delta\psi) \) expressed in the scan-\( k \) frame (because we solved \( \mathbf{q}\approx \mathbf{R}\mathbf{p}+\mathbf{t} \)). Then the global pose update is:

\[ \begin{aligned} \psi_{k+1} &= \psi_k + \Delta\psi,\\ \begin{bmatrix}x_{k+1}\\y_{k+1}\end{bmatrix} &= \begin{bmatrix}x_k\\y_k\end{bmatrix} + \mathbf{R}(\psi_k) \begin{bmatrix}\Delta x\\\Delta y\end{bmatrix}. \end{aligned} \]

Quality signals you should log in the lab: (i) final ICP RMSE, (ii) number of inliers after gating/trimming, (iii) estimated rotation magnitude, (iv) “degeneracy flags” (e.g., covariance ill-conditioning if you compute it; or simply detect corridor-like geometry via low spread orthogonal to motion).

graph TD
  A["ICP output: R,t and metrics (RMSE, inliers)"] --> B["Convert to delta pose: dx, dy, dpsi"]
  B --> C["Integrate pose: update x,y with rotation; update yaw by dpsi"]
  C --> D["Sanity checks: yaw bound; inlier ratio bound; RMSE bound"]
  D -->|pass| E["Publish scan-matching odometry"]
  D -->|fail| F["Fallback: reduce step; \nreinit from wheel/IMU; or skip update"]
  

Useful libraries (optional): NumPy for linear algebra; SciPy KD-tree for fast association; Open3D for 3D ICP workflows (conceptually similar); ROS/ROS2 scan-matching packages for integration testing. This lab’s code intentionally implements 2D ICP “from scratch” to expose the mathematics and robustness logic.

Code: \( \) Chapter10_Lesson5.py

# Chapter10_Lesson5.py
"""
Autonomous Mobile Robots — Chapter 10, Lesson 5
Lab: ICP-Based Motion Estimation (2D)

This script implements a robust 2D point-to-point ICP to estimate relative motion
between two LiDAR scans (represented as 2D point sets).

Dependencies: numpy, scipy (optional but recommended for KD-tree)
Run: python Chapter10_Lesson5.py
"""

from __future__ import annotations
import math
import numpy as np

try:
    from scipy.spatial import cKDTree as KDTree
    _HAS_SCIPY = True
except Exception:
    _HAS_SCIPY = False
    KDTree = None


def rot2(theta: float) -> np.ndarray:
    """2D rotation matrix."""
    c, s = math.cos(theta), math.sin(theta)
    return np.array([[c, -s],
                     [s,  c]], dtype=float)


def apply_transform(P: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray:
    """Apply rigid transform: P' = R P + t."""
    return (R @ P.T).T + t.reshape(1, 2)


def huber_weights(r: np.ndarray, delta: float) -> np.ndarray:
    """Huber weights on residual norms."""
    # r: (N,) residual magnitudes
    w = np.ones_like(r)
    mask = r > delta
    w[mask] = delta / (r[mask] + 1e-12)
    return w


def weighted_centroid(P: np.ndarray, w: np.ndarray) -> np.ndarray:
    wsum = float(np.sum(w)) + 1e-12
    return (w.reshape(-1, 1) * P).sum(axis=0) / wsum


def best_fit_transform_2d(src: np.ndarray,
                          dst: np.ndarray,
                          w: np.ndarray | None = None) -> tuple[np.ndarray, np.ndarray]:
    """
    Compute optimal 2D rigid transform (R,t) minimizing sum_i w_i || R src_i + t - dst_i ||^2.
    Uses SVD (Kabsch) on 2x2 cross-covariance.
    """
    assert src.shape == dst.shape and src.shape[1] == 2
    N = src.shape[0]
    if w is None:
        w = np.ones(N, dtype=float)

    mu_s = weighted_centroid(src, w)
    mu_d = weighted_centroid(dst, w)

    X = src - mu_s
    Y = dst - mu_d

    # Weighted cross-covariance
    H = (w.reshape(-1, 1) * X).T @ Y  # 2x2

    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T

    # Ensure proper rotation (det = +1)
    if np.linalg.det(R) < 0:
        Vt[1, :] *= -1.0
        R = Vt.T @ U.T

    t = mu_d - R @ mu_s
    return R, t


def nearest_neighbors(src: np.ndarray, dst: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    """
    For each src point, find nearest neighbor in dst.
    Returns:
        nn: (N,2) matched dst points
        d2: (N,) squared distances
    """
    if _HAS_SCIPY:
        tree = KDTree(dst)
        d, idx = tree.query(src, k=1)
        return dst[idx], d**2
    # Fallback: brute-force (O(N^2))
    nn = np.empty_like(src)
    d2 = np.empty(src.shape[0], dtype=float)
    for i, p in enumerate(src):
        diffs = dst - p.reshape(1, 2)
        di2 = np.sum(diffs * diffs, axis=1)
        j = int(np.argmin(di2))
        nn[i] = dst[j]
        d2[i] = di2[j]
    return nn, d2


def icp_2d(src: np.ndarray,
           dst: np.ndarray,
           init_R: np.ndarray | None = None,
           init_t: np.ndarray | None = None,
           max_iter: int = 50,
           tol: float = 1e-6,
           reject_dist: float | None = None,
           trim_fraction: float | None = 0.8,
           huber_delta: float | None = 0.25) -> dict:
    """
    Robust 2D ICP (point-to-point).

    Args:
        src, dst: (N,2) and (M,2) point sets.
        init_R, init_t: optional initial guess mapping src -> dst.
        reject_dist: if set, reject correspondences with distance > reject_dist.
        trim_fraction: keep best fraction of correspondences by distance (0,1].
        huber_delta: if set, apply Huber weights based on residual norms.

    Returns: dict with R, t, history, rmse, iters.
    """
    src0 = np.array(src, dtype=float)
    dst0 = np.array(dst, dtype=float)

    if init_R is None:
        R = np.eye(2)
    else:
        R = np.array(init_R, dtype=float)

    if init_t is None:
        t = np.zeros(2)
    else:
        t = np.array(init_t, dtype=float).reshape(2,)

    history = []
    prev_rmse = None

    for it in range(max_iter):
        src_trans = apply_transform(src0, R, t)
        nn, d2 = nearest_neighbors(src_trans, dst0)
        d = np.sqrt(d2 + 1e-12)

        # Optional distance gating
        mask = np.ones(src_trans.shape[0], dtype=bool)
        if reject_dist is not None:
            mask &= (d <= reject_dist)

        # Optional trimming (keep smallest distances)
        if trim_fraction is not None and 0.0 < trim_fraction < 1.0:
            idx_sorted = np.argsort(d)
            k = max(3, int(trim_fraction * len(d)))
            keep = np.zeros_like(mask)
            keep[idx_sorted[:k]] = True
            mask &= keep

        src_m = src_trans[mask]
        nn_m = nn[mask]
        if src_m.shape[0] < 3:
            raise RuntimeError("Too few correspondences after filtering. Relax thresholds.")

        # Robust weights
        w = None
        if huber_delta is not None and huber_delta > 0:
            w = huber_weights(np.linalg.norm(src_m - nn_m, axis=1), huber_delta)

        dR, dt = best_fit_transform_2d(src_m, nn_m, w=w)

        # Compose: new transform maps original src0 -> dst0
        R = dR @ R
        t = dR @ t + dt

        rmse = float(np.sqrt(np.mean(np.sum((apply_transform(src0, R, t) - nearest_neighbors(apply_transform(src0, R, t), dst0)[0])**2, axis=1))))
        history.append(rmse)

        if prev_rmse is not None and abs(prev_rmse - rmse) < tol:
            break
        prev_rmse = rmse

    return {"R": R, "t": t, "rmse": history[-1], "history": history, "iters": len(history)}


def simulate_world_points(n: int = 400, seed: int = 7) -> np.ndarray:
    """
    Create a structured point set (walls + scatter) to mimic a 2D LiDAR environment.
    """
    rng = np.random.default_rng(seed)
    # Two perpendicular walls (an "L" corner)
    wall1_x = rng.uniform(0.0, 8.0, size=n//4)
    wall1_y = np.zeros_like(wall1_x)
    wall2_y = rng.uniform(0.0, 6.0, size=n//4)
    wall2_x = np.zeros_like(wall2_y)
    # Random scatter points
    scat = rng.uniform([0.0, 0.0], [8.0, 6.0], size=(n//2, 2))
    P = np.vstack([
        np.stack([wall1_x, wall1_y], axis=1),
        np.stack([wall2_x, wall2_y], axis=1),
        scat
    ])
    return P


def scan_from_pose(world_pts: np.ndarray, R_wb: np.ndarray, t_wb: np.ndarray,
                   noise_std: float = 0.01,
                   max_range: float = 10.0,
                   fov: float = math.radians(270.0),
                   seed: int = 0) -> np.ndarray:
    """
    Generate a 2D "scan" in the body (sensor) frame from world points.
    Pose is (R_wb, t_wb): world = R_wb * body + t_wb.
    Points returned are p_b = R_wb^T (p_w - t_wb), then filtered by range and FOV.
    """
    rng = np.random.default_rng(seed)
    R_bw = R_wb.T
    P_b = (R_bw @ (world_pts - t_wb.reshape(1, 2)).T).T

    r = np.linalg.norm(P_b, axis=1)
    ang = np.arctan2(P_b[:, 1], P_b[:, 0])

    # FOV symmetric around forward axis
    mask = (r <= max_range) & (np.abs(ang) <= fov / 2.0)

    P = P_b[mask].copy()
    P += rng.normal(0.0, noise_std, size=P.shape)

    # Add a few outliers (dynamic obstacles / spurious returns)
    k_out = max(5, P.shape[0] // 40)
    out = rng.uniform([-2.0, -2.0], [2.0, 2.0], size=(k_out, 2))
    P = np.vstack([P, out])
    return P


def main():
    # Create a world and two robot poses
    world = simulate_world_points(n=600, seed=7)

    # Pose k (identity)
    R0 = rot2(0.0)
    t0 = np.array([0.0, 0.0])

    # Pose k+1 (ground truth motion)
    theta_gt = math.radians(6.0)     # 6 degrees yaw
    t_gt = np.array([0.35, 0.10])    # 35 cm forward, 10 cm left (in world)

    R1 = rot2(theta_gt)
    t1 = t_gt.copy()

    # Generate scans in body frames
    scan_k  = scan_from_pose(world, R0, t0, noise_std=0.01, seed=1)
    scan_k1 = scan_from_pose(world, R1, t1, noise_std=0.01, seed=2)

    # Estimate motion that maps scan_{k+1} -> scan_k (should recover R1, t1)
    # Provide a weak initial guess (e.g., from wheel odometry)
    init_R = rot2(math.radians(3.0))
    init_t = np.array([0.20, 0.00])

    res = icp_2d(
        src=scan_k1,
        dst=scan_k,
        init_R=init_R,
        init_t=init_t,
        max_iter=60,
        tol=1e-7,
        reject_dist=0.5,
        trim_fraction=0.85,
        huber_delta=0.20
    )

    R_est, t_est = res["R"], res["t"]

    def rot_to_angle(R):
        return math.degrees(math.atan2(R[1, 0], R[0, 0]))

    print("=== ICP-Based Motion Estimation (2D) ===")
    print(f"Ground truth: theta = {math.degrees(theta_gt):.3f} deg, t = {t_gt}")
    print(f"Estimated   : theta = {rot_to_angle(R_est):.3f} deg, t = {t_est}")
    print(f"Iterations  : {res['iters']}, final RMSE ≈ {res['rmse']:.6f}")
    print(f"Used SciPy KDTree: {_HAS_SCIPY}")

    # Simple sanity check: transform scan_k1 into frame k and compute mean NN distance
    scan_k1_to_k = apply_transform(scan_k1, R_est, t_est)
    nn, _ = nearest_neighbors(scan_k1_to_k, scan_k)
    mean_err = np.mean(np.linalg.norm(scan_k1_to_k - nn, axis=1))
    print(f"Mean NN error after alignment: {mean_err:.6f} m")


if __name__ == "__main__":
    main()
      

Useful libraries (optional): Eigen for linear algebra (used here); PCL for point-cloud tooling; nanoflann for KD-trees; ROS2 for message I/O and benchmarking. For pedagogical clarity, this implementation uses brute-force nearest neighbors (small N).

Code: Chapter10_Lesson5.cpp

// Chapter10_Lesson5.cpp
/*
Autonomous Mobile Robots — Chapter 10, Lesson 5
Lab: ICP-Based Motion Estimation (2D)

A compact from-scratch 2D point-to-point ICP using:
- brute-force nearest neighbors (simple but O(N^2))
- closed-form 2D rotation estimate from cross-covariance
- optional distance gating and trimming

Dependencies: Eigen (header-only linear algebra)
Compile (example):
  g++ -O2 -std=c++17 Chapter10_Lesson5.cpp -I /path/to/eigen -o icp2d
Run:
  ./icp2d
*/

#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <algorithm>
#include <limits>
#include <Eigen/Dense>

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

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

static std::vector<Vec2> applyTransform(const std::vector<Vec2>& P, const Mat2& R, const Vec2& t) {
    std::vector<Vec2> out;
    out.reserve(P.size());
    for (const auto& p : P) out.push_back(R * p + t);
    return out;
}

static double norm2(const Vec2& v) { return v.squaredNorm(); }

static Vec2 centroid(const std::vector<Vec2>& P, const std::vector<double>* w = nullptr) {
    double ws = 0.0;
    Vec2 mu(0.0, 0.0);
    if (!w) {
        for (const auto& p : P) mu += p;
        return mu / std::max<size_t>(1, P.size());
    }
    for (size_t i = 0; i < P.size(); ++i) {
        mu += (*w)[i] * P[i];
        ws += (*w)[i];
    }
    return mu / (ws + 1e-12);
}

// Closed-form 2D optimal rotation from correspondences (no SVD needed).
// Given centered pairs (x_i, y_i), define:
//   a = sum( x_i dot y_i )
//   b = sum( x_i cross y_i )  (2D scalar cross: x_x y_y - x_y y_x)
// theta = atan2(b, a), R(theta) is optimal for least squares.
static void bestFit2D(const std::vector<Vec2>& src,
                      const std::vector<Vec2>& dst,
                      Mat2& R, Vec2& t,
                      const std::vector<double>* w = nullptr) {
    Vec2 mu_s = centroid(src, w);
    Vec2 mu_d = centroid(dst, w);

    double a = 0.0, b = 0.0;
    double ws = 0.0;
    for (size_t i = 0; i < src.size(); ++i) {
        Vec2 xs = src[i] - mu_s;
        Vec2 yd = dst[i] - mu_d;
        double wi = (w ? (*w)[i] : 1.0);
        a += wi * (xs.dot(yd));
        b += wi * (xs.x() * yd.y() - xs.y() * yd.x());
        ws += wi;
    }
    (void)ws;
    double theta = std::atan2(b, a);
    R = rot2(theta);
    t = mu_d - R * mu_s;
}

static size_t nearestIndex(const Vec2& p, const std::vector<Vec2>& Q) {
    size_t best = 0;
    double bestd = std::numeric_limits<double>::infinity();
    for (size_t j = 0; j < Q.size(); ++j) {
        double d = (Q[j] - p).squaredNorm();
        if (d < bestd) { bestd = d; best = j; }
    }
    return best;
}

struct ICPResult {
    Mat2 R;
    Vec2 t;
    int iters;
    double rmse;
};

static ICPResult icp2D(const std::vector<Vec2>& src0,
                       const std::vector<Vec2>& dst0,
                       Mat2 R, Vec2 t,
                       int maxIter = 60,
                       double tol = 1e-7,
                       double rejectDist = 0.5,   // meters
                       double trimFraction = 0.85 // keep best fraction
) {
    std::vector<double> rmseHist;
    rmseHist.reserve(maxIter);

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

    for (int it = 0; it < maxIter; ++it) {
        auto srcT = applyTransform(src0, R, t);

        // Build correspondences
        std::vector<Vec2> srcM, dstM;
        std::vector<double> dists;
        srcM.reserve(srcT.size());
        dstM.reserve(srcT.size());
        dists.reserve(srcT.size());

        for (size_t i = 0; i < srcT.size(); ++i) {
            size_t j = nearestIndex(srcT[i], dst0);
            double d = (dst0[j] - srcT[i]).norm();
            if (d <= rejectDist) {
                srcM.push_back(srcT[i]);
                dstM.push_back(dst0[j]);
                dists.push_back(d);
            }
        }

        if (srcM.size() < 3) throw std::runtime_error("Too few correspondences after gating.");

        // Trimming: keep smallest distances
        std::vector<size_t> idx(srcM.size());
        for (size_t i = 0; i < idx.size(); ++i) idx[i] = i;
        std::sort(idx.begin(), idx.end(), [&](size_t a, size_t b){ return dists[a] < dists[b]; });
        size_t k = std::max<size_t>(3, static_cast<size_t>(trimFraction * idx.size()));

        std::vector<Vec2> srcK, dstK;
        srcK.reserve(k); dstK.reserve(k);
        for (size_t ii = 0; ii < k; ++ii) {
            srcK.push_back(srcM[idx[ii]]);
            dstK.push_back(dstM[idx[ii]]);
        }

        Mat2 dR; Vec2 dt;
        bestFit2D(srcK, dstK, dR, dt);

        // Compose update
        R = dR * R;
        t = dR * t + dt;

        // RMSE on the kept set
        double sse = 0.0;
        for (size_t i = 0; i < k; ++i) {
            Vec2 e = (dR * srcK[i] + dt) - dstK[i];
            sse += e.squaredNorm();
        }
        double rmse = std::sqrt(sse / std::max<size_t>(1, k));
        rmseHist.push_back(rmse);

        if (std::abs(prev - rmse) < tol) {
            return {R, t, it + 1, rmse};
        }
        prev = rmse;
    }
    return {R, t, maxIter, rmseHist.empty() ? 0.0 : rmseHist.back()};
}

static std::vector<Vec2> simulateWorld(size_t n, int seed) {
    std::mt19937 rng(seed);
    std::uniform_real_distribution<double> ux(0.0, 8.0), uy(0.0, 6.0);

    std::vector<Vec2> P;
    P.reserve(n);

    // L-corner walls
    for (size_t i = 0; i < n/4; ++i) P.push_back(Vec2(ux(rng), 0.0));
    for (size_t i = 0; i < n/4; ++i) P.push_back(Vec2(0.0, uy(rng)));

    // Scatter
    for (size_t i = 0; i < n/2; ++i) P.push_back(Vec2(ux(rng), uy(rng)));
    return P;
}

static std::vector<Vec2> scanFromPose(const std::vector<Vec2>& world,
                                      const Mat2& R_wb, const Vec2& t_wb,
                                      double noiseStd, int seed) {
    std::mt19937 rng(seed);
    std::normal_distribution<double> n01(0.0, noiseStd);

    Mat2 R_bw = R_wb.transpose();
    std::vector<Vec2> scan;
    scan.reserve(world.size());

    double maxRange = 10.0;
    double fov = 270.0 * M_PI / 180.0;

    for (const auto& Xw : world) {
        Vec2 Xb = R_bw * (Xw - t_wb);
        double r = Xb.norm();
        double a = std::atan2(Xb.y(), Xb.x());
        if (r <= maxRange && std::abs(a) <= fov/2.0) {
            Xb.x() += n01(rng);
            Xb.y() += n01(rng);
            scan.push_back(Xb);
        }
    }

    // A few outliers
    std::uniform_real_distribution<double> uo(-2.0, 2.0);
    size_t kout = std::max<size_t>(5, scan.size()/40);
    for (size_t i = 0; i < kout; ++i) scan.push_back(Vec2(uo(rng), uo(rng)));

    return scan;
}

static double angleDeg(const Mat2& R) {
    return std::atan2(R(1,0), R(0,0)) * 180.0 / M_PI;
}

int main() {
    auto world = simulateWorld(700, 7);

    Mat2 R0 = rot2(0.0);
    Vec2 t0(0.0, 0.0);

    double thetaGT = 6.0 * M_PI / 180.0;
    Mat2 R1 = rot2(thetaGT);
    Vec2 t1(0.35, 0.10);

    auto scanK  = scanFromPose(world, R0, t0, 0.01, 1);
    auto scanK1 = scanFromPose(world, R1, t1, 0.01, 2);

    // Initial guess (odometry-like)
    Mat2 Rinit = rot2(3.0 * M_PI / 180.0);
    Vec2 tinit(0.20, 0.0);

    auto res = icp2D(scanK1, scanK, Rinit, tinit, 60, 1e-7, 0.5, 0.85);

    std::cout << "=== ICP-Based Motion Estimation (2D) ===\n";
    std::cout << "Ground truth: theta = " << (thetaGT * 180.0 / M_PI) << " deg, "
              << "t = [" << t1.x() << ", " << t1.y() << "]\n";
    std::cout << "Estimated   : theta = " << angleDeg(res.R) << " deg, "
              << "t = [" << res.t.x() << ", " << res.t.y() << "]\n";
    std::cout << "Iterations  : " << res.iters << ", final RMSE ≈ " << res.rmse << "\n";

    return 0;
}
      

Useful libraries (optional): EJML or Apache Commons Math for linear algebra/SVD. For this lab, we use the 2D closed-form rotation update (no SVD required), which keeps the Java implementation dependency-free and emphasizes the underlying geometry.

Code: Chapter10_Lesson5.java

// Chapter10_Lesson5.java
/*
Autonomous Mobile Robots — Chapter 10, Lesson 5
Lab: ICP-Based Motion Estimation (2D)

A dependency-free 2D ICP example (no external linear algebra needed).
- nearest-neighbor correspondences via brute force
- closed-form 2D least-squares rotation:
    a = sum( x·y ), b = sum( x cross y ), theta = atan2(b, a)

Compile:
  javac Chapter10_Lesson5.java
Run:
  java Chapter10_Lesson5
*/

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

public class Chapter10_Lesson5 {

    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 cross(Vec2 o){ return x*o.y - y*o.x; } // 2D scalar cross
        double norm(){ return sqrt(x*x + y*y); }
    }

    static class Mat2 {
        double a,b,c,d; // [[a,b],[c,d]]
        Mat2(double a,double b,double c,double d){ this.a=a; this.b=b; this.c=c; this.d=d; }
        static Mat2 rot(double th){
            double cs = cos(th), sn = sin(th);
            return new Mat2(cs, -sn, sn, cs);
        }
        Vec2 mul(Vec2 v){ return new Vec2(a*v.x + b*v.y, c*v.x + d*v.y); }
        Mat2 mul(Mat2 o){
            return new Mat2(
                a*o.a + b*o.c, a*o.b + b*o.d,
                c*o.a + d*o.c, c*o.b + d*o.d
            );
        }
        Mat2 T(){ return new Mat2(a,c,b,d); }
    }

    static Vec2 centroid(List<Vec2> P){
        Vec2 s = new Vec2(0,0);
        for (Vec2 p: P) s = s.add(p);
        return s.mul(1.0 / max(1, P.size()));
    }

    static List<Vec2> apply(List<Vec2> P, Mat2 R, Vec2 t){
        ArrayList<Vec2> out = new ArrayList<>(P.size());
        for (Vec2 p: P) out.add(R.mul(p).add(t));
        return out;
    }

    static int nnIndex(Vec2 p, List<Vec2> Q){
        int best = 0;
        double bestd = Double.POSITIVE_INFINITY;
        for (int j=0;j<Q.size();j++){
            Vec2 d = Q.get(j).sub(p);
            double dd = d.x*d.x + d.y*d.y;
            if (dd < bestd){ bestd = dd; best = j; }
        }
        return best;
    }

    static void bestFit2D(List<Vec2> src, List<Vec2> dst, Mat2[] Rout, Vec2[] tout){
        Vec2 muS = centroid(src);
        Vec2 muD = centroid(dst);

        double A = 0.0, B = 0.0;
        for (int i=0;i<src.size();i++){
            Vec2 xs = src.get(i).sub(muS);
            Vec2 yd = dst.get(i).sub(muD);
            A += xs.dot(yd);
            B += xs.cross(yd);
        }
        double theta = atan2(B, A);
        Mat2 R = Mat2.rot(theta);
        Vec2 t = muD.sub(R.mul(muS));

        Rout[0] = R;
        tout[0] = t;
    }

    static class ICPResult {
        Mat2 R; Vec2 t; int iters; double rmse;
        ICPResult(Mat2 R, Vec2 t, int iters, double rmse){
            this.R=R; this.t=t; this.iters=iters; this.rmse=rmse;
        }
    }

    static ICPResult icp2D(List<Vec2> src0, List<Vec2> dst0,
                           Mat2 R, Vec2 t,
                           int maxIter, double tol,
                           double rejectDist, double trimFraction){
        double prev = Double.POSITIVE_INFINITY;

        for (int it=0; it<maxIter; it++){
            List<Vec2> srcT = apply(src0, R, t);

            ArrayList<Vec2> srcM = new ArrayList<>();
            ArrayList<Vec2> dstM = new ArrayList<>();
            ArrayList<Double> dist = new ArrayList<>();

            for (int i=0;i<srcT.size();i++){
                Vec2 p = srcT.get(i);
                int j = nnIndex(p, dst0);
                double d = dst0.get(j).sub(p).norm();
                if (d <= rejectDist){
                    srcM.add(p);
                    dstM.add(dst0.get(j));
                    dist.add(d);
                }
            }
            if (srcM.size() < 3) throw new RuntimeException("Too few correspondences after gating.");

            // trimming
            Integer[] idx = new Integer[srcM.size()];
            for (int i=0;i<idx.length;i++) idx[i]=i;
            Arrays.sort(idx, Comparator.comparingDouble(i -> dist.get(i)));
            int k = max(3, (int)(trimFraction * idx.length));

            ArrayList<Vec2> srcK = new ArrayList<>(k);
            ArrayList<Vec2> dstK = new ArrayList<>(k);
            for (int ii=0; ii<k; ii++){
                int i = idx[ii];
                srcK.add(srcM.get(i));
                dstK.add(dstM.get(i));
            }

            Mat2[] dR = new Mat2[1];
            Vec2[] dt = new Vec2[1];
            bestFit2D(srcK, dstK, dR, dt);

            // compose update
            R = dR[0].mul(R);
            t = dR[0].mul(t).add(dt[0]);

            // RMSE on trimmed set after update (approx)
            double sse = 0.0;
            for (int i=0;i<k;i++){
                Vec2 e = dR[0].mul(srcK.get(i)).add(dt[0]).sub(dstK.get(i));
                sse += e.x*e.x + e.y*e.y;
            }
            double rmse = sqrt(sse / max(1, k));

            if (abs(prev - rmse) < tol) return new ICPResult(R, t, it+1, rmse);
            prev = rmse;
        }
        return new ICPResult(R, t, maxIter, prev);
    }

    static List<Vec2> simulateWorld(int n, Random rng){
        ArrayList<Vec2> P = new ArrayList<>(n);
        for (int i=0;i<n/4;i++) P.add(new Vec2(rng.nextDouble()*8.0, 0.0));
        for (int i=0;i<n/4;i++) P.add(new Vec2(0.0, rng.nextDouble()*6.0));
        for (int i=0;i<n/2;i++) P.add(new Vec2(rng.nextDouble()*8.0, rng.nextDouble()*6.0));
        return P;
    }

    static List<Vec2> scanFromPose(List<Vec2> world, Mat2 R_wb, Vec2 t_wb,
                                  double noiseStd, Random rng){
        Mat2 R_bw = R_wb.T();
        ArrayList<Vec2> scan = new ArrayList<>();
        double maxRange = 10.0;
        double fov = toRadians(270.0);

        for (Vec2 Xw: world){
            Vec2 Xb = R_bw.mul(Xw.sub(t_wb));
            double r = Xb.norm();
            double a = atan2(Xb.y, Xb.x);
            if (r <= maxRange && abs(a) <= fov/2.0){
                double nx = noiseStd * rng.nextGaussian();
                double ny = noiseStd * rng.nextGaussian();
                scan.add(new Vec2(Xb.x + nx, Xb.y + ny));
            }
        }

        int kout = max(5, scan.size()/40);
        for (int i=0;i<kout;i++){
            double ox = -2.0 + 4.0*rng.nextDouble();
            double oy = -2.0 + 4.0*rng.nextDouble();
            scan.add(new Vec2(ox, oy));
        }
        return scan;
    }

    static double angleDeg(Mat2 R){
        return toDegrees(atan2(R.c, R.a));
    }

    public static void main(String[] args){
        Random rng = new Random(7);

        List<Vec2> world = simulateWorld(700, rng);

        Mat2 R0 = Mat2.rot(0.0);
        Vec2 t0 = new Vec2(0.0, 0.0);

        double thetaGT = toRadians(6.0);
        Mat2 R1 = Mat2.rot(thetaGT);
        Vec2 t1 = new Vec2(0.35, 0.10);

        List<Vec2> scanK  = scanFromPose(world, R0, t0, 0.01, new Random(1));
        List<Vec2> scanK1 = scanFromPose(world, R1, t1, 0.01, new Random(2));

        Mat2 Rinit = Mat2.rot(toRadians(3.0));
        Vec2 tinit = new Vec2(0.20, 0.0);

        ICPResult res = icp2D(scanK1, scanK, Rinit, tinit, 60, 1e-7, 0.5, 0.85);

        System.out.println("=== ICP-Based Motion Estimation (2D) ===");
        System.out.printf("Ground truth: theta = %.3f deg, t = [%.3f, %.3f]%n",
                          toDegrees(thetaGT), t1.x, t1.y);
        System.out.printf("Estimated   : theta = %.3f deg, t = [%.3f, %.3f]%n",
                          angleDeg(res.R), res.t.x, res.t.y);
        System.out.printf("Iterations  : %d, final RMSE ≈ %.6f%n", res.iters, res.rmse);
    }
}
      

Useful toolboxes (optional): Robotics System Toolbox and ROS Toolbox (for real scan ingestion). For this lab, we provide a clean MATLAB implementation of 2D ICP using the 2D closed-form rotation. For Simulink, you typically wrap the ICP step in a MATLAB Function block and run a reduced iteration budget for real-time behavior.

Code: Chapter10_Lesson5.m

% Chapter10_Lesson5.m
% Autonomous Mobile Robots — Chapter 10, Lesson 5
% Lab: ICP-Based Motion Estimation (2D)
%
% This script implements a basic 2D point-to-point ICP to estimate relative
% motion between two LiDAR scans (2D point sets).
%
% MATLAB toolboxes (optional):
% - Robotics System Toolbox (for pointCloud, pcregistericp in 3D workflows)
% - ROS Toolbox (for reading LaserScan messages)
%
% Run:
%   Chapter10_Lesson5

function Chapter10_Lesson5()

rng(7);

% ----- Simulate world points (L-corner + scatter) -----
n = 700;
world = [8*rand(n/4,1), zeros(n/4,1); ...
         zeros(n/4,1), 6*rand(n/4,1); ...
         8*rand(n/2,1), 6*rand(n/2,1)];

% Pose k (identity)
R0 = rot2(0);
t0 = [0;0];

% Pose k+1 (ground truth motion)
thetaGT = deg2rad(6);
R1 = rot2(thetaGT);
t1 = [0.35; 0.10];

scanK  = scanFromPose(world, R0, t0, 0.01, 1);
scanK1 = scanFromPose(world, R1, t1, 0.01, 2);

% Weak initial guess (odometry-like)
Rinit = rot2(deg2rad(3));
tinit = [0.20; 0.00];

% ----- Run ICP: estimate transform mapping scan_{k+1} -> scan_k -----
opts.maxIter = 60;
opts.tol = 1e-7;
opts.rejectDist = 0.5;
opts.trimFraction = 0.85;

[Rhat, that, rmseHist] = icp2d(scanK1, scanK, Rinit, tinit, opts);

thetaHat = atan2(Rhat(2,1), Rhat(1,1));

fprintf("=== ICP-Based Motion Estimation (2D) ===\n");
fprintf("Ground truth: theta = %.3f deg, t = [%.3f, %.3f]\n", rad2deg(thetaGT), t1(1), t1(2));
fprintf("Estimated   : theta = %.3f deg, t = [%.3f, %.3f]\n", rad2deg(thetaHat), that(1), that(2));
fprintf("Iterations  : %d, final RMSE ≈ %.6f\n", numel(rmseHist), rmseHist(end));

% ----- Simulink note -----
% To use inside Simulink:
% 1) Create a MATLAB Function block.
% 2) Put a persistent previousScan and previousPose in the block.
% 3) Call a helper like icp2d() to compute [Rhat,that] between scans.
% 4) Convert [Rhat,that] to delta pose and integrate.
%
% Keep the ICP iteration budget small (e.g., 10-20) for real-time execution.

end

% ---------------- Helpers ----------------

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

function scan = scanFromPose(world, R_wb, t_wb, noiseStd, seed)
rng(seed);
R_bw = R_wb';
P = (R_bw * (world' - t_wb)).';  % body-frame points

r = sqrt(sum(P.^2,2));
ang = atan2(P(:,2), P(:,1));
maxRange = 10.0;
fov = deg2rad(270);

mask = (r <= maxRange) & (abs(ang) <= fov/2);
scan = P(mask,:) + noiseStd*randn(sum(mask),2);

% add outliers
kout = max(5, floor(size(scan,1)/40));
out = -2 + 4*rand(kout,2);
scan = [scan; out];
end

function [R, t, rmseHist] = icp2d(src0, dst0, R0, t0, opts)
R = R0; t = t0(:);
rmseHist = [];
prev = inf;

for it = 1:opts.maxIter
    srcT = (R*src0.' + t).';  % transform src

    [nn, dist] = nearestNeighbors(srcT, dst0);

    % gating
    mask = dist <= opts.rejectDist;

    % trimming
    dist2 = dist(mask);
    srcM = srcT(mask,:);
    nnM  = nn(mask,:);
    if size(srcM,1) < 3
        error("Too few correspondences after filtering.");
    end

    if opts.trimFraction < 1
        [~, idx] = sort(dist2, 'ascend');
        k = max(3, floor(opts.trimFraction * numel(idx)));
        idx = idx(1:k);
        srcM = srcM(idx,:);
        nnM  = nnM(idx,:);
        dist2 = dist2(idx);
    end

    [dR, dt] = bestFit2D(srcM, nnM);
    R = dR * R;
    t = dR * t + dt;

    rmse = sqrt(mean(dist2.^2));
    rmseHist(end+1) = rmse; %#ok<AGROW>
    if abs(prev - rmse) < opts.tol
        break;
    end
    prev = rmse;
end

end

function [nn, dist] = nearestNeighbors(P, Q)
% brute-force NN (for clarity)
N = size(P,1);
nn = zeros(N,2);
dist = zeros(N,1);
for i = 1:N
    dif = Q - P(i,:);
    d2 = sum(dif.^2,2);
    [m, j] = min(d2);
    nn(i,:) = Q(j,:);
    dist(i) = sqrt(m);
end
end

function [R, t] = bestFit2D(src, dst)
% Closed-form 2D least-squares rotation without SVD.
muS = mean(src,1).';
muD = mean(dst,1).';

Xs = src.' - muS;
Yd = dst.' - muD;

A = sum(Xs(1,:).*Yd(1,:) + Xs(2,:).*Yd(2,:));
B = sum(Xs(1,:).*Yd(2,:) - Xs(2,:).*Yd(1,:));
theta = atan2(B, A);
R = rot2(theta);
t = muD - R*muS;
end
      

Mathematica is convenient for verifying derivations and quickly prototyping alignment logic. The notebook below implements the closed-form 2D rotation and translation (for fixed correspondences), then demonstrates a synthetic example recovering a known motion.

Code: Chapter10_Lesson5.nb

(* Chapter10_Lesson5.nb *)
Notebook[{
 Cell["Autonomous Mobile Robots — Chapter 10, Lesson 5\nLab: ICP-Based Motion Estimation (2D)", "Section"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"rot2", "[", "\[Theta]_", "]"}], ":=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Cos", "[", "\[Theta]", "]"}], ",", 
       RowBox[{"-", 
        RowBox[{"Sin", "[", "\[Theta]", "]"}]}]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Sin", "[", "\[Theta]", "]"}], ",", 
       RowBox[{"Cos", "[", "\[Theta]", "]"}]}], "}"}]}], "}"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"bestFit2D", "[", 
    RowBox[{"src_", ",", "dst_"}], "]"}], ":=", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
      "\[Mu]s", ",", "\[Mu]d", ",", "Xs", ",", "Yd", ",", "A", ",", "B", ",", 
       "\[Theta]", ",", "R", ",", "t"}], "}"}], ",", "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"\[Mu]s", "=", 
       RowBox[{"Mean", "[", "src", "]"}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"\[Mu]d", "=", 
       RowBox[{"Mean", "[", "dst", "]"}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"Xs", "=", 
       RowBox[{"src", "-", "\[Mu]s"}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"Yd", "=", 
       RowBox[{"dst", "-", "\[Mu]d"}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"A", "=", 
       RowBox[{"Total", "[", 
        RowBox[{
         RowBox[{"Xs", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "1"}], "]"}], "]"}], "*", 
         RowBox[{"Yd", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "1"}], "]"}], "]"}], "+", 
         RowBox[{"Xs", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "2"}], "]"}], "]"}], "*", 
         RowBox[{"Yd", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "2"}], "]"}], "]"}]}], "]"}]}], ";", 
      "\[IndentingNewLine]", 
      RowBox[{"B", "=", 
       RowBox[{"Total", "[", 
        RowBox[{
         RowBox[{"Xs", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "1"}], "]"}], "]"}], "*", 
         RowBox[{"Yd", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "2"}], "]"}], "]"}], "-", 
         RowBox[{"Xs", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "2"}], "]"}], "]"}], "*", 
         RowBox[{"Yd", "[", 
          RowBox[{"[", 
           RowBox[{"All", ",", "1"}], "]"}], "]"}]}], "]"}]}], ";", 
      "\[IndentingNewLine]", 
      RowBox[{"\[Theta]", "=", 
       RowBox[{"ArcTan", "[", 
        RowBox[{"B", ",", "A"}], "]"}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"R", "=", 
       RowBox[{"rot2", "[", "\[Theta]", "]"}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"t", "=", 
       RowBox[{"\[Mu]d", "-", 
        RowBox[{"(", 
         RowBox[{"R", ".", "\[Mu]s"}], ")"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"{", 
       RowBox[{"R", ",", "t"}], "}"}]}]}], "]"}]}]], "Input"],
 Cell["Synthetic demo: recover a known motion between two scans.", "Text"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"SeedRandom", "[", "7", "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"world", "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"RandomReal", "[", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"0", ",", "8"}], "}"}], ",", "150"}], "]"}], ",", 
         RowBox[{"ConstantArray", "[", 
          RowBox[{"0", ",", "150"}], "]"}]}], "}"}], "]"}], ",", 
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"ConstantArray", "[", 
          RowBox[{"0", ",", "150"}], "]"}], ",", 
         RowBox[{"RandomReal", "[", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"0", ",", "6"}], "}"}], ",", "150"}], "]"}]}], "}"}], 
       "]"}], ",", 
      RowBox[{"RandomReal", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"0", ",", "0"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"8", ",", "6"}], "}"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"300", ",", "2"}], "}"}]}], "]"}]}], "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{"thetaGT", "=", 
    RowBox[{"6", "Degree"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{"R1", "=", 
    RowBox[{"rot2", "[", "thetaGT", "]"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{"t1", "=", 
    RowBox[{"{", 
     RowBox[{"0.35", ",", "0.10"}], "}"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{"scanK", "=", "world"}], ";", "\[IndentingNewLine]", 
   RowBox[{"scanK1", "=", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       RowBox[{"Transpose", "[", "R1", "]"}], ".", 
       RowBox[{"Transpose", "[", 
        RowBox[{"world", "-", "t1"}], "]"}]}], ")"}], "\[Transpose]"}]}], ";",
    "\[IndentingNewLine]", 
   RowBox[{"{", 
    RowBox[{"Rhat", ",", "that"}], "}"}], "=", 
   RowBox[{"bestFit2D", "[", 
    RowBox[{"scanK1", ",", "scanK"}], "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"Print", "[", 
    RowBox[{"\"thetaGT(deg)=\"", ",", 
     RowBox[{"N", "[", 
      RowBox[{"thetaGT", "/", "Degree"}], "]"}], ",", "\"  thetaHat(deg)=\"", 
     ",", 
     RowBox[{"N", "[", 
      RowBox[{
       RowBox[{"ArcTan", "[", 
        RowBox[{"Rhat", "[", 
         RowBox[{"[", 
          RowBox[{"2", ",", "1"}], "]"}], "]"}], ",", 
        RowBox[{"Rhat", "[", 
         RowBox[{"[", 
          RowBox[{"1", ",", "1"}], "]"}], "]"}]}], "]"}], "/", "Degree"}], 
      "]"}]}], "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"Print", "[", 
    RowBox[{"\"tGT=\"", ",", "t1", ",", "\"  tHat=\"", ",", "that"}], "]"}], 
   ";"}]], "Input"]
},
WindowSize->{1100, 700},
WindowTitle->"Chapter10_Lesson5.nb"
]
      

Problem 1 (Optimal translation): Starting from \( J(\mathbf{R},\mathbf{t})=\sum_{i=1}^{m}\left\lVert \mathbf{R}\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i\right\rVert_2^2 \), derive \( \mathbf{t}^\star=\bar{\mathbf{q}}-\mathbf{R}\bar{\mathbf{p}} \) for fixed \( \mathbf{R} \).

Solution: Expand and differentiate w.r.t. \( \mathbf{t} \): \( \frac{\partial J}{\partial \mathbf{t}} = 2\sum_i(\mathbf{R}\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i) \). Setting to zero yields \( m\mathbf{t} = \sum_i \mathbf{q}_i - \mathbf{R}\sum_i \mathbf{p}_i \), hence \( \mathbf{t}^\star=\bar{\mathbf{q}}-\mathbf{R}\bar{\mathbf{p}} \).

Problem 2 (2D closed-form rotation): For centered correspondences \( \mathbf{x}_i=[x_i,y_i]^\top \) and \( \mathbf{y}_i=[u_i,v_i]^\top \), show that maximizing \( \operatorname{tr}(\mathbf{R}(\theta)\mathbf{H}) \) is equivalent to maximizing \( a\cos\theta + b\sin\theta \), and conclude \( \theta^\star=\operatorname{atan2}(b,a) \).

Solution: In 2D, write the objective after centering as \( \sum_i \mathbf{y}_i^\top \mathbf{R}(\theta)\mathbf{x}_i \). Using \( \mathbf{R}(\theta)\mathbf{x}_i = [x_i\cos\theta - y_i\sin\theta,\; x_i\sin\theta + y_i\cos\theta]^\top \), we obtain \( \sum_i (u_i x_i + v_i y_i)\cos\theta + \sum_i (u_i(-y_i)+v_i x_i)\sin\theta \). Let \( a=\sum_i(x_i u_i + y_i v_i) \) and \( b=\sum_i(x_i v_i - y_i u_i) \). Then the objective is \( a\cos\theta + b\sin\theta \), maximized at \( \theta^\star=\operatorname{atan2}(b,a) \).

Problem 3 (Trimming effect): Suppose \( 30\% \) of points in scan \( k+1 \) come from a moving object and thus have no consistent match in scan \( k \). If you choose trimming fraction \( \rho=0.7 \), argue why trimmed ICP can remain stable. What happens if \( \rho=0.95 \)?

Solution: With \( \rho=0.7 \), the optimization can discard up to \( 30\% \) highest residual pairs, which likely correspond to the moving object, leaving mostly static-scene correspondences that satisfy a single rigid transform. With \( \rho=0.95 \), many outliers remain in the inlier set; the least-squares update becomes biased and can pull the transform toward dynamic geometry, increasing drift and possibly causing divergence.

Problem 4 (Convergence vs. local minima): Give a geometric scenario where ICP converges to an incorrect local minimum even with a small final RMSE, and explain how an initial guess helps.

Solution: In a corridor with near-parallel walls, many points can slide along the corridor direction while maintaining small point-to-point distances, producing multiple transforms with similar costs (a “valley” in the objective). ICP may converge to the wrong translation along that direction. A good initial guess (from wheel/IMU odometry) breaks symmetry and biases ICP toward the physically plausible solution.

Problem 5 (Odometry integration): If ICP yields \( (\Delta x,\Delta y,\Delta\psi) \) in the scan-\( k \) frame, derive the global update \( [x_{k+1},y_{k+1}]^\top = [x_k,y_k]^\top + \mathbf{R}(\psi_k)[\Delta x,\Delta y]^\top \).

Solution: The increment is expressed in the robot/body coordinates at time \( k \). To add it to global coordinates, rotate the increment by the current global heading \( \psi_k \) via \( \mathbf{R}(\psi_k) \), then translate the global position accordingly.

This lab implemented scan-to-scan motion estimation using ICP in 2D: (i) formulate rigid alignment, (ii) derive the closed-form optimal transform for fixed correspondences (SVD/Kabsch and the 2D atan2 form), (iii) build a robust ICP loop with gating/trimming/Huber weighting, and (iv) integrate the resulting transform into scan-matching odometry with quality-driven sanity checks. These components provide a practical foundation for SLAM front-ends (next chapters) while remaining consistent with the AMR-centric pipeline.