In mobile robotics, scan/point-cloud registration is the problem of estimating the rigid-body transform that best aligns a source measurement to a target reference. Typical AMR uses include: incremental motion estimation (scan-to-scan odometry), scan-to-map localization, map building, and loop closure.

Let the source scan be \( \mathcal{P} = \{\mathbf{p}_i\}_{i=1}^N \) and the target scan/map be \( \mathcal{Q} = \{\mathbf{q}_j\}_{j=1}^M \). Registration seeks a transform \( \mathbf{T} \) (in 2D: SE(2), in 3D: SE(3)) such that transformed source points align with target geometry.

flowchart TD
  A["Raw scan/point cloud at time k"] --> B["Preprocess: filter, downsample, deskew"]
  B --> C["Choose reference: previous scan or map"]
  C --> D["Solve alignment objective to estimate relative pose"]
  D --> E["Quality checks: overlap, residuals, degeneracy"]
  E --> F["Use result: odometry increment, map update, loop closure constraint"]
        

The key output is a relative pose estimate (and ideally uncertainty). In 2D scan matching (common for indoor AMR), the pose is \( \boldsymbol{\xi} = [t_x, t_y, \theta]^\top \). In 3D, it is \( \boldsymbol{\xi} = [t_x, t_y, t_z, \phi, \psi, \omega]^\top \) (or any equivalent minimal parameterization).

Registration is always frame-dependent. Suppose the source points are expressed in frame \( \{S\} \) and the target in frame \( \{T\} \). A rigid transform maps coordinates via \( \mathbf{p}^T \): \( \mathbf{p}^T = \mathbf{R}\mathbf{p}^S + \mathbf{t} \).

In homogeneous form (useful for composition), \( \mathbf{T} \in SE(3) \) is

\[ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \quad \mathbf{R}\in SO(3),\; \mathbf{t}\in\mathbb{R}^3,\; \mathbf{R}^\top\mathbf{R}=\mathbf{I},\; \det(\mathbf{R})=1. \]

For planar LiDAR scan matching (SE(2)), we typically use \( \mathbf{R}(\theta) \) and \( \mathbf{t}=[t_x,t_y]^\top \):

\[ \mathbf{R}(\theta)= \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}, \quad \mathbf{p}^T = \mathbf{R}(\theta)\mathbf{p}^S + \mathbf{t}. \]

Goal statement (deterministic): find \( \mathbf{R},\mathbf{t} \) that minimizes a chosen misalignment metric.
Goal statement (probabilistic): find \( \mathbf{R},\mathbf{t} \) that maximizes likelihood (or posterior) under a sensor/noise model.

Registration objectives differ mainly by (i) the residual definition and (ii) how correspondences are handled. This lesson focuses on objective definitions; correspondence strategies and algorithmic variants appear in later lessons.

Paired-point (known correspondences): suppose each source point \( \mathbf{p}_i \) corresponds to target point \( \mathbf{q}_i \). The point-to-point least squares cost is

\[ \min_{\mathbf{R},\mathbf{t} } \sum_{i=1}^{N} \left\| \mathbf{q}_i - (\mathbf{R}\mathbf{p}_i + \mathbf{t}) \right\|_2^2 \quad \text{s.t. } \mathbf{R}\in SO(d). \]

Point-to-plane (surface-aware): if target geometry provides normals \( \mathbf{n}_i \), one uses the signed point-to-plane residual \( r_i \): \( r_i = \mathbf{n}_i^\top\!\left(\mathbf{q}_i - (\mathbf{R}\mathbf{p}_i + \mathbf{t})\right) \). The cost becomes

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

Robust M-estimation (outliers/dynamics): to reduce sensitivity to mismatches and moving objects, replace squared loss with a robust penalty \( \rho(\cdot) \):

\[ \min_{\mathbf{R},\mathbf{t} } \sum_{i=1}^{N} \rho\!\left( \left\| \mathbf{q}_i - (\mathbf{R}\mathbf{p}_i + \mathbf{t}) \right\|_2 \right). \]

flowchart TD
  I["Initialization (from odometry/IMU/guess)"] --> C["Define residual model (pt-pt / pt-plane / robust)"]
  C --> S["Solve: minimize cost to update pose"]
  S --> Q["Check: residual drop, overlap, stability"]
  Q -->|"converged"| O["Output pose + quality/uncertainty"]
  Q -->|"not converged"| I

Important AMR interpretation: a registration objective is only useful if it (i) has a basin of convergence compatible with expected odometry drift, and (ii) produces constraints consistent enough for downstream fusion (EKF/graph SLAM).

When correspondences are known (or fixed), the point-to-point problem admits a near-closed-form solution: translate by centroids and solve a constrained rotation fit. Define centroids \( \bar{\mathbf{p} } \): \( \bar{\mathbf{p} }=\frac{1}{N}\sum_i \mathbf{p}_i \), \( \bar{\mathbf{q} } \): \( \bar{\mathbf{q} }=\frac{1}{N}\sum_i \mathbf{q}_i \), and centered sets \( \mathbf{x}_i=\mathbf{p}_i-\bar{\mathbf{p} } \), \( \mathbf{y}_i=\mathbf{q}_i-\bar{\mathbf{q} } \).

The optimal translation is \( \mathbf{t}^\star \): \( \mathbf{t}^\star = \bar{\mathbf{q} } - \mathbf{R}\bar{\mathbf{p} } \), reducing the problem to

\[ \min_{\mathbf{R}\in SO(d)} \sum_{i=1}^{N} \|\mathbf{y}_i - \mathbf{R}\mathbf{x}_i\|_2^2. \]

Expanding and using \( \mathbf{R}^\top\mathbf{R}=\mathbf{I} \), this is equivalent to maximizing a trace:

\[ \sum_{i=1}^N \|\mathbf{y}_i - \mathbf{R}\mathbf{x}_i\|_2^2 = \sum_{i=1}^N \|\mathbf{y}_i\|_2^2 + \sum_{i=1}^N \|\mathbf{x}_i\|_2^2 - 2\,\mathrm{tr}\!\left(\mathbf{R}\mathbf{H}\right), \quad \mathbf{H}=\sum_{i=1}^N \mathbf{x}_i\mathbf{y}_i^\top. \]

Therefore, \( \mathbf{R}^\star \): \( \mathbf{R}^\star = \arg\max_{\mathbf{R}\in SO(d)} \mathrm{tr}(\mathbf{R}\mathbf{H}) \). If \( \mathbf{H}=\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^\top \) is the SVD, the maximizer is

\[ \mathbf{R}^\star = \mathbf{V}\mathbf{D}\mathbf{U}^\top, \quad \mathbf{D}=\mathrm{diag}\!\big(1,\dots,1,\det(\mathbf{V}\mathbf{U}^\top)\big), \quad \det(\mathbf{R}^\star)=1. \]

Why this matters for AMR: even if you later estimate correspondences iteratively (ICP-like), this closed-form “inner solve” clarifies what the algorithm is trying to minimize at each step: a rigid transform that best aligns paired geometric features in least squares.

Registration is often used inside probabilistic estimators. A standard interpretation: assume target points satisfy \( \mathbf{q}_i = \mathbf{R}\mathbf{p}_i + \mathbf{t} + \boldsymbol{\varepsilon}_i \), with i.i.d. Gaussian noise \( \boldsymbol{\varepsilon}_i \sim \mathcal{N}(\mathbf{0},\sigma^2\mathbf{I}) \).

The likelihood is \( p(\{\mathbf{q}_i\} \mid \mathbf{R},\mathbf{t}) \) and the ML estimate is \( (\mathbf{R}^\star,\mathbf{t}^\star) \):

\[ (\mathbf{R}^\star,\mathbf{t}^\star) = \arg\max_{\mathbf{R},\mathbf{t} } \prod_{i=1}^N \exp\!\left( -\frac{1}{2\sigma^2} \left\| \mathbf{q}_i - (\mathbf{R}\mathbf{p}_i+\mathbf{t}) \right\|_2^2 \right) \;\;\Longleftrightarrow\;\; \arg\min_{\mathbf{R},\mathbf{t} } \sum_{i=1}^{N} \left\| \mathbf{q}_i - (\mathbf{R}\mathbf{p}_i+\mathbf{t}) \right\|_2^2. \]

Uncertainty (local): if we parameterize pose by a minimal vector \( \boldsymbol{\xi} \) and linearize residuals \( \mathbf{r}(\boldsymbol{\xi}) \) around the solution, a common approximation is

\[ \mathbf{\Sigma}_{\boldsymbol{\xi} } \approx \sigma^2\left(\mathbf{J}^\top\mathbf{J}\right)^{-1}, \quad \mathbf{J} = \left.\frac{\partial \mathbf{r} }{\partial \boldsymbol{\xi} }\right|_{\boldsymbol{\xi}=\boldsymbol{\xi}^\star}, \quad \text{valid when linearization error is small and } \|\Delta\boldsymbol{\xi}\| < \epsilon. \]

AMR warning: in corridors or along long planar walls, the information matrix \( \mathbf{J}^\top\mathbf{J} \) becomes ill-conditioned; the registration goal may be under-constrained (large covariance along certain directions).

A registration result is not just a transform; it must pass robotics-driven quality tests. Common criteria include:

• Residual quality: mean/median point residual, trimmed RMSE, or robust cost value.
• Geometric overlap: fraction of source points with plausible matches inside a gate radius.
• Stability: local curvature (Hessian) indicates whether the solution is well-conditioned.
• Consistency with motion bounds: the pose increment should match feasible AMR motion (e.g., bounded yaw rate).

A practical “fitness” score for gating can be defined using a match radius \( \delta \):

\[ \mathrm{overlap}(\mathbf{T}) = \frac{1}{N}\sum_{i=1}^N \mathbf{1}\!\left( \min_{j} \left\| \mathbf{q}_j - (\mathbf{R}\mathbf{p}_i+\mathbf{t}) \right\|_2 < \delta \right), \quad \text{accept if overlap}(\mathbf{T}) > \eta. \]

Degeneracy (local observability): if the environment geometry does not excite all degrees of freedom, there exist nonzero perturbations \( \Delta\boldsymbol{\xi} \neq \mathbf{0} \) such that \( \mathbf{J}\Delta\boldsymbol{\xi}\approx\mathbf{0} \), implying weak constraints and inflated covariance.

While this lesson is objective-focused, it is useful to know the standard tooling used in robotics stacks:

• Python: NumPy (linear algebra), Open3D (ICP/FPFH/RANSAC), ROS2 Python (tf2 transforms), SciPy (optimization).
• C++: Eigen (linear algebra), PCL (filters, normals, ICP/NDT), Ceres Solver (nonlinear least squares), ROS2 (tf2, sensor_msgs/PointCloud2).
• Java: EJML (linear algebra), ROSJava (middleware integration), custom implementations (common for academic exercises).
• MATLAB/Simulink: point set processing via core MATLAB, Robotics System Toolbox (transforms, ROS), Simulink for pipeline prototyping and dataflow validation.
• Wolfram Mathematica: symbolic derivations (Jacobians/Hessians), numeric linear algebra, and prototyping objective functions.

The multi-language lab below implements the paired-point (known correspondences) goal, which is the cleanest mathematical baseline to validate coordinate frames, cost definitions, and numerical conditioning.

Each implementation generates synthetic 2D points, applies a ground-truth transform, adds noise, and recovers the transform via an SVD-based Procrustes/Kabsch solve. This isolates the registration goal from correspondence search (ICP) and from map representations.

Chapter10_Lesson1.py

# Chapter10_Lesson1.py
# Point-Cloud / Scan Registration Goals — minimal educational reference implementation
#
# This script demonstrates the *goal* of scan registration: estimate the rigid-body transform
# (R, t) that aligns a source point set to a target point set, assuming correspondences are known.
#
# Here we implement a closed-form 2D Procrustes/Kabsch solution + an optional Gauss–Newton refinement
# in SE(2) with fixed correspondences. (ICP variants, correspondence search, and robust kernels are
# treated in later lessons.)

from __future__ import annotations

import numpy as np


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


def kabsch_2d(P: np.ndarray, Q: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    """
    Estimate R,t such that Q ~ R P + t in least-squares sense, given paired points.
    P, Q: (N,2)
    Returns: R (2,2), t (2,)
    """
    if P.shape != Q.shape or P.shape[1] != 2:
        raise ValueError("P and Q must have shape (N,2) and match.")

    pbar = P.mean(axis=0)
    qbar = Q.mean(axis=0)

    X = P - pbar
    Y = Q - qbar

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

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

    t = qbar - R @ pbar
    return R, t


def apply_transform(P: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray:
    return (R @ P.T).T + t.reshape(1, 2)


def rmse(P: np.ndarray, Q: np.ndarray) -> float:
    return float(np.sqrt(np.mean(np.sum((P - Q) ** 2, axis=1))))


def gn_refine_se2(P: np.ndarray, Q: np.ndarray, R0: np.ndarray, t0: np.ndarray, iters: int = 5) -> tuple[np.ndarray, np.ndarray]:
    """
    Gauss–Newton refinement for SE(2) with fixed correspondences.
    Pose parameterization: (tx, ty, theta).
    """
    # Extract theta from R0
    theta = float(np.arctan2(R0[1, 0], R0[0, 0]))
    tx, ty = float(t0[0]), float(t0[1])

    for _ in range(iters):
        R = rot2(theta)
        t = np.array([tx, ty], dtype=float)

        # residuals r_i = Q_i - (R P_i + t)
        RP = (R @ P.T).T
        r = Q - (RP + t.reshape(1, 2))

        # Jacobian wrt [tx, ty, theta]:
        # dr/dt = -I
        # dr/dtheta = - d(RP)/dtheta
        c, s = np.cos(theta), np.sin(theta)
        dR_dtheta = np.array([[-s, -c], [c, -s]], dtype=float)

        J = np.zeros((2 * P.shape[0], 3), dtype=float)
        b = np.zeros((2 * P.shape[0],), dtype=float)

        for i in range(P.shape[0]):
            d = dR_dtheta @ P[i]
            # r_i is 2-vector
            J[2 * i : 2 * i + 2, 0:2] = -np.eye(2)
            J[2 * i : 2 * i + 2, 2] = -d
            b[2 * i : 2 * i + 2] = r[i]

        # Solve normal equations: (J^T J) delta = J^T b
        H = J.T @ J
        g = J.T @ b
        delta = np.linalg.solve(H, g)

        tx += float(delta[0])
        ty += float(delta[1])
        theta += float(delta[2])

        if np.linalg.norm(delta) < 1e-10:
            break

    return rot2(theta), np.array([tx, ty], dtype=float)


def approx_pose_covariance_se2(P: np.ndarray, theta: float, sigma2: float) -> np.ndarray:
    """
    Approximate covariance of [tx, ty, theta] from linearized least squares with fixed correspondences.
    Sigma ≈ sigma^2 (J^T J)^{-1}
    """
    c, s = np.cos(theta), np.sin(theta)
    dR_dtheta = np.array([[-s, -c], [c, -s]], dtype=float)

    J = np.zeros((2 * P.shape[0], 3), dtype=float)
    for i in range(P.shape[0]):
        d = dR_dtheta @ P[i]
        J[2 * i : 2 * i + 2, 0:2] = -np.eye(2)
        J[2 * i : 2 * i + 2, 2] = -d

    H = J.T @ J
    return sigma2 * np.linalg.inv(H)


def main() -> None:
    np.random.seed(7)

    # Create synthetic "scan" points (source)
    N = 200
    P = np.random.uniform(low=[-5.0, -3.0], high=[5.0, 3.0], size=(N, 2))

    # Ground-truth transform
    theta_gt = np.deg2rad(12.0)
    t_gt = np.array([1.2, -0.7], dtype=float)
    R_gt = rot2(theta_gt)

    # Generate target with noise (paired correspondences)
    Q_clean = apply_transform(P, R_gt, t_gt)
    sigma = 0.02
    Q = Q_clean + np.random.normal(0.0, sigma, size=Q_clean.shape)

    # Closed-form estimate
    R_hat, t_hat = kabsch_2d(P, Q)

    # Optional GN refinement
    R_ref, t_ref = gn_refine_se2(P, Q, R_hat, t_hat, iters=5)

    # Report
    print("Ground truth theta(deg), t:", np.rad2deg(theta_gt), t_gt)
    print("Kabsch      theta(deg), t:", np.rad2deg(np.arctan2(R_hat[1,0], R_hat[0,0])), t_hat)
    print("GN refined  theta(deg), t:", np.rad2deg(np.arctan2(R_ref[1,0], R_ref[0,0])), t_ref)

    Q_hat = apply_transform(P, R_hat, t_hat)
    Q_ref = apply_transform(P, R_ref, t_ref)
    print("RMSE Kabsch  :", rmse(Q_hat, Q))
    print("RMSE Refined :", rmse(Q_ref, Q))

    # Approx covariance (linearized)
    theta_ref = float(np.arctan2(R_ref[1, 0], R_ref[0, 0]))
    Sigma = approx_pose_covariance_se2(P, theta_ref, sigma2=sigma**2)
    print("Approx pose covariance diag [tx,ty,theta]:", np.diag(Sigma))


if __name__ == "__main__":
    main()

Chapter10_Lesson1.cpp

// Chapter10_Lesson1.cpp
// Point-Cloud / Scan Registration Goals — minimal educational C++ example
//
// Closed-form 2D point-set registration (paired correspondences) via SVD (Kabsch/Procrustes).
// Dependencies: Eigen3.
//
// Build example (Linux/macOS):
//   g++ -O2 -std=c++17 Chapter10_Lesson1.cpp -I /usr/include/eigen3 -o Chapter10_Lesson1
//
// Windows (MSVC): add Eigen include directory to project include paths.

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

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

struct Pose2 {
  Mat2 R;
  Vec2 t;
};

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

Pose2 Kabsch2D(const Eigen::MatrixXd& P, const Eigen::MatrixXd& Q) {
  // P, Q: (N,2)
  if (P.rows() != Q.rows() || P.cols() != 2 || Q.cols() != 2) {
    throw std::runtime_error("P and Q must be (N,2) and same size.");
  }

  Vec2 pbar = P.colwise().mean();
  Vec2 qbar = Q.colwise().mean();

  Eigen::MatrixXd X = P.rowwise() - pbar.transpose();
  Eigen::MatrixXd Y = Q.rowwise() - qbar.transpose();

  Eigen::Matrix2d H = X.transpose() * Y;
  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.0) {
    V.col(1) *= -1.0;
    R = V * U.transpose();
  }

  Vec2 t = qbar - R * pbar;

  return Pose2{R, t};
}

double RMSE(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) {
  Eigen::MatrixXd D = A - B;
  double s = 0.0;
  for (int i = 0; i < D.rows(); ++i) {
    s += D.row(i).squaredNorm();
  }
  return std::sqrt(s / static_cast<double>(D.rows()));
}

int main() {
  std::mt19937 rng(7);
  std::uniform_real_distribution<double> ux(-5.0, 5.0);
  std::uniform_real_distribution<double> uy(-3.0, 3.0);
  std::normal_distribution<double> n01(0.0, 1.0);

  const int N = 200;
  Eigen::MatrixXd P(N, 2);
  for (int i = 0; i < N; ++i) {
    P(i,0) = ux(rng);
    P(i,1) = uy(rng);
  }

  double theta_gt = 12.0 * M_PI / 180.0;
  Vec2 t_gt(1.2, -0.7);
  Mat2 R_gt = Rot2(theta_gt);

  double sigma = 0.02;
  Eigen::MatrixXd Q(N, 2);
  for (int i = 0; i < N; ++i) {
    Vec2 q = R_gt * P.row(i).transpose() + t_gt;
    q(0) += sigma * n01(rng);
    q(1) += sigma * n01(rng);
    Q.row(i) = q.transpose();
  }

  Pose2 est = Kabsch2D(P, Q);

  double theta_hat = std::atan2(est.R(1,0), est.R(0,0));

  std::cout << "theta_gt(deg) = " << theta_gt * 180.0 / M_PI
            << ", t_gt = [" << t_gt.transpose() << "]\n";
  std::cout << "theta_hat(deg)= " << theta_hat * 180.0 / M_PI
            << ", t_hat= [" << est.t.transpose() << "]\n";

  Eigen::MatrixXd Q_hat(N, 2);
  for (int i = 0; i < N; ++i) {
    Vec2 q = est.R * P.row(i).transpose() + est.t;
    Q_hat.row(i) = q.transpose();
  }

  std::cout << "RMSE(Q_hat, Q) = " << RMSE(Q_hat, Q) << "\n";
  return 0;
}

Chapter10_Lesson1.java

// Chapter10_Lesson1.java
// Point-Cloud / Scan Registration Goals — minimal educational Java example
//
// Closed-form 2D paired-point registration via SVD (Kabsch/Procrustes).
//
// Dependency (recommended): EJML (Efficient Java Matrix Library).
// Maven coordinates:
//   org.ejml:ejml-simple:0.43 (or newer compatible)
//   org.ejml:ejml-ddense:0.43
//
// This file focuses on algorithmic clarity rather than I/O.

import org.ejml.simple.SimpleMatrix;
import org.ejml.simple.SimpleSVD;

import java.util.Random;

public class Chapter10_Lesson1 {

    static SimpleMatrix rot2(double theta) {
        double c = Math.cos(theta), s = Math.sin(theta);
        return new SimpleMatrix(new double[][]{
                { c, -s },
                { s,  c }
        });
    }

    static class Pose2 {
        SimpleMatrix R; // 2x2
        SimpleMatrix t; // 2x1
        Pose2(SimpleMatrix R, SimpleMatrix t) { this.R = R; this.t = t; }
    }

    static Pose2 kabsch2D(SimpleMatrix P, SimpleMatrix Q) {
        // P, Q: N x 2
        if (P.numCols() != 2 || Q.numCols() != 2 || P.numRows() != Q.numRows()) {
            throw new IllegalArgumentException("P and Q must be N x 2 and same N.");
        }

        int N = P.numRows();
        SimpleMatrix pbar = new SimpleMatrix(1,2);
        SimpleMatrix qbar = new SimpleMatrix(1,2);
        for (int i = 0; i < N; i++) {
            pbar = pbar.plus(P.extractVector(true, i));
            qbar = qbar.plus(Q.extractVector(true, i));
        }
        pbar = pbar.divide(N);
        qbar = qbar.divide(N);

        SimpleMatrix X = new SimpleMatrix(N, 2);
        SimpleMatrix Y = new SimpleMatrix(N, 2);
        for (int i = 0; i < N; i++) {
            X.insertIntoThis(i, 0, P.extractVector(true, i).minus(pbar));
            Y.insertIntoThis(i, 0, Q.extractVector(true, i).minus(qbar));
        }

        SimpleMatrix H = X.transpose().mult(Y); // 2x2
        SimpleSVD<SimpleMatrix> svd = H.svd();
        SimpleMatrix U = svd.getU();
        SimpleMatrix V = svd.getV();

        SimpleMatrix R = V.mult(U.transpose());

        // Enforce det(R)=+1
        double det = R.determinant();
        if (det < 0.0) {
            // Flip second column of V (since 2D)
            SimpleMatrix Vfix = V.copy();
            Vfix.set(0,1, -Vfix.get(0,1));
            Vfix.set(1,1, -Vfix.get(1,1));
            R = Vfix.mult(U.transpose());
        }

        // t = qbar^T - R pbar^T
        SimpleMatrix t = qbar.transpose().minus(R.mult(pbar.transpose()));

        return new Pose2(R, t);
    }

    static double rmse(SimpleMatrix A, SimpleMatrix B) {
        if (A.numRows() != B.numRows() || A.numCols() != B.numCols()) throw new IllegalArgumentException();
        int N = A.numRows();
        double s = 0.0;
        for (int i = 0; i < N; i++) {
            double dx = A.get(i,0) - B.get(i,0);
            double dy = A.get(i,1) - B.get(i,1);
            s += dx*dx + dy*dy;
        }
        return Math.sqrt(s / N);
    }

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

        int N = 200;
        SimpleMatrix P = new SimpleMatrix(N, 2);
        for (int i = 0; i < N; i++) {
            double x = -5.0 + 10.0 * rng.nextDouble();
            double y = -3.0 +  6.0 * rng.nextDouble();
            P.set(i,0, x);
            P.set(i,1, y);
        }

        double thetaGT = Math.toRadians(12.0);
        SimpleMatrix Rgt = rot2(thetaGT);
        SimpleMatrix tgt = new SimpleMatrix(new double[][]{ {1.2},{-0.7} });

        double sigma = 0.02;
        SimpleMatrix Q = new SimpleMatrix(N, 2);
        for (int i = 0; i < N; i++) {
            SimpleMatrix p = P.extractVector(true, i).transpose(); // 2x1
            SimpleMatrix q = Rgt.mult(p).plus(tgt);
            q.set(0,0, q.get(0,0) + sigma * rng.nextGaussian());
            q.set(1,0, q.get(1,0) + sigma * rng.nextGaussian());
            Q.set(i,0, q.get(0,0));
            Q.set(i,1, q.get(1,0));
        }

        Pose2 est = kabsch2D(P, Q);

        double thetaHat = Math.atan2(est.R.get(1,0), est.R.get(0,0));

        System.out.printf("thetaGT(deg)=%.6f, tGT=[%.4f, %.4f]%n",
                Math.toDegrees(thetaGT), tgt.get(0), tgt.get(1));
        System.out.printf("thetaHat(deg)=%.6f, tHat=[%.4f, %.4f]%n",
                Math.toDegrees(thetaHat), est.t.get(0), est.t.get(1));

        // Apply transform to P
        SimpleMatrix Qhat = new SimpleMatrix(N, 2);
        for (int i = 0; i < N; i++) {
            SimpleMatrix p = P.extractVector(true, i).transpose();
            SimpleMatrix q = est.R.mult(p).plus(est.t);
            Qhat.set(i,0, q.get(0));
            Qhat.set(i,1, q.get(1));
        }

        System.out.printf("RMSE(Qhat,Q)=%.6f%n", rmse(Qhat, Q));
    }
}

Chapter10_Lesson1.m

% Chapter10_Lesson1.m
% Point-Cloud / Scan Registration Goals — MATLAB + Simulink script
%
% This file demonstrates the registration *goal* for 2D scans:
% estimate a rigid transform (R,t) aligning paired points.
%
% Part A: Closed-form Kabsch/Procrustes in 2D.
% Part B: Programmatically build a tiny Simulink model that calls a MATLAB Function
%         block to estimate the same transform for a fixed dataset.

function Chapter10_Lesson1()
    rng(7);

    % Synthetic paired points
    N = 200;
    P = [ -5 + 10*rand(N,1), -3 + 6*rand(N,1) ];

    theta_gt = deg2rad(12.0);
    t_gt = [1.2; -0.7];
    R_gt = [cos(theta_gt), -sin(theta_gt); sin(theta_gt), cos(theta_gt)];

    sigma = 0.02;
    Q = (R_gt * P')' + repmat(t_gt', N, 1) + sigma*randn(N,2);

    [R_hat, t_hat] = kabsch2d(P, Q);

    theta_hat = atan2(R_hat(2,1), R_hat(1,1));

    fprintf('theta_gt(deg)=%.6f, t_gt=[%.4f %.4f]\\n', rad2deg(theta_gt), t_gt(1), t_gt(2));
    fprintf('theta_hat(deg)=%.6f, t_hat=[%.4f %.4f]\\n', rad2deg(theta_hat), t_hat(1), t_hat(2));

    Q_hat = (R_hat * P')' + repmat(t_hat', N, 1);
    err = sqrt(mean(sum((Q_hat - Q).^2, 2)));
    fprintf('RMSE(Q_hat,Q)=%.6f\\n', err);

    % Optional: build a minimal Simulink model
    build_simulink_model(P, Q);
end

function [R, t] = kabsch2d(P, Q)
    % P,Q: N x 2
    pbar = mean(P, 1);
    qbar = mean(Q, 1);

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

    H = X' * Y;
    [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 build_simulink_model(P, Q)
    % Creates a Simulink model "Chapter10_Lesson1_Simulink" in the current folder.
    % Requires Simulink.
    mdl = 'Chapter10_Lesson1_Simulink';

    if bdIsLoaded(mdl)
        close_system(mdl, 0);
    end

    new_system(mdl);
    open_system(mdl);

    % Save P and Q to base workspace for Constant blocks
    assignin('base', 'P_data', P);
    assignin('base', 'Q_data', Q);

    % Add Constant blocks
    add_block('simulink/Sources/Constant', [mdl '/P'], 'Value', 'P_data', 'Position', [30 50 120 80]);
    add_block('simulink/Sources/Constant', [mdl '/Q'], 'Value', 'Q_data', 'Position', [30 120 120 150]);

    % MATLAB Function block to estimate transform
    add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/EstimateSE2'], 'Position', [200 70 360 150]);

    % Add Outports
    add_block('simulink/Sinks/Out1', [mdl '/R'], 'Position', [430 80 460 100]);
    add_block('simulink/Sinks/Out1', [mdl '/t'], 'Position', [430 130 460 150]);

    % Wire
    add_line(mdl, 'P/1', 'EstimateSE2/1');
    add_line(mdl, 'Q/1', 'EstimateSE2/2');
    add_line(mdl, 'EstimateSE2/1', 'R/1');
    add_line(mdl, 'EstimateSE2/2', 't/1');

    % Set MATLAB Function code
    code = [
        "function [R,t] = f(P,Q)" newline ...
        "%#codegen" newline ...
        "pbar = mean(P,1); qbar = mean(Q,1);" newline ...
        "X = P - pbar; Y = Q - qbar;" newline ...
        "H = X' * Y;" newline ...
        "[U,~,V] = svd(H);" newline ...
        "R = V * U';" newline ...
        "if det(R) < 0" newline ...
        "  V(:,2) = -V(:,2);" newline ...
        "  R = V * U';" newline ...
        "end" newline ...
        "t = qbar' - R * pbar';" newline ...
        "end"
    ];
    set_param([mdl '/EstimateSE2'], 'Script', code);

    save_system(mdl);
    fprintf('Simulink model created: %s.slx\\n', mdl);
end

Chapter10_Lesson1.nb

(* Chapter10_Lesson1.nb
   Point-Cloud / Scan Registration Goals — Wolfram Mathematica notebook content (plain-text)
   Save as Chapter10_Lesson1.nb and open in Mathematica.
*)

Notebook[{
  Cell["Chapter 10 — Lesson 1: Point-Cloud / Scan Registration Goals", "Title"],
  Cell["Closed-form 2D registration (paired correspondences) via SVD (Kabsch/Procrustes).", "Text"],

  Cell[BoxData@ToBoxes[
    Module[{seed = 7, n = 200, P, thetaGT, RGT, tGT, sigma, Q, pbar, qbar, X, Y, H, svd, U, V, R, t, thetaHat, rmse},
      SeedRandom[seed];
      P = Transpose@{RandomReal[{-5, 5}, n], RandomReal[{-3, 3}, n]};
      thetaGT = 12 Degree;
      RGT = { {Cos[thetaGT], -Sin[thetaGT]}, {Sin[thetaGT], Cos[thetaGT]} };
      tGT = {1.2, -0.7};
      sigma = 0.02;
      Q = (RGT.# + tGT)& /@ P + RandomVariate[NormalDistribution[0, sigma], {n, 2}];

      pbar = Mean[P]; qbar = Mean[Q];
      X = (# - pbar)& /@ P;
      Y = (# - qbar)& /@ Q;

      H = Transpose[X].Y;
      svd = SingularValueDecomposition[H];
      U = svd[[1]]; V = svd[[3]];

      R = V.Transpose[U];
      If[Det[R] < 0, V[[All, 2]] = -V[[All, 2]]; R = V.Transpose[U]];

      t = qbar - (R.pbar);
      thetaHat = ArcTan[R[[1, 1]], R[[2, 1]]];

      rmse = Sqrt[Mean[Total[( (R.# + t)& /@ P - Q)^2, {2}]]];

      {
        "thetaGT(deg)" -> N[thetaGT/Degree],
        "tGT" -> tGT,
        "thetaHat(deg)" -> N[thetaHat/Degree],
        "tHat" -> N[t],
        "RMSE" -> N[rmse]
      }
    ],
    StandardForm
  ], "Input"]
}]

What to look for: (i) your estimated \( \theta \) and \( \mathbf{t} \) should be close to ground truth, (ii) RMSE should scale with noise level \( \sigma \), and (iii) if you make points nearly collinear, the rotation/translation become less well-conditioned (degeneracy).

Problem 1 (ML equivalence): Assume paired points satisfy \( \mathbf{q}_i = \mathbf{R}\mathbf{p}_i + \mathbf{t} + \boldsymbol{\varepsilon}_i \) with i.i.d. \( \boldsymbol{\varepsilon}_i \sim \mathcal{N}(\mathbf{0},\sigma^2\mathbf{I}) \). Show that the ML estimate of \( (\mathbf{R},\mathbf{t}) \) minimizes the sum of squared residuals.

Solution: The likelihood is \( \prod_i (2\pi\sigma^2)^{-d/2}\exp\!\big(-\frac{1}{2\sigma^2}\|\mathbf{q}_i-(\mathbf{R}\mathbf{p}_i+\mathbf{t})\|_2^2\big) \). Taking negative log-likelihood and removing constants yields minimization of \( \sum_i \|\mathbf{q}_i-(\mathbf{R}\mathbf{p}_i+\mathbf{t})\|_2^2 \).

Problem 2 (centroid reduction): For the paired-point objective in Section 3, prove that for any fixed rotation \( \mathbf{R} \), the optimal translation is \( \mathbf{t}^\star = \bar{\mathbf{q} } - \mathbf{R}\bar{\mathbf{p} } \).

Solution: Write the cost \( f(\mathbf{t})=\sum_i \|\mathbf{q}_i-\mathbf{R}\mathbf{p}_i-\mathbf{t}\|_2^2 \). Differentiate w.r.t. \( \mathbf{t} \):

\[ \frac{\partial f}{\partial \mathbf{t} } = -2\sum_{i=1}^N \left(\mathbf{q}_i - \mathbf{R}\mathbf{p}_i - \mathbf{t}\right) = \mathbf{0} \;\;\Longrightarrow\;\; N\mathbf{t} = \sum_i \mathbf{q}_i - \mathbf{R}\sum_i \mathbf{p}_i \;\;\Longrightarrow\;\; \mathbf{t}^\star = \bar{\mathbf{q} } - \mathbf{R}\bar{\mathbf{p} }. \]

Problem 3 (SVD/Kabsch optimal rotation): Let \( \mathbf{H}=\sum_i \mathbf{x}_i\mathbf{y}_i^\top \) be the cross-covariance of centered points. Show that minimizing \( \sum_i\|\mathbf{y}_i-\mathbf{R}\mathbf{x}_i\|_2^2 \) over \( \mathbf{R}\in SO(d) \) is equivalent to maximizing \( \mathrm{tr}(\mathbf{R}\mathbf{H}) \), and conclude the SVD solution in Section 4.

Solution: Expand and use orthogonality of \( \mathbf{R} \): \( \sum_i\|\mathbf{y}_i-\mathbf{R}\mathbf{x}_i\|_2^2 = \sum_i\|\mathbf{y}_i\|_2^2 + \sum_i\|\mathbf{x}_i\|_2^2 - 2\mathrm{tr}(\mathbf{R}\mathbf{H}) \). The first two terms do not depend on \( \mathbf{R} \), so the minimizer maximizes the trace. With SVD \( \mathbf{H}=\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^\top \), the constrained trace maximizer is \( \mathbf{R}^\star=\mathbf{V}\mathbf{D}\mathbf{U}^\top \) with \( \mathbf{D} \) enforcing \( \det(\mathbf{R}^\star)=1 \).

Problem 4 (degeneracy in corridors): Consider 2D scan matching against a long straight wall. Explain why translation along the wall direction becomes weakly observable, using the idea that \( \mathbf{J}^\top\mathbf{J} \) becomes ill-conditioned.

Solution: If the environment geometry is approximately invariant to a motion (e.g., sliding along an infinite wall), residuals change very little under that perturbation. Therefore, there exists a direction \( \Delta\boldsymbol{\xi} \neq \mathbf{0} \) such that \( \mathbf{J}\Delta\boldsymbol{\xi}\approx\mathbf{0} \), which implies \( \mathbf{J}^\top\mathbf{J} \) has a small eigenvalue and \( (\mathbf{J}^\top\mathbf{J})^{-1} \) has a large eigenvalue (inflated uncertainty).

Problem 5 (gating with overlap): Suppose you accept a registration only if \( \mathrm{overlap}(\mathbf{T}) > \eta \) and the robust objective value is below a threshold \( \mathcal{C}(\mathbf{T}) < \gamma \). Give a principled reason to use both tests, not just one.

Solution: A low objective value alone can occur when only a small subset of points is matched (overfitting to a partial overlap), while high overlap alone can occur with large systematic residuals (broad but poor alignment). Using both enforces (i) sufficient geometric support (overlap) and (ii) actual alignment quality (cost), improving robustness in AMR pipelines.

Registration goals in AMR are defined by (i) a rigid transform model, (ii) an objective (point-to-point, point-to-plane, robust), (iii) a noise/likelihood interpretation for principled uncertainty, and (iv) robotics-driven quality/degeneracy checks. These goals directly motivate the algorithmic design space explored in later lessons (ICP variants, correlative scan matching, and robustness under dynamics).