In Lessons 1–3, scan matching was implicitly “static”: the points in scan A and scan B were assumed to observe the same surfaces. In practice, a mobile robot often sees unmodeled movers: pedestrians, carts, doors, or even vegetation. These points violate the correspondence assumption behind ICP and correlative matching: a moving object produces matches with systematic residuals. If enough such points exist, the optimizer is pulled toward a pose that explains the movers rather than the static background (bias).

A simple viewpoint is: scan matching is estimation with gross errors. We must either (i) downweight / reject inconsistent points, or (ii) explicitly model the probability that each measurement is from a static surface.

flowchart TD
  A["Input: scan_t (current) + scan_{t-1} / local map"] --> B["Preprocess: range limits, downsample, deskew (if available)"]
  B --> C["Initial guess from odom/IMU"]
  C --> D["Build correspondences: NN / projective / correlation"]
  D --> E["Dynamic-robust filtering: \n(a) gating (b) robust weights (c) trimming (d) temporal persistence"]
  E --> F["Solve pose update: weighted least squares / Gauss-Newton"]
  F --> G["Convergence check: cost drop + inlier ratio"]
  G -->|"ok"| H["Output pose + quality metrics (inlier ratio, covariance)"]
  G -->|"not ok"| C
        

Key AMR takeaway: robust scan matching is not a single “trick”. It is a pipeline that combines geometry (residuals), statistics (robust loss / likelihood), and temporal reasoning (what persists over time is likely static).

Consider two 2D point sets (e.g., consecutive LiDAR scans) in their local frames: \( \left\{\mathbf{p}_i\right\}_{i=1}^N \subset \mathbb{R}^2 \) (source) and \( \left\{\mathbf{q}_j\right\}_{j=1}^M \subset \mathbb{R}^2 \) (target). A rigid-body transform in SE(2) is \( \mathbf{T} = (\mathbf{R}(\theta), \mathbf{t}) \), with \( \mathbf{R}(\theta)\in SO(2) \) and \( \mathbf{t}\in \mathbb{R}^2 \).

With a correspondence function \( \pi(i) \in \{1,\dots,M\} \) (e.g., nearest neighbor in Lesson 2), the classic point-to-point ICP objective is

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

Define the residual vector for point i as \( \mathbf{r}_i(\mathbf{T}) \): \( \mathbf{r}_i(\mathbf{T}) = \mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{q}_{\pi(i)} \). In a static world with small Gaussian measurement noise, least squares is statistically optimal (maximum likelihood).

Dynamic obstacles violate the Gaussian noise assumption: movers generate gross residuals with heavy tails. Least squares has unbounded influence: a single huge residual can shift the solution.

A standard robustification replaces the quadratic loss with a robust loss \( \rho(\cdot) \):

\[ \min_{\mathbf{T} } \; \sum_{i=1}^{N} \rho\!\left( s_i(\mathbf{T}) \right), \quad s_i(\mathbf{T}) = \left\lVert \mathbf{r}_i(\mathbf{T}) \right\rVert_2. \]

The loss grows slower than \( s^2 \) for large residuals, reducing sensitivity to movers. Two canonical choices:

Huber loss (quadratic near 0, linear in the tails):

\[ \rho_\delta(s) = \begin{cases} \tfrac{1}{2}s^2 & \text{if } s \;\; <= \;\; \delta \\ \delta\left(s - \tfrac{1}{2}\delta\right) & \text{if } s \;\; > \;\; \delta \end{cases} \]

Tukey biweight (redescending influence; can fully reject large residuals):

\[ \rho_c(s) = \begin{cases} \tfrac{c^2}{6}\left[1 - \left(1-\left(\tfrac{s}{c}\right)^2\right)^3\right] & \text{if } s \;\; <= \;\; c \\ \tfrac{c^2}{6} & \text{if } s \;\; > \;\; c \end{cases} \]

3.1 Deriving IRLS (Iteratively Reweighted Least Squares)

Let \( \psi(s)=\tfrac{d}{ds}\rho(s) \) be the influence function. Consider the objective \( J(\mathbf{T}) = \sum_i \rho(s_i(\mathbf{T})) \):. For a small increment \( \Delta \boldsymbol{\xi}\in\mathbb{R}^3 \) in SE(2) (angle + translation), linearize residuals (as in Gauss–Newton): \( \mathbf{r}_i(\mathbf{T}\oplus \Delta \boldsymbol{\xi}) \approx \mathbf{r}_i + \mathbf{J}_i \Delta\boldsymbol{\xi} \): where \( \mathbf{J}_i \) is the 2×3 Jacobian.

Using the chain rule, the first-order optimality condition for a stationary point is

\[ \nabla_{\Delta\boldsymbol{\xi} } J \;\approx\; \sum_{i=1}^N \psi(s_i)\; \frac{1}{s_i}\; \mathbf{J}_i^\top \mathbf{r}_i \;=\; \mathbf{0}, \quad \text{for } s_i \neq 0. \]

Define weights \( w_i = \frac{\psi(s_i)}{s_i} \): (and \( w_i=\psi'(0) \) at \( s_i=0 \) by continuity). Then the condition becomes the normal equation of a weighted least squares problem:

\[ \sum_{i=1}^N w_i\; \mathbf{J}_i^\top \mathbf{r}_i \;=\; \mathbf{0} \quad \Longleftrightarrow \quad \left(\sum_i \mathbf{J}_i^\top w_i \mathbf{J}_i\right)\Delta\boldsymbol{\xi} \;=\; -\sum_i \mathbf{J}_i^\top w_i \mathbf{r}_i. \]

Interpretation: robust scan matching can be implemented by iteratively: (i) computing residuals, (ii) converting them to weights \(w_i\), and (iii) solving a weighted least squares update. This is IRLS. For Huber, \( \psi_\delta(s)=s \) when \( s \;\;<=\;\;\delta \) and \( \psi_\delta(s)=\delta \) when \( s \;\;>\;\delta \), yielding weights \( w_i = 1 \) (inliers) and \( w_i = \delta/s_i \) (downweighted outliers).

A practical rule is to monitor the inlier ratio: \( \eta = \frac{1}{N}\sum_{i=1}^N \mathbb{I}[s_i \;\;<=\;\;\delta] \): if \( \eta \) collapses, either the initial guess is poor or the environment is dominated by movers.

Robust losses can be derived from explicit probabilistic models. A simple and effective model is a two-component mixture for each residual:

\[ p(\mathbf{r}_i) = \pi\;\mathcal{N}(\mathbf{r}_i;\mathbf{0},\sigma^2\mathbf{I}) + (1-\pi)\; \mathcal{U}(\mathbf{r}_i;\Omega), \]

where the Gaussian term represents static inliers and the uniform term represents dynamic/outlier measurements over a bounded residual region \( \Omega \). Introduce a latent indicator \( z_i\in\{0,1\} \): \( z_i=1 \) means “static/inlier”.

The posterior responsibility (E-step) is \( \gamma_i = P(z_i=1 \mid \mathbf{r}_i) \):

\[ \gamma_i = \frac{ \pi\;\mathcal{N}(\mathbf{r}_i;\mathbf{0},\sigma^2\mathbf{I}) }{ \pi\;\mathcal{N}(\mathbf{r}_i;\mathbf{0},\sigma^2\mathbf{I}) + (1-\pi)\;\frac{1}{|\Omega|} }. \]

In the M-step (pose update), the expected complete-data negative log-likelihood leads to a weighted least squares problem where the weights are exactly the responsibilities \( \gamma_i \). Thus, EM produces an IRLS-like algorithm, but with a statistical interpretation: points likely to be dynamic are assigned small \( \gamma_i \).

flowchart TD
  S0["Start with pose guess T0"] --> C0["Compute correspondences + residuals r_i"]
  C0 --> W["Compute weights: Huber w_i or EM responsibilities gamma_i"]
  W --> LS["Solve weighted update: (J^T W J) dx = -J^T W r"]
  LS --> UP["Update pose: T = T o Exp(dx)"]
  UP --> CHK["Stop? (cost drop + stable inlier ratio)"]
  CHK -->|"no"| C0
  CHK -->|"yes"| OUT["Output T + quality"]
        

This mixture view is especially useful for AMR because it suggests how to fuse other cues into \( \gamma_i \): e.g., if a point is in a region detected as “moving” by a tracker, lower \( \gamma_i \) even before matching.

Robust weighting is powerful, but dynamic obstacle robustness improves substantially when we exploit time. Static surfaces persist; movers do not. A lightweight test usable in real-time AMR:

Maintain a short window of scans (or a local submap) and define a “persistence score” for a measurement: \( \kappa_i \):

\[ \kappa_i = \frac{1}{L}\sum_{\ell=1}^{L} \mathbb{I}\!\left[ \left\lVert \mathbf{r}_{i}^{(\ell)} \right\rVert_2 \;\; <= \;\; \epsilon \right], \]

where \( \mathbf{r}_{i}^{(\ell)} \) is the residual of the point when matched against the local map constructed from scan \( t-\ell \), and \( \epsilon \) is a tight threshold based on sensor noise. Points with low persistence \( \kappa_i \) are likely dynamic and can be rejected (hard) or downweighted (soft).

A probabilistic variant uses a Bernoulli model: if a point is static, it matches with probability \( p_s \); if dynamic, \( p_d \ll p_s \). A likelihood ratio test decides “static vs dynamic” from the count of successful matches.

\[ \Lambda = \frac{p_s^{m}(1-p_s)^{L-m} }{p_d^{m}(1-p_d)^{L-m} }, \quad \text{decide static if } \log \Lambda \;\; > \;\; \tau. \]

This adds robustness with minimal new machinery: it only relies on repeated scan-to-map matching already in a localization pipeline.

5.1 Quality / covariance under outliers

A simple covariance proxy for the pose increment is the Gauss–Newton approximation: \( \mathbf{\Sigma}_{\Delta\xi} \approx \sigma^2 (\mathbf{J}^\top \mathbf{W}\mathbf{J})^{-1} \): when dynamic obstacles reduce inliers, \( \mathbf{W} \) shrinks and uncertainty grows. Reporting an inlier ratio and this covariance is practical for downstream navigation (e.g., triggering recovery behaviors in Chapter 14).

Robust threshold selection. For Huber, a common rule is \( \delta = k \hat{\sigma} \): with \( k \in [1.5,3] \), where \( \hat{\sigma} \) is a robust scale estimate of residuals (e.g., MAD).

\[ \hat{\sigma} = 1.4826\; \operatorname{median}_i\left(\left| s_i - \operatorname{median}_j(s_j) \right|\right). \]

Trimming ratio. If you expect up to 30% of points to be dynamic, use keep_ratio ≈ 0.7–0.85. Too aggressive trimming can remove useful geometry (degeneracy).

Degeneracy in corridors. In long corridors, translation along the corridor direction is weakly observable. Dynamic obstacles can worsen this by removing the few “informative” points. Detect it by monitoring the condition number of \( \mathbf{J}^\top \mathbf{W}\mathbf{J} \).

Interaction with correlative scan matching. Correlative methods can be made robust by (i) using occupancy-grid cells labeled “static” via persistence, and (ii) scoring with robust functions (e.g., saturating match scores for dense moving clusters).

Robotics ecosystem notes (Python): common stacks include ROS/ROS2 (rclpy), NumPy/SciPy, Open3D, and (for mapping) nav2-related tooling in ROS2. The code below is a minimal dependency-free implementation to expose the math.

Chapter10_Lesson4.py

# Chapter10_Lesson4.py
# Robust scan matching (2D) under dynamic obstacles using IRLS-Huber + trimming
# Dependencies: numpy (only)

import numpy as np


def rot2(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s],
                     [s,  c]], dtype=float)


def weighted_kabsch_2d(P, Q, w):
    # Solve: min_{R,t} sum_i w_i || R P_i + t - Q_i ||^2
    # P, Q: (N,2) arrays, w: (N,) nonnegative weights
    # Returns R (2x2), t (2,)
    w = np.asarray(w, dtype=float).reshape(-1)
    w_sum = np.sum(w) + 1e-12
    p_bar = (w[:, None] * P).sum(axis=0) / w_sum
    q_bar = (w[:, None] * Q).sum(axis=0) / w_sum

    X = P - p_bar
    Y = Q - q_bar

    # Cross-covariance S = sum w * (X)(Y)^T  (2x2)
    S = (w[:, None, None] * (X[:, :, None] @ Y[:, None, :])).sum(axis=0)

    # Closed-form 2D Kabsch: theta = atan2(S12 - S21, S11 + S22)
    num = S[0, 1] - S[1, 0]
    den = S[0, 0] + S[1, 1]
    theta = np.arctan2(num, den)
    R = rot2(theta)
    t = q_bar - R @ p_bar
    return R, t


def nearest_neighbors_bruteforce(A, B):
    # For each point in A (N,2), find closest in B (M,2).
    # Returns indices (N,), and squared distances (N,).
    diff = A[:, None, :] - B[None, :, :]
    d2 = np.sum(diff * diff, axis=2)
    idx = np.argmin(d2, axis=1)
    return idx, d2[np.arange(A.shape[0]), idx]


def huber_weights(r, delta):
    # r: residual norms (N,)
    # delta: Huber threshold
    # w = 1, if r <= delta
    #     delta/r, otherwise
    r = np.asarray(r, dtype=float)
    w = np.ones_like(r)
    mask = r > delta
    w[mask] = delta / (r[mask] + 1e-12)
    return w


def robust_icp_2d(source, target, iters=25, delta=0.25, keep_ratio=0.85):
    # Robust ICP using:
    #  - nearest neighbor correspondences
    #  - IRLS with Huber weights
    #  - trimming (keep only a ratio of best correspondences)
    # Returns R, t, and history dict.
    src = np.asarray(source, dtype=float)
    tgt = np.asarray(target, dtype=float)

    R = np.eye(2)
    t = np.zeros(2)

    hist = {"rmse": [], "inliers": []}

    for _ in range(iters):
        src_w = (src @ R.T) + t

        nn_idx, d2 = nearest_neighbors_bruteforce(src_w, tgt)
        Q = tgt[nn_idx]
        residuals = src_w - Q
        r = np.sqrt(np.sum(residuals * residuals, axis=1))

        # Trimming: keep smallest residuals
        thr = np.quantile(r, keep_ratio)
        inlier_mask = r <= thr
        P_in = src[inlier_mask]
        Q_in = Q[inlier_mask]
        r_in = r[inlier_mask]

        w = huber_weights(r_in, delta)

        dR, dt = weighted_kabsch_2d(P_in, Q_in, w)

        # Compose transforms: new (R,t) = (dR,dt) o (R,t)
        R = dR @ R
        t = dR @ t + dt

        rmse = float(np.sqrt(np.mean(r_in ** 2)))
        hist["rmse"].append(rmse)
        hist["inliers"].append(int(np.sum(inlier_mask)))

    return R, t, hist


def vanilla_icp_2d(source, target, iters=25):
    # Plain ICP (no robust weighting).
    src = np.asarray(source, dtype=float)
    tgt = np.asarray(target, dtype=float)

    R = np.eye(2)
    t = np.zeros(2)

    for _ in range(iters):
        src_w = (src @ R.T) + t
        nn_idx, _ = nearest_neighbors_bruteforce(src_w, tgt)
        Q = tgt[nn_idx]
        w = np.ones(src.shape[0])
        dR, dt = weighted_kabsch_2d(src, Q, w)
        R = dR @ R
        t = dR @ t + dt

    return R, t


def make_synthetic_scene(n_static=250, n_dyn=60, seed=7):
    # Create two scans:
    #  - static environment points
    #  - dynamic obstacle cluster that moves between scans
    #  - global robot motion between scans (unknown pose)
    rng = np.random.default_rng(seed)

    # Static points: circle + two lines
    ang = rng.uniform(0, 2*np.pi, size=n_static//2)
    circle = np.c_[2.0*np.cos(ang), 2.0*np.sin(ang)]
    line1 = np.c_[rng.uniform(-3, 3, size=n_static//4), -1.5*np.ones(n_static//4)]
    line2 = np.c_[1.5*np.ones(n_static//4), rng.uniform(-2, 2, size=n_static//4)]
    static = np.vstack([circle, line1, line2])

    # Dynamic obstacle cluster (e.g., a pedestrian)
    dyn_center_1 = np.array([-0.5, 0.8])
    dyn1 = dyn_center_1 + 0.15*rng.normal(size=(n_dyn, 2))

    # Robot motion between scans (true transform from scan1 -> scan2 frame)
    theta_true = np.deg2rad(12.0)
    t_true = np.array([0.35, -0.10])
    R_true = rot2(theta_true)

    # Scan1
    scan1 = np.vstack([static, dyn1])
    scan1 += 0.02*rng.normal(size=scan1.shape)

    # Dynamic obstacle moves
    dyn_center_2 = dyn_center_1 + np.array([0.55, -0.25])
    dyn2_world = dyn_center_2 + 0.15*rng.normal(size=(n_dyn, 2))

    # Scan2: static transformed, dynamic inconsistent
    static2 = (static @ R_true.T) + t_true
    dyn2 = dyn2_world

    scan2 = np.vstack([static2, dyn2])
    scan2 += 0.02*rng.normal(size=scan2.shape)

    return scan1, scan2, R_true, t_true


def pose_error(R_est, t_est, R_true, t_true):
    # rotation error angle
    dR = R_est @ R_true.T
    ang = np.arctan2(dR[1, 0], dR[0, 0])
    t_err = np.linalg.norm(t_est - t_true)
    return float(np.rad2deg(abs(ang))), float(t_err)


if __name__ == "__main__":
    scan1, scan2, R_true, t_true = make_synthetic_scene()

    # Vanilla ICP (often pulled toward moving cluster)
    R_v, t_v = vanilla_icp_2d(scan1, scan2, iters=25)
    aerr_v, terr_v = pose_error(R_v, t_v, R_true, t_true)

    # Robust ICP (Huber + trimming)
    R_r, t_r, hist = robust_icp_2d(scan1, scan2, iters=25, delta=0.18, keep_ratio=0.85)
    aerr_r, terr_r = pose_error(R_r, t_r, R_true, t_true)

    print("True theta(deg):", 12.0, " True t:", t_true)
    print("Vanilla ICP: angle_err(deg) =", aerr_v, " trans_err =", terr_v, " t_est =", t_v)
    print("Robust  ICP: angle_err(deg) =", aerr_r, " trans_err =", terr_r, " t_est =", t_r)
    print("Robust ICP RMSE history (first 5):", hist["rmse"][:5], " ... last:", hist["rmse"][-1])
      

Robotics ecosystem notes (C++): ICP is typically implemented via PCL, libpointmatcher, or custom code. In ROS/ROS2, scan matching nodes frequently use C++ for real-time constraints. The following Eigen-based code mirrors the Python logic.

Chapter10_Lesson4.cpp

// Chapter10_Lesson4.cpp
// Robust scan matching (2D) under dynamic obstacles using IRLS-Huber + trimming
// Dependencies: Eigen (header-only). Compile example (Linux/macOS):
//   g++ -O2 -std=c++17 Chapter10_Lesson4.cpp -I /usr/include/eigen3 -o robust_icp
//
// Educational reference implementation (O(NM) nearest neighbors).

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

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 void nearestNeighborsBruteForce(
    const std::vector<Vec2>& A,
    const std::vector<Vec2>& B,
    std::vector<int>& idx,
    std::vector<double>& d2
) {
    idx.resize(A.size());
    d2.resize(A.size());
    for (size_t i = 0; i < A.size(); ++i) {
        double best = std::numeric_limits<double>::infinity();
        int bestj = -1;
        for (size_t j = 0; j < B.size(); ++j) {
            double dist2 = (A[i] - B[j]).squaredNorm();
            if (dist2 < best) { best = dist2; bestj = (int)j; }
        }
        idx[i] = bestj;
        d2[i] = best;
    }
}

static void weightedKabsch2D(
    const std::vector<Vec2>& P,
    const std::vector<Vec2>& Q,
    const std::vector<double>& w,
    Mat2& R, Vec2& t
) {
    double wsum = 1e-12;
    Vec2 pbar(0,0), qbar(0,0);
    for (size_t i = 0; i < P.size(); ++i) {
        wsum += w[i];
        pbar += w[i] * P[i];
        qbar += w[i] * Q[i];
    }
    pbar /= wsum;
    qbar /= wsum;

    Mat2 S = Mat2::Zero();
    for (size_t i = 0; i < P.size(); ++i) {
        Vec2 x = P[i] - pbar;
        Vec2 y = Q[i] - qbar;
        S += w[i] * (x * y.transpose());
    }

    // 2D Kabsch angle
    double num = S(0,1) - S(1,0);
    double den = S(0,0) + S(1,1);
    double theta = std::atan2(num, den);

    R = rot2(theta);
    t = qbar - R * pbar;
}

static double huberWeight(double r, double delta) {
    return (r <= delta) ? 1.0 : (delta / (r + 1e-12));
}

static void robustICP2D(
    const std::vector<Vec2>& src,
    const std::vector<Vec2>& tgt,
    int iters,
    double delta,
    double keepRatio,
    Mat2& R, Vec2& t
) {
    R = Mat2::Identity();
    t = Vec2::Zero();

    std::vector<Vec2> srcW(src.size());
    std::vector<int> nnIdx;
    std::vector<double> d2;

    for (int k = 0; k < iters; ++k) {
        for (size_t i = 0; i < src.size(); ++i) srcW[i] = R * src[i] + t;

        nearestNeighborsBruteForce(srcW, tgt, nnIdx, d2);

        std::vector<double> r(src.size());
        for (size_t i = 0; i < src.size(); ++i) r[i] = std::sqrt(d2[i]);

        // trimming threshold by quantile (approx)
        std::vector<double> rSorted = r;
        size_t qIdx = (size_t)std::floor(keepRatio * (rSorted.size() - 1));
        std::nth_element(rSorted.begin(), rSorted.begin() + qIdx, rSorted.end());
        double thr = rSorted[qIdx];

        std::vector<Vec2> P_in, Q_in;
        std::vector<double> w_in;
        P_in.reserve(src.size());
        Q_in.reserve(src.size());
        w_in.reserve(src.size());

        for (size_t i = 0; i < src.size(); ++i) {
            if (r[i] <= thr) {
                P_in.push_back(src[i]);
                Q_in.push_back(tgt[nnIdx[i]]);
                w_in.push_back(huberWeight(r[i], delta));
            }
        }

        Mat2 dR; Vec2 dt;
        weightedKabsch2D(P_in, Q_in, w_in, dR, dt);

        R = dR * R;
        t = dR * t + dt;
    }
}

static void makeSyntheticScene(
    std::vector<Vec2>& scan1,
    std::vector<Vec2>& scan2,
    Mat2& Rtrue,
    Vec2& ttrue,
    int nStatic=250,
    int nDyn=60,
    int seed=7
) {
    std::mt19937 gen(seed);
    std::uniform_real_distribution<double> unif(0.0, 1.0);
    std::normal_distribution<double> gauss(0.0, 1.0);

    auto randu = [&](){ return unif(gen); };
    auto randn = [&](){ return gauss(gen); };

    std::vector<Vec2> staticPts;
    staticPts.reserve(nStatic);

    for (int i = 0; i < nStatic/2; ++i) {
        double ang = 2.0 * M_PI * randu();
        staticPts.push_back(Vec2(2.0*std::cos(ang), 2.0*std::sin(ang)));
    }
    for (int i = 0; i < nStatic/4; ++i) staticPts.push_back(Vec2(-3.0 + 6.0*randu(), -1.5));
    for (int i = 0; i < nStatic/4; ++i) staticPts.push_back(Vec2(1.5, -2.0 + 4.0*randu()));

    Vec2 dyn1c(-0.5, 0.8);
    std::vector<Vec2> dyn1; dyn1.reserve(nDyn);
    for (int i = 0; i < nDyn; ++i) dyn1.push_back(dyn1c + 0.15*Vec2(randn(), randn()));

    double theta = 12.0 * M_PI / 180.0;
    ttrue = Vec2(0.35, -0.10);
    Rtrue = rot2(theta);

    scan1.clear();
    scan1.insert(scan1.end(), staticPts.begin(), staticPts.end());
    scan1.insert(scan1.end(), dyn1.begin(), dyn1.end());
    for (auto& p : scan1) p += 0.02*Vec2(randn(), randn());

    Vec2 dyn2c = dyn1c + Vec2(0.55, -0.25);
    std::vector<Vec2> dyn2; dyn2.reserve(nDyn);
    for (int i = 0; i < nDyn; ++i) dyn2.push_back(dyn2c + 0.15*Vec2(randn(), randn()));

    scan2.clear();
    for (auto& p : staticPts) scan2.push_back(Rtrue * p + ttrue);
    scan2.insert(scan2.end(), dyn2.begin(), dyn2.end());
    for (auto& p : scan2) p += 0.02*Vec2(randn(), randn());
}

static double angleErrorDeg(const Mat2& Rest, const Mat2& Rtrue) {
    Mat2 dR = Rest * Rtrue.transpose();
    double ang = std::atan2(dR(1,0), dR(0,0));
    return std::abs(ang) * 180.0 / M_PI;
}

int main() {
    std::vector<Vec2> scan1, scan2;
    Mat2 Rtrue; Vec2 ttrue;
    makeSyntheticScene(scan1, scan2, Rtrue, ttrue);

    Mat2 R; Vec2 t;
    robustICP2D(scan1, scan2, 25, 0.18, 0.85, R, t);

    std::cout << "True t: " << ttrue.transpose() << "\n";
    std::cout << "Est  t: " << t.transpose() << "\n";
    std::cout << "Angle error (deg): " << angleErrorDeg(R, Rtrue) << "\n";
    std::cout << "Translation error : " << (t - ttrue).norm() << "\n";
    return 0;
}
      

Robotics ecosystem notes (Java): ROS has ROSJava; matrix ops often use EJML. For didactic clarity, we use a closed-form 2D rotation update without external libraries.

Chapter10_Lesson4.java

// Chapter10_Lesson4.java
// Robust scan matching (2D) under dynamic obstacles using IRLS-Huber + trimming
// Pure Java (no external deps). Educational O(NM) nearest neighbors.

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

public class Chapter10_Lesson4 {

    static class Vec2 {
        double x, y;
        Vec2(double x, double y) { this.x = x; this.y = y; }
        Vec2 add(Vec2 b) { return new Vec2(x + b.x, y + b.y); }
        Vec2 sub(Vec2 b) { return new Vec2(x - b.x, y - b.y); }
        Vec2 mul(double s) { return new Vec2(s * x, s * y); }
        double norm2() { return x*x + y*y; }
        double norm() { return sqrt(norm2()); }
    }

    static class Pose2 {
        double theta; // rotation angle
        Vec2 t;       // translation
        Pose2(double theta, Vec2 t) { this.theta = theta; this.t = t; }
    }

    static Vec2 rot(Vec2 p, double theta) {
        double c = cos(theta), s = sin(theta);
        return new Vec2(c*p.x - s*p.y, s*p.x + c*p.y);
    }

    static Vec2 transform(Vec2 p, Pose2 T) {
        Vec2 rp = rot(p, T.theta);
        return rp.add(T.t);
    }

    static double huberWeight(double r, double delta) {
        return (r <= delta) ? 1.0 : (delta / (r + 1e-12));
    }

    static int[] nearestNeighbors(List<Vec2> A, List<Vec2> B) {
        int[] idx = new int[A.size()];
        for (int i = 0; i < A.size(); ++i) {
            Vec2 a = A.get(i);
            double best = Double.POSITIVE_INFINITY;
            int bestj = -1;
            for (int j = 0; j < B.size(); ++j) {
                double dx = a.x - B.get(j).x;
                double dy = a.y - B.get(j).y;
                double dist2 = dx*dx + dy*dy;
                if (dist2 < best) { best = dist2; bestj = j; }
            }
            idx[i] = bestj;
        }
        return idx;
    }

    // Weighted 2D Kabsch in angle form:
    // theta = atan2(S12 - S21, S11 + S22) where S = sum w * (p-pbar)(q-qbar)^T
    static Pose2 weightedKabsch2D(List<Vec2> P, List<Vec2> Q, double[] w) {
        double wsum = 1e-12;
        double px = 0, py = 0, qx = 0, qy = 0;
        for (int i = 0; i < P.size(); ++i) {
            wsum += w[i];
            px += w[i] * P.get(i).x; py += w[i] * P.get(i).y;
            qx += w[i] * Q.get(i).x; qy += w[i] * Q.get(i).y;
        }
        px /= wsum; py /= wsum; qx /= wsum; qy /= wsum;

        double S11=0, S12=0, S21=0, S22=0;
        for (int i = 0; i < P.size(); ++i) {
            double x1 = P.get(i).x - px;
            double x2 = P.get(i).y - py;
            double y1 = Q.get(i).x - qx;
            double y2 = Q.get(i).y - qy;
            S11 += w[i] * x1 * y1;
            S12 += w[i] * x1 * y2;
            S21 += w[i] * x2 * y1;
            S22 += w[i] * x2 * y2;
        }

        double theta = atan2(S12 - S21, S11 + S22);
        Vec2 t = new Vec2(qx, qy).sub(rot(new Vec2(px, py), theta));
        return new Pose2(theta, t);
    }

    static Pose2 robustICP2D(List<Vec2> src, List<Vec2> tgt, int iters, double delta, double keepRatio) {
        Pose2 T = new Pose2(0.0, new Vec2(0.0, 0.0));

        for (int k = 0; k < iters; ++k) {
            ArrayList<Vec2> srcW = new ArrayList<>(src.size());
            for (Vec2 p : src) srcW.add(transform(p, T));

            int[] nn = nearestNeighbors(srcW, tgt);

            double[] r = new double[src.size()];
            for (int i = 0; i < src.size(); ++i) {
                Vec2 d = srcW.get(i).sub(tgt.get(nn[i]));
                r[i] = d.norm();
            }

            double[] rSorted = r.clone();
            Arrays.sort(rSorted);
            int qIdx = (int)floor(keepRatio * (rSorted.length - 1));
            double thr = rSorted[qIdx];

            ArrayList<Vec2> P_in = new ArrayList<>();
            ArrayList<Vec2> Q_in = new ArrayList<>();
            ArrayList<Double> wList = new ArrayList<>();

            for (int i = 0; i < src.size(); ++i) {
                if (r[i] <= thr) {
                    P_in.add(src.get(i));
                    Q_in.add(tgt.get(nn[i]));
                    wList.add(huberWeight(r[i], delta));
                }
            }

            double[] w_in = new double[wList.size()];
            for (int i = 0; i < w_in.length; ++i) w_in[i] = wList.get(i);

            Pose2 dT = weightedKabsch2D(P_in, Q_in, w_in);

            // Compose: dT o T
            double thetaNew = dT.theta + T.theta;
            Vec2 tNew = rot(T.t, dT.theta).add(dT.t);
            T = new Pose2(thetaNew, tNew);
        }

        return T;
    }

    static class Scene {
        ArrayList<Vec2> scan1, scan2;
        Pose2 Ttrue;
        Scene(ArrayList<Vec2> s1, ArrayList<Vec2> s2, Pose2 T) { scan1=s1; scan2=s2; Ttrue=T; }
    }

    static Scene makeSyntheticScene(int nStatic, int nDyn, long seed) {
        Random rng = new Random(seed);

        ArrayList<Vec2> stat = new ArrayList<>();
        for (int i = 0; i < nStatic/2; ++i) {
            double ang = 2.0 * PI * rng.nextDouble();
            stat.add(new Vec2(2.0*cos(ang), 2.0*sin(ang)));
        }
        for (int i = 0; i < nStatic/4; ++i) stat.add(new Vec2(-3.0 + 6.0*rng.nextDouble(), -1.5));
        for (int i = 0; i < nStatic/4; ++i) stat.add(new Vec2(1.5, -2.0 + 4.0*rng.nextDouble()));

        ArrayList<Vec2> dyn1 = new ArrayList<>();
        Vec2 c1 = new Vec2(-0.5, 0.8);
        for (int i = 0; i < nDyn; ++i) dyn1.add(c1.add(new Vec2(0.15*rng.nextGaussian(), 0.15*rng.nextGaussian())));

        double thetaTrue = toRadians(12.0);
        Vec2 tTrue = new Vec2(0.35, -0.10);
        Pose2 Ttrue = new Pose2(thetaTrue, tTrue);

        ArrayList<Vec2> scan1 = new ArrayList<>();
        scan1.addAll(stat);
        scan1.addAll(dyn1);

        for (int i = 0; i < scan1.size(); ++i) {
            Vec2 p = scan1.get(i);
            scan1.set(i, new Vec2(p.x + 0.02*rng.nextGaussian(), p.y + 0.02*rng.nextGaussian()));
        }

        ArrayList<Vec2> dyn2 = new ArrayList<>();
        Vec2 c2 = c1.add(new Vec2(0.55, -0.25));
        for (int i = 0; i < nDyn; ++i) dyn2.add(c2.add(new Vec2(0.15*rng.nextGaussian(), 0.15*rng.nextGaussian())));

        ArrayList<Vec2> scan2 = new ArrayList<>();
        for (Vec2 p : stat) scan2.add(transform(p, Ttrue));
        scan2.addAll(dyn2);

        for (int i = 0; i < scan2.size(); ++i) {
            Vec2 p = scan2.get(i);
            scan2.set(i, new Vec2(p.x + 0.02*rng.nextGaussian(), p.y + 0.02*rng.nextGaussian()));
        }

        return new Scene(scan1, scan2, Ttrue);
    }

    static double poseAngleErrorDeg(Pose2 Test, Pose2 Ttrue) {
        return abs(toDegrees(Test.theta - Ttrue.theta));
    }

    static double poseTranslationError(Pose2 Test, Pose2 Ttrue) {
        return Test.t.sub(Ttrue.t).norm();
    }

    public static void main(String[] args) {
        Scene sc = makeSyntheticScene(250, 60, 7);
        Pose2 Test = robustICP2D(sc.scan1, sc.scan2, 25, 0.18, 0.85);

        System.out.printf("True t = (%.3f, %.3f)%n", sc.Ttrue.t.x, sc.Ttrue.t.y);
        System.out.printf("Est  t = (%.3f, %.3f)%n", Test.t.x, Test.t.y);
        System.out.printf("Angle error (deg) = %.3f%n", poseAngleErrorDeg(Test, sc.Ttrue));
        System.out.printf("Translation error  = %.3f%n", poseTranslationError(Test, sc.Ttrue));
    }
}
      

Robotics ecosystem notes (MATLAB): Robotics System Toolbox provides ICP-style registration utilities, and Simulink enables integration of estimation blocks with navigation logic. Below is a self-contained MATLAB script (it can be wrapped in a MATLAB Function block for Simulink integration).

Chapter10_Lesson4.m

% Chapter10_Lesson4.m
% Robust scan matching (2D) under dynamic obstacles using IRLS-Huber + trimming
% This script is self-contained and uses only base MATLAB.
%
% Simulink note:
%   You can wrap robust_icp_2d below inside a MATLAB Function block (or call it
%   from a MATLAB System block) to integrate with a larger estimation pipeline.

function Chapter10_Lesson4()
    rng(7);

    [scan1, scan2, R_true, t_true] = make_synthetic_scene(250, 60);

    iters = 25;
    delta = 0.18;
    keep_ratio = 0.85;

    [R_est, t_est, rmse_hist] = robust_icp_2d(scan1, scan2, iters, delta, keep_ratio);

    [ang_err_deg, trans_err] = pose_error(R_est, t_est, R_true, t_true);

    fprintf('True t: [%.3f %.3f]\\n', t_true(1), t_true(2));
    fprintf('Est  t: [%.3f %.3f]\\n', t_est(1), t_est(2));
    fprintf('Angle error (deg): %.3f\\n', ang_err_deg);
    fprintf('Translation error : %.3f\\n', trans_err);
    fprintf('RMSE (first 5): %s ... last: %.4f\\n', mat2str(rmse_hist(1:min(5,end))), rmse_hist(end));
end

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

function [R, t] = weighted_kabsch_2d(P, Q, w)
    w = w(:);
    wsum = sum(w) + 1e-12;
    pbar = (w' * P) / wsum;
    qbar = (w' * Q) / wsum;

    X = P - pbar;
    Y = Q - qbar;

    S = zeros(2,2);
    for i=1:size(P,1)
        S = S + w(i) * (X(i,:)' * Y(i,:));
    end

    num = S(1,2) - S(2,1);
    den = S(1,1) + S(2,2);
    theta = atan2(num, den);

    R = rot2(theta);
    t = qbar' - R*pbar';
end

function [idx, d2] = nearest_neighbors_bruteforce(A, B)
    N = size(A,1);
    idx = zeros(N,1);
    d2  = zeros(N,1);
    for i=1:N
        dif = B - A(i,:);
        dist2 = sum(dif.^2, 2);
        [d2(i), idx(i)] = min(dist2);
    end
end

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

function [R, t, rmse_hist] = robust_icp_2d(source, target, iters, delta, keep_ratio)
    src = source;
    tgt = target;

    R = eye(2);
    t = [0; 0];
    rmse_hist = zeros(iters,1);

    for k=1:iters
        src_w = (R*src')' + t';  % Nx2

        [nn_idx, d2] = nearest_neighbors_bruteforce(src_w, tgt);
        Q = tgt(nn_idx, :);
        res = src_w - Q;
        r = sqrt(sum(res.^2, 2));

        thr = quantile(r, keep_ratio);
        inliers = r <= thr;

        P_in = src(inliers, :);
        Q_in = Q(inliers, :);
        r_in = r(inliers);

        w = huber_weights(r_in, delta);

        [dR, dt] = weighted_kabsch_2d(P_in, Q_in, w);

        R = dR * R;
        t = dR * t + dt;

        rmse_hist(k) = sqrt(mean(r_in.^2));
    end
end

function [scan1, scan2, R_true, t_true] = make_synthetic_scene(n_static, n_dyn)
    ang = 2*pi*rand(n_static/2,1);
    circle = [2*cos(ang), 2*sin(ang)];
    line1  = [-3 + 6*rand(n_static/4,1), -1.5*ones(n_static/4,1)];
    line2  = [1.5*ones(n_static/4,1), -2 + 4*rand(n_static/4,1)];
    stat = [circle; line1; line2];

    dyn_c1 = [-0.5, 0.8];
    dyn1 = dyn_c1 + 0.15*randn(n_dyn,2);

    theta_true = deg2rad(12.0);
    t_true = [0.35; -0.10];
    R_true = rot2(theta_true);

    scan1 = [stat; dyn1] + 0.02*randn(size(stat,1)+n_dyn, 2);

    dyn_c2 = dyn_c1 + [0.55, -0.25];
    dyn2 = dyn_c2 + 0.15*randn(n_dyn,2);

    stat2 = (R_true*stat')' + t_true';
    scan2 = [stat2; dyn2] + 0.02*randn(size(stat2,1)+n_dyn, 2);
end

function [ang_err_deg, trans_err] = pose_error(R_est, t_est, R_true, t_true)
    dR = R_est * R_true';
    ang = atan2(dR(2,1), dR(1,1));
    ang_err_deg = abs(rad2deg(ang));
    trans_err = norm(t_est - t_true);
end
      

Mathematica is useful for symbolic checks (Jacobians, influence functions) and rapid prototyping. Below is a plain-text .nb notebook expression that you can save and open.

Chapter10_Lesson4.nb

(* Chapter10_Lesson4.nb *)
Notebook[{
  Cell["Robust Scan Matching (2D) with Huber-IRLS + Trimming", "Title"],
  Cell["This notebook is plain-text Notebook[] form; save as .nb and open in Mathematica.", "Text"],

  Cell[BoxData[RowBox[{"ClearAll", "[", "\"Global`*\"", "]"}]], "Input"],

  Cell[BoxData[
    RowBox[{
      "rot2", "[", "th_", "]", ":=", 
      RowBox[{"{", 
        RowBox[{
          RowBox[{"{", 
            RowBox[{
              RowBox[{"Cos", "[", "th", "]"}], ",", 
              RowBox[{"-", 
                RowBox[{"Sin", "[", "th", "]"}]}]}], "}"}], ",", 
          RowBox[{"{", 
            RowBox[{
              RowBox[{"Sin", "[", "th", "]"}], ",", 
              RowBox[{"Cos", "[", "th", "]"}]}], "}"}]}], "}"}]}]], "Input"],

  Cell[BoxData[
    RowBox[{
      "huberW", "[", 
      RowBox[{"r_", ",", "del_"}], "]", ":=", 
      RowBox[{"If", "[", 
        RowBox[{
          RowBox[{"r", "\[LessEqual]", "del"}], ",", "1.0", ",", 
          RowBox[{"del", "/", 
            RowBox[{"(", 
              RowBox[{"r", "+", "10^-12"}], ")"}]}]}], "]"}]}]], "Input"],

  Cell["A full ICP implementation follows the same structure as the Python/MATLAB versions.", "Text"]
},
WindowSize->{1000, 700},
StyleDefinitions->"Default.nb"
]
      

Problem 1 (Huber IRLS weights): Starting from \( \min_{\mathbf{T} } \sum_i \rho_\delta(\|\mathbf{r}_i(\mathbf{T})\|_2) \): derive the IRLS weight rule \( w_i = \psi_\delta(s_i)/s_i \): and specialize it for the Huber loss.

Solution: Let \( s_i = \|\mathbf{r}_i\|_2 \). With linearization \( \mathbf{r}_i \approx \mathbf{r}_i^0 + \mathbf{J}_i \Delta\boldsymbol{\xi} \), the gradient is \( \nabla J \approx \sum_i \psi(s_i)\frac{1}{s_i}\mathbf{J}_i^\top \mathbf{r}_i \). Setting \( \nabla J=0 \) yields weighted normal equations with \( w_i = \psi(s_i)/s_i \):. For Huber, \( \psi_\delta(s)=s \) when \( s \;\;<=\;\;\delta \) and \( \psi_\delta(s)=\delta \) when \( s \;\;>\;\delta \), hence \( w_i = 1 \) for inliers and \( w_i = \delta/s_i \) for outliers.

Problem 2 (Convexity of Huber): Prove that the Huber loss \( \rho_\delta(s) \) is convex in \( s \ge 0 \).

Solution: For \( s \;\;<=\;\;\delta \), \( \rho_\delta(s)=\tfrac{1}{2}s^2 \) has second derivative 1. For \( s \;\;>\;\delta \), \( \rho_\delta(s)=\delta(s-\tfrac{1}{2}\delta) \) has second derivative 0. The first derivative is continuous at \( s=\delta \) because both sides give \( \rho'(\delta)=\delta \). Since \( \rho''(s) \ge 0 \) everywhere and the function is continuously differentiable, \( \rho_\delta \) is convex.

Problem 3 (Mixture-model responsibility): For the Gaussian+uniform mixture in Section 4, derive the responsibility \( \gamma_i = P(z_i=1 \mid \mathbf{r}_i) \):.

Solution: By Bayes’ rule, \( \gamma_i = \frac{\pi p(\mathbf{r}_i\mid z_i=1)}{\pi p(\mathbf{r}_i\mid z_i=1)+(1-\pi)p(\mathbf{r}_i\mid z_i=0)} \). Substitute \( p(\mathbf{r}_i\mid z_i=1)=\mathcal{N}(\mathbf{r}_i;0,\sigma^2I) \) and \( p(\mathbf{r}_i\mid z_i=0)=1/|\Omega| \) to obtain the expression in Section 4.

Problem 4 (Trimmed ICP robustness): Consider minimizing the sum of the smallest K squared residuals (trimmed least squares). Explain why this increases robustness to moving objects, and what can go wrong if K is too small.

Solution: Dynamic obstacles tend to yield large residuals under a correct static pose, so discarding the largest \(N-K\) residuals reduces their influence (higher breakdown point). If K is too small, the retained points may not constrain the pose (degeneracy), leading to unstable estimates; additionally, if the kept set comes from a single planar structure, rotation/translation may become poorly observable.

Problem 5 (Persistence test decision): Suppose a point is observed over \(L\) frames and is successfully matched in \(m\) frames (within threshold \( \epsilon \)). Under a Bernoulli model with static success probability \(p_s\) and dynamic success probability \(p_d\), derive the log-likelihood ratio test and interpret the threshold.

Solution: The likelihoods are \( \mathcal{L}_s = p_s^m(1-p_s)^{L-m} \) and \( \mathcal{L}_d = p_d^m(1-p_d)^{L-m} \). The log-likelihood ratio is

\[ \log \Lambda = m\log\!\left(\frac{p_s}{p_d}\right) + (L-m)\log\!\left(\frac{1-p_s}{1-p_d}\right). \]

Decide “static” when \( \log\Lambda \;\;>\;\tau \). Larger \(m\) increases evidence for static structure. The threshold \( \tau \) controls the false positive/false negative tradeoff: too small labels movers as static, too large rejects useful static points (hurting scan matching).

Dynamic obstacles turn scan matching into a robust estimation problem. We reformulated registration using robust M-estimators (Huber/Tukey), proved the IRLS weight update, and showed a probabilistic mixture-model interpretation (EM responsibilities). We then introduced temporal persistence tests that are lightweight and highly effective in AMR settings. The implementations illustrate how robust weighting + trimming can stabilize ICP even when a significant fraction of returns come from movers.