From f773d31178bd7c664b16b5f44dc9ae332ae2ce62 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 6 Nov 2026 07:26:41 +0000 Subject: [PATCH] feat(solvers): add PointSetAlignment (NT67) Weighted least-squares rigid or similarity transform between corresponding point sets: centred moments and cross-covariance, the closed-form planar angle, the 3-D Kabsch solution from one SVD with the reflection fix and a refusal for collinear points, Umeyama's scale and the weighted residual RMS. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 30 +-- ROADMAP.md | 16 +- doc/solvers/PointSetAlignment.md | 101 ++++++++++ doc/solvers/README.md | 35 ++-- numerical/solvers/CMakeLists.txt | 2 + numerical/solvers/PointSetAlignment.cpp | 7 + numerical/solvers/PointSetAlignment.hpp | 180 ++++++++++++++++++ numerical/solvers/test/CMakeLists.txt | 1 + .../solvers/test/TestPointSetAlignment.cpp | 161 ++++++++++++++++ roadmap/README.md | 2 +- .../solvers/PointSetAlignment/explanation.md | 34 ---- .../PointSetAlignment/implementation.md | 93 --------- roadmap/solvers/PointSetAlignment/tests.md | 52 ----- 13 files changed, 490 insertions(+), 224 deletions(-) create mode 100644 doc/solvers/PointSetAlignment.md create mode 100644 numerical/solvers/PointSetAlignment.cpp create mode 100644 numerical/solvers/PointSetAlignment.hpp create mode 100644 numerical/solvers/test/TestPointSetAlignment.cpp delete mode 100644 roadmap/solvers/PointSetAlignment/explanation.md delete mode 100644 roadmap/solvers/PointSetAlignment/implementation.md delete mode 100644 roadmap/solvers/PointSetAlignment/tests.md diff --git a/README.md b/README.md index 6e0a376..21272d0 100644 --- a/README.md +++ b/README.md @@ -14,21 +14,21 @@ Refer to the documentation to quickly integrate and utilize the library's signal ## Documentation -| Category | Description | -|--------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | -| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space | -| [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | -| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS, Incremental QR Least Squares (rank-revealing) | -| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Residual-Form Kalman Update, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | -| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers, Levenberg–Marquardt | -| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | -| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log), SE(2) Transform (Planar Twists, Exp/Log) | -| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | -| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet), Damped Pseudo-Inverse + Null-Space Projector, Dense Convex QP (Goldfarb–Idnani) | -| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | -| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | -| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | +| Category | Description | +|--------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | +| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space | +| [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | +| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS, Incremental QR Least Squares (rank-revealing) | +| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Residual-Form Kalman Update, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | +| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers, Levenberg–Marquardt | +| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log), SE(2) Transform (Planar Twists, Exp/Log) | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | +| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet), Damped Pseudo-Inverse + Null-Space Projector, Dense Convex QP (Goldfarb–Idnani), Point-Set Alignment (Kabsch / Umeyama) | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | +| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | +| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 3c225e0..53cab8d 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,7 +26,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| -| 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | | 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ | | 69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | ★★★☆☆ | | 70 | Particle filter (bounded, systematic resampling) | `filters/active` | ★★★☆☆ | @@ -34,9 +33,9 @@ Difficulty legend: | 72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` (new) | ★★★★☆ | | 73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` (new) | ★★★★★ | -Items 67–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as -NT67–NT73; NT53–NT66 and NT74 are deployed). They serve its mobile, aerial, legged, marine and -localization families and also have non-robotics consumers. Items 67 and 70 have specs under +Items 68–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as +NT68–NT73; NT53–NT67 and NT74 are deployed). They serve its mobile, aerial, legged, marine and +localization families and also have non-robotics consumers. Item 70 has a spec under [`roadmap/`](roadmap/README.md); items 68, 69 and 71–73 have none yet. All are *(float-first)*. Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family @@ -46,14 +45,7 @@ the library does not yet expose. Detailed below under ## Tier 2 — Easy ★★☆☆☆ -### 67. Kabsch / Umeyama alignment -- **What:** Least-squares rotation, translation and optional scale between two corresponding point sets - (orthogonal Procrustes), with the reflection fix, in 2-D and 3-D. -- **Embedded value:** The inner step of ICP scan matching, hand-eye and frame calibration, motion-capture - and fiducial registration. -- **Algorithm / paper:** W. Kabsch, *Acta Cryst.* A32, 1976; S. Umeyama, *IEEE T-PAMI*, 13(4), 1991. -- **Reuses:** `SingularValueDecomposition`. -- **Spec:** [roadmap/solvers/PointSetAlignment](roadmap/solvers/PointSetAlignment/implementation.md). +All Tier 2 items are deployed. --- diff --git a/doc/solvers/PointSetAlignment.md b/doc/solvers/PointSetAlignment.md new file mode 100644 index 0000000..441ac17 --- /dev/null +++ b/doc/solvers/PointSetAlignment.md @@ -0,0 +1,101 @@ +# Point-Set Alignment (Kabsch / Umeyama) + +## Overview & Motivation + +Many estimation problems reduce to one question: given two sets of corresponding points, which rigid motion maps one onto the other? Scan matching asks it at every iteration of ICP. Calibration asks it to relate a camera to a robot flange or a motion-capture frame to a robot base. Fiducial and marker registration asks it to place a tracked object. + +The **Kabsch algorithm** answers in closed form. It finds the rotation and translation that minimise the weighted sum of squared distances between the transformed source points and their targets. **Umeyama's** extension also estimates a uniform scale. In three dimensions the solution needs one $3 \times 3$ singular value decomposition, and in two dimensions a single arctangent. +Neither involves iteration or an initial guess. + +## Mathematical Theory + +### Problem + +Given source points $p_i$, target points $q_i$ and non-negative weights $w_i$, find a proper rotation $R \in SO(d)$, a translation $t$ and optionally a scale $s > 0$ that minimise + +$$E(R, t, s) = \sum_i w_i \left\| q_i - (s R p_i + t) \right\|^2$$ + +### Centring + +With the total weight $W = \sum_i w_i$ and the weighted centroids $\mu_p$ and $\mu_q$, the optimal translation always maps $\mu_p$ onto $\mu_q$. The rotation follows from the centred points $a_i = p_i - \mu_p$ and $b_i = q_i - \mu_q$ through the cross-covariance + +$$\Sigma = \frac{1}{W}\sum_i w_i\, b_i a_i^T, \qquad \sigma_p^2 = \frac{1}{W}\sum_i w_i \|a_i\|^2$$ + +Accumulating on centred coordinates avoids the single-precision cancellation of the textbook form $\sum p q^T - N \mu_p \mu_q^T$ when the clouds are far from the origin. + +### Three Dimensions: Kabsch with the Reflection Fix + +With the singular value decomposition $\Sigma = U\,\mathrm{diag}(d_0, d_1, d_2)\,V^T$, the optimal rotation is + +$$R = U\,\mathrm{diag}\big(1, 1, \operatorname{sign}(\det U \det V)\big)\,V^T$$ + +The sign term prevents a reflection ($\det R = -1$), which the unconstrained solution $U V^T$ returns for mirrored, noisy or near-planar data. It flips the weakest axis instead. Collinear points leave the rotation about their line undetermined: $d_1$ vanishes, and the alignment is refused. + +### Two Dimensions: Closed Form + +In the plane, the optimal angle follows directly from the cross-covariance, with no decomposition: + +$$\theta = \operatorname{atan2}\big(\Sigma_{10} - \Sigma_{01},\; \Sigma_{00} + \Sigma_{11}\big)$$ + +The result is always a proper rotation. Only coincident points are degenerate. + +### Scale and Residual + +Umeyama's scale is the trace of $R^T\Sigma$ divided by the source spread: + +$$s = \frac{\operatorname{tr}(R^T \Sigma)}{\sigma_p^2}, \qquad t = \mu_q - s R \mu_p$$ + +The reported residual is the weighted root-mean-square distance after alignment. Weights of zero remove points, such as outliers rejected by an ICP iteration, without copying the point sets. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|------------------|--------|--------|--------------------------------------------------------------| +| Two dimensions | $O(N)$ | $O(1)$ | Two passes over the points and one arctangent | +| Three dimensions | $O(N)$ | $O(1)$ | Two passes and one $3 \times 3$ singular value decomposition | +| Residual | $O(N)$ | $O(1)$ | One extra pass over the points | + +## Step-by-Step Walkthrough + +Four planar points $(0, 0)$, $(1, 0)$, $(0, 2)$, $(-1, 1)$ and five spatial points $(0, 0, 0)$, $(1, 0, 0)$, $(0, 2, 0)$, $(0, 0, 3)$, $(1, 1, 1)$: + +1. **Planar motion**: rotating the planar points by $30°$ and shifting them by $(0.5, -1)$ is recovered exactly, with zero residual. +2. **Spatial motion**: a rotation of $0.7$ rad about $(1, 2, 3)/\sqrt{14}$ plus an offset $(0.2, -0.4, 1.0)$ is recovered to $10^{-5}$. +3. **Scale**: the same motion with the points scaled by $2.5$ gives $s = 2.5$ when scale estimation is on. +4. **Mirror**: matching the spatial points against their mirror image in the $z$ axis returns a proper rotation with residual $0.925$. A reflection would fit them exactly, which is exactly what the sign fix prevents. +5. **Outlier**: moving one target point by $(5, 5, 5)$ raises the residual to $3.44$. Giving that point zero weight recovers the exact motion. +6. **Noise**: adding noise of about $1$ cm to the planar targets gives the angle $0.5287$ rad, the translation $(0.508, -0.997)$ and a residual of $0.0165$. +7. **Degenerate inputs**: collinear spatial points, a single planar point, coincident planar points and mismatched sizes are all refused. + +## Pitfalls & Edge Cases + +- **Correspondences**: the algorithm assumes `target[i]` corresponds to `source[i]`. Wrong pairs give a wrong but confident answer. ICP alternates association and alignment for this reason. +- **Reflections**: dropping the sign fix lets mirrored or planar data produce an improper rotation. +- **Collinear data**: in three dimensions, points on a line leave one rotation axis free. The alignment is refused rather than returning an arbitrary rotation. +- **Scale on noisy data**: Umeyama's scale is biased low when the source points are noisy. Estimate scale only when it is genuinely unknown. +- **Weights**: at least as many positively weighted points as dimensions are required. + +## Variants & Generalizations + +- **Quaternion method (Horn)**: the same optimum from the largest eigenvector of a $4 \times 4$ matrix. +- **Point-to-plane and point-to-line metrics**: linearised alignment for surfaces and contours, used in ICP. +- **Robust alignment**: iteratively reweighted least squares with Huber or Tukey weights. +- **Hand-eye calibration**: solving $AX = XB$ with rotations aligned in the same spirit. + +## Applications + +- The alignment step of point-to-point ICP scan matching. +- Frame and hand-eye calibration from paired measurements. +- Fiducial marker, motion-capture and medical image registration. +- Shape analysis and structural biology (root-mean-square deviation of molecular structures). + +## Connections to Other Algorithms + +- [Singular Value Decomposition](SingularValueDecomposition.md): provides the rotation in three dimensions. +- [Levenberg–Marquardt](../optimization/LevenbergMarquardt.md): the iterative alternative for non-rigid or non-Euclidean residuals. + +## References & Further Reading + +- Kabsch, W. (1976). "A solution for the best rotation to relate two sets of vectors." *Acta Crystallographica*, A32, 922–923. +- Umeyama, S. (1991). "Least-squares estimation of transformation parameters between two point patterns." *IEEE Transactions on Pattern Analysis and Machine Intelligence*, 13(4), 376–380. +- Arun, K.S., Huang, T.S. and Blostein, S.D. (1987). "Least-squares fitting of two 3-D point sets." *IEEE Transactions on Pattern Analysis and Machine Intelligence*, 9(5), 698–700. diff --git a/doc/solvers/README.md b/doc/solvers/README.md index d62fc34..3de87bf 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -4,20 +4,21 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations. ## Algorithms -| Algorithm | Description | -|----------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------| -| [Gaussian Elimination](GaussianElimination.md) | Direct solver for dense linear systems using partial pivoting | -| [Levinson-Durbin](LevinsonDurbin.md) | Fast solver for Toeplitz linear systems exploiting structural symmetry | -| [Durand-Kerner](DurandKerner.md) | Simultaneous iterative root-finder for polynomials | -| [Discrete Algebraic Riccati Equation](DiscreteAlgebraicRiccatiEquation.md) | Iterative solver for the DARE arising in LQR and Kalman filter design | -| [Runge-Kutta ODE Integrators](RungeKuttaIntegrators.md) | Fixed-step RK4 and adaptive Dormand-Prince RK45 for ODE integration | -| [Spectral Radius](SpectralRadius.md) | Dominant eigenvalue magnitude and discrete-time Schur stability margin | -| [Condition Number](ConditionNumber.md) | 1-norm condition number estimate via column-wise inverse solve | -| [QR Decomposition](QrDecomposition.md) | Householder factorization and Givens streaming row update for least-squares solves | -| [LU Decomposition](LuDecomposition.md) | PA = LU factorization with partial pivoting for general dense linear systems | -| [Lyapunov / Sylvester Solvers](LyapunovSylvester.md) | Sylvester AX+XB=C and continuous/discrete Lyapunov solvers via Kronecker vectorisation | -| [Singular Value Decomposition](SingularValueDecomposition.md) | Golub-Kahan bidiagonalization and implicit-shift QR for SVD, pseudo-inverse, rank, and condition number | -| [Jacobi Eigenvalue Solver](JacobiEigenSolver.md) | Cyclic Jacobi rotations for the full symmetric eigenvalue/eigenvector problem | -| [Symplectic Integrators](SymplecticIntegrators.md) | Semi-implicit Euler and velocity Verlet for second-order systems — bounded energy error on conservative plants | -| [Damped Pseudo-Inverse](DampedPseudoInverse.md) | Damped (Tikhonov) least-squares inverse and exact null-space projector from one SVD — tall or wide matrices | -| [Active-Set QP (Goldfarb–Idnani)](ActiveSetQp.md) | Dense strictly convex QP with equality and inequality constraints via the dual active-set method — no feasible start needed | +| Algorithm | Description | +|----------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Gaussian Elimination](GaussianElimination.md) | Direct solver for dense linear systems using partial pivoting | +| [Levinson-Durbin](LevinsonDurbin.md) | Fast solver for Toeplitz linear systems exploiting structural symmetry | +| [Durand-Kerner](DurandKerner.md) | Simultaneous iterative root-finder for polynomials | +| [Discrete Algebraic Riccati Equation](DiscreteAlgebraicRiccatiEquation.md) | Iterative solver for the DARE arising in LQR and Kalman filter design | +| [Runge-Kutta ODE Integrators](RungeKuttaIntegrators.md) | Fixed-step RK4 and adaptive Dormand-Prince RK45 for ODE integration | +| [Spectral Radius](SpectralRadius.md) | Dominant eigenvalue magnitude and discrete-time Schur stability margin | +| [Condition Number](ConditionNumber.md) | 1-norm condition number estimate via column-wise inverse solve | +| [QR Decomposition](QrDecomposition.md) | Householder factorization and Givens streaming row update for least-squares solves | +| [LU Decomposition](LuDecomposition.md) | PA = LU factorization with partial pivoting for general dense linear systems | +| [Lyapunov / Sylvester Solvers](LyapunovSylvester.md) | Sylvester AX+XB=C and continuous/discrete Lyapunov solvers via Kronecker vectorisation | +| [Singular Value Decomposition](SingularValueDecomposition.md) | Golub-Kahan bidiagonalization and implicit-shift QR for SVD, pseudo-inverse, rank, and condition number | +| [Jacobi Eigenvalue Solver](JacobiEigenSolver.md) | Cyclic Jacobi rotations for the full symmetric eigenvalue/eigenvector problem | +| [Symplectic Integrators](SymplecticIntegrators.md) | Semi-implicit Euler and velocity Verlet for second-order systems — bounded energy error on conservative plants | +| [Damped Pseudo-Inverse](DampedPseudoInverse.md) | Damped (Tikhonov) least-squares inverse and exact null-space projector from one SVD — tall or wide matrices | +| [Active-Set QP (Goldfarb–Idnani)](ActiveSetQp.md) | Dense strictly convex QP with equality and inequality constraints via the dual active-set method — no feasible start needed | +| [Point-Set Alignment (Kabsch / Umeyama)](PointSetAlignment.md) | Weighted least-squares rotation, translation and optional scale between corresponding point sets — closed form in 2-D, one SVD with the reflection fix in 3-D | diff --git a/numerical/solvers/CMakeLists.txt b/numerical/solvers/CMakeLists.txt index e97e39a..e1c0881 100644 --- a/numerical/solvers/CMakeLists.txt +++ b/numerical/solvers/CMakeLists.txt @@ -23,6 +23,7 @@ target_sources(numerical.solver PRIVATE LuDecomposition.hpp LyapunovSylvester.hpp OdeSystem.hpp + PointSetAlignment.hpp QrDecomposition.hpp RungeKuttaIntegrators.hpp SecondOrderSystem.hpp @@ -42,6 +43,7 @@ numerical_add_coverage_sources(numerical.solver LevinsonDurbin.cpp LuDecomposition.cpp LyapunovSylvester.cpp + PointSetAlignment.cpp QrDecomposition.cpp RungeKuttaIntegrators.cpp SingularValueDecomposition.cpp diff --git a/numerical/solvers/PointSetAlignment.cpp b/numerical/solvers/PointSetAlignment.cpp new file mode 100644 index 0000000..93aca8c --- /dev/null +++ b/numerical/solvers/PointSetAlignment.cpp @@ -0,0 +1,7 @@ +#include "numerical/solvers/PointSetAlignment.hpp" + +namespace solvers +{ + template class PointSetAlignment; + template class PointSetAlignment; +} diff --git a/numerical/solvers/PointSetAlignment.hpp b/numerical/solvers/PointSetAlignment.hpp new file mode 100644 index 0000000..4d9051b --- /dev/null +++ b/numerical/solvers/PointSetAlignment.hpp @@ -0,0 +1,180 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/MatrixNorms.hpp" +#include "numerical/solvers/SingularValueDecomposition.hpp" +#include +#include +#include +#include + +namespace solvers +{ + template + struct Alignment + { + static_assert(std::is_floating_point_v, "Alignment supports floating-point types"); + static_assert(Dim == 2 || Dim == 3, "Alignment supports two or three dimensions"); + + math::SquareMatrix rotation{ math::SquareMatrix::Identity() }; + math::Vector translation{}; + T scale{ 1 }; + T rms{}; + }; + + template + class PointSetAlignment + { + static_assert(std::is_floating_point_v, "PointSetAlignment supports floating-point types"); + static_assert(Dim == 2 || Dim == 3, "PointSetAlignment supports two or three dimensions"); + + public: + using Point = math::Vector; + using Rotation = math::SquareMatrix; + + OPTIMIZE_FOR_SPEED static std::optional> Align(std::span source, std::span target, bool estimateScale = false, + std::span weights = {}); + + private: + struct Moments + { + Point sourceMean; + Point targetMean; + T sourceSpread; + T targetSpread; + Rotation crossCovariance; + T totalWeight; + }; + + struct Fit + { + Rotation rotation; + T trace; + }; + + static constexpr T tolerance{ T{ 1e-5f } }; + + static T Weight(std::span weights, std::size_t index); + static std::optional Accumulate(std::span source, std::span target, std::span weights); + static std::optional SolveRotation(const Moments& moments); + static T Determinant(const math::SquareMatrix& m); + static T Rms(std::span source, std::span target, std::span weights, const Alignment& alignment, T totalWeight); + }; + + template + OPTIMIZE_FOR_SPEED std::optional> PointSetAlignment::Align(std::span source, std::span target, bool estimateScale, + std::span weights) + { + const auto moments{ Accumulate(source, target, weights) }; + if (!moments) + return std::nullopt; + + const auto fit{ SolveRotation(*moments) }; + if (!fit) + return std::nullopt; + + Alignment alignment{}; + alignment.rotation = fit->rotation; + alignment.scale = estimateScale ? fit->trace / moments->sourceSpread : T{ 1 }; + alignment.translation = moments->targetMean - fit->rotation * moments->sourceMean * alignment.scale; + alignment.rms = Rms(source, target, weights, alignment, moments->totalWeight); + return alignment; + } + + template + T PointSetAlignment::Weight(std::span weights, std::size_t index) + { + return weights.empty() ? T{ 1 } : weights[index]; + } + + template + auto PointSetAlignment::Accumulate(std::span source, std::span target, std::span weights) -> std::optional + { + if (source.size() != target.size() || (!weights.empty() && weights.size() != source.size())) + return std::nullopt; + + Moments moments{}; + std::size_t supported{ 0 }; + for (std::size_t i = 0; i < source.size(); ++i) + { + const T weight{ Weight(weights, i) }; + supported += weight > T{} ? 1 : 0; + moments.totalWeight += weight; + moments.sourceMean = moments.sourceMean + source[i] * weight; + moments.targetMean = moments.targetMean + target[i] * weight; + } + + if (supported < Dim) + return std::nullopt; + + moments.sourceMean = moments.sourceMean * (T{ 1 } / moments.totalWeight); + moments.targetMean = moments.targetMean * (T{ 1 } / moments.totalWeight); + + for (std::size_t i = 0; i < source.size(); ++i) + { + const T weight{ Weight(weights, i) / moments.totalWeight }; + const Point a{ source[i] - moments.sourceMean }; + const Point b{ target[i] - moments.targetMean }; + moments.sourceSpread += weight * math::DotProduct(a, a); + moments.targetSpread += weight * math::DotProduct(b, b); + moments.crossCovariance = moments.crossCovariance + b * a.Transpose() * weight; + } + + return moments; + } + + template + auto PointSetAlignment::SolveRotation(const Moments& moments) -> std::optional + { + const auto& cross{ moments.crossCovariance }; + if constexpr (Dim == 2) + { + const T cosine{ cross.at(0, 0) + cross.at(1, 1) }; + const T sine{ cross.at(1, 0) - cross.at(0, 1) }; + const T magnitude{ math::Hypot(cosine, sine) }; + if (magnitude <= tolerance * math::Sqrt(moments.sourceSpread * moments.targetSpread)) + return std::nullopt; + + const T angle{ math::Atan2(sine, cosine) }; + return Fit{ Rotation{ { math::Cos(angle), -math::Sin(angle) }, { math::Sin(angle), math::Cos(angle) } }, magnitude }; + } + else + { + SingularValueDecomposition svd{}; + const bool decomposed{ svd.Decompose(cross) }; + const auto& singular{ svd.SingularValues() }; + if (!decomposed || singular[1] <= tolerance * singular[0]) + return std::nullopt; + + const T sign{ Determinant(svd.U()) * Determinant(svd.V()) < T{} ? T{ -1 } : T{ 1 } }; + const Rotation flip{ { T{ 1 }, T{}, T{} }, { T{}, T{ 1 }, T{} }, { T{}, T{}, sign } }; + return Fit{ svd.U() * flip * svd.V().Transpose(), singular[0] + singular[1] + sign * singular[2] }; + } + } + + template + T PointSetAlignment::Determinant(const math::SquareMatrix& m) + { + return m.at(0, 0) * (m.at(1, 1) * m.at(2, 2) - m.at(1, 2) * m.at(2, 1)) - m.at(0, 1) * (m.at(1, 0) * m.at(2, 2) - m.at(1, 2) * m.at(2, 0)) + + m.at(0, 2) * (m.at(1, 0) * m.at(2, 1) - m.at(1, 1) * m.at(2, 0)); + } + + template + T PointSetAlignment::Rms(std::span source, std::span target, std::span weights, const Alignment& alignment, T totalWeight) + { + T sum{}; + for (std::size_t i = 0; i < source.size(); ++i) + { + const Point error{ target[i] - (alignment.rotation * source[i] * alignment.scale + alignment.translation) }; + sum += Weight(weights, i) * math::DotProduct(error, error); + } + return math::Sqrt(sum / totalWeight); + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class PointSetAlignment; + extern template class PointSetAlignment; +#endif +} diff --git a/numerical/solvers/test/CMakeLists.txt b/numerical/solvers/test/CMakeLists.txt index cad72eb..9ad9030 100644 --- a/numerical/solvers/test/CMakeLists.txt +++ b/numerical/solvers/test/CMakeLists.txt @@ -20,6 +20,7 @@ target_sources(numerical.solvers_test PRIVATE TestLevinsonDurbin.cpp TestLuDecomposition.cpp TestLyapunovSylvester.cpp + TestPointSetAlignment.cpp TestQrDecomposition.cpp TestRungeKuttaIntegrators.cpp TestSingularValueDecomposition.cpp diff --git a/numerical/solvers/test/TestPointSetAlignment.cpp b/numerical/solvers/test/TestPointSetAlignment.cpp new file mode 100644 index 0000000..5ca8e23 --- /dev/null +++ b/numerical/solvers/test/TestPointSetAlignment.cpp @@ -0,0 +1,161 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/solvers/PointSetAlignment.hpp" +#include +#include +#include +#include +#include + +namespace +{ + using Point2 = math::Vector; + using Point3 = math::Vector; + using Matrix3 = math::SquareMatrix; + using Planar = solvers::PointSetAlignment; + using Spatial = solvers::PointSetAlignment; + + constexpr float pi{ std::numbers::pi_v }; + + class TestPointSetAlignment + : public ::testing::Test + { + protected: + static Point3 Normalized(float x, float y, float z) + { + const float norm{ std::sqrt(x * x + y * y + z * z) }; + return Point3{ x / norm, y / norm, z / norm }; + } + + static std::array MovePlanar(const std::array& points, float angle, const Point2& shift) + { + std::array moved{}; + for (std::size_t i = 0; i < points.size(); ++i) + moved[i] = Point2{ std::cos(angle) * points[i][0] - std::sin(angle) * points[i][1] + shift[0], std::sin(angle) * points[i][0] + std::cos(angle) * points[i][1] + shift[1] }; + return moved; + } + + std::array MoveSpatial(float scale) const + { + std::array moved{}; + for (std::size_t i = 0; i < spatial.size(); ++i) + moved[i] = rotation * spatial[i] * scale + offset; + return moved; + } + + static void ExpectNear(const Point3& actual, const Point3& expected, float tolerance) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + static void ExpectNear(const Matrix3& actual, const Matrix3& expected) + { + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(actual.at(r, c), expected.at(r, c), 1e-5f); + } + + std::array planar{ Point2{ 0.0f, 0.0f }, Point2{ 1.0f, 0.0f }, Point2{ 0.0f, 2.0f }, Point2{ -1.0f, 1.0f } }; + std::array spatial{ Point3{ 0.0f, 0.0f, 0.0f }, Point3{ 1.0f, 0.0f, 0.0f }, Point3{ 0.0f, 2.0f, 0.0f }, Point3{ 0.0f, 0.0f, 3.0f }, Point3{ 1.0f, 1.0f, 1.0f } }; + Matrix3 rotation{ math::RotationAboutAxis(Normalized(1.0f, 2.0f, 3.0f), 0.7f) }; + Point3 offset{ 0.2f, -0.4f, 1.0f }; + }; +} + +TEST_F(TestPointSetAlignment, planar_rigid_motion_is_recovered_exactly) +{ + const auto target{ MovePlanar(planar, pi / 6.0f, Point2{ 0.5f, -1.0f }) }; + + const auto alignment{ Planar::Align(planar, target) }; + + ASSERT_TRUE(alignment.has_value()); + EXPECT_NEAR(std::atan2(alignment->rotation.at(1, 0), alignment->rotation.at(0, 0)), pi / 6.0f, 1e-5f); + EXPECT_NEAR(alignment->translation[0], 0.5f, 1e-5f); + EXPECT_NEAR(alignment->translation[1], -1.0f, 1e-5f); + EXPECT_NEAR(alignment->rms, 0.0f, 1e-5f); +} + +TEST_F(TestPointSetAlignment, spatial_rigid_motion_is_recovered_exactly) +{ + const auto target{ MoveSpatial(1.0f) }; + + const auto alignment{ Spatial::Align(spatial, target) }; + + ASSERT_TRUE(alignment.has_value()); + ExpectNear(alignment->rotation, rotation); + ExpectNear(alignment->translation, offset, 1e-5f); + EXPECT_NEAR(alignment->rms, 0.0f, 1e-5f); +} + +TEST_F(TestPointSetAlignment, umeyama_recovers_the_scale) +{ + const auto target{ MoveSpatial(2.5f) }; + + const auto alignment{ Spatial::Align(spatial, target, true) }; + + ASSERT_TRUE(alignment.has_value()); + EXPECT_NEAR(alignment->scale, 2.5f, 1e-5f); + ExpectNear(alignment->translation, offset, 1e-4f); + EXPECT_NEAR(alignment->rms, 0.0f, 1e-4f); +} + +TEST_F(TestPointSetAlignment, mirrored_points_still_give_a_proper_rotation) +{ + std::array mirrored{ spatial }; + for (auto& point : mirrored) + point[2] = -point[2]; + + const auto alignment{ Spatial::Align(spatial, mirrored) }; + + ASSERT_TRUE(alignment.has_value()); + const auto& r{ alignment->rotation }; + const float determinant{ r.at(0, 0) * (r.at(1, 1) * r.at(2, 2) - r.at(1, 2) * r.at(2, 1)) - r.at(0, 1) * (r.at(1, 0) * r.at(2, 2) - r.at(1, 2) * r.at(2, 0)) + + r.at(0, 2) * (r.at(1, 0) * r.at(2, 1) - r.at(1, 1) * r.at(2, 0)) }; + EXPECT_NEAR(determinant, 1.0f, 1e-5f); + EXPECT_NEAR(alignment->rms, 0.925196f, 1e-5f); +} + +TEST_F(TestPointSetAlignment, zero_weight_removes_an_outlier) +{ + auto target{ MoveSpatial(1.0f) }; + target[4] = target[4] + Point3{ 5.0f, 5.0f, 5.0f }; + const std::array weights{ 1.0f, 1.0f, 1.0f, 1.0f, 0.0f }; + + const auto weighted{ Spatial::Align(spatial, target, false, weights) }; + const auto unweighted{ Spatial::Align(spatial, target) }; + + ASSERT_TRUE(weighted.has_value()); + ExpectNear(weighted->rotation, rotation); + EXPECT_NEAR(weighted->rms, 0.0f, 1e-5f); + ASSERT_TRUE(unweighted.has_value()); + EXPECT_NEAR(unweighted->rms, 3.442415f, 1e-4f); +} + +TEST_F(TestPointSetAlignment, noisy_planar_fit_matches_the_reference) +{ + auto target{ MovePlanar(planar, pi / 6.0f, Point2{ 0.5f, -1.0f }) }; + const std::array noise{ Point2{ 0.01f, -0.02f }, Point2{ 0.0f, 0.015f }, Point2{ -0.01f, 0.0f }, Point2{ 0.02f, 0.01f } }; + for (std::size_t i = 0; i < target.size(); ++i) + target[i] = target[i] + noise[i]; + + const auto alignment{ Planar::Align(planar, target) }; + + ASSERT_TRUE(alignment.has_value()); + EXPECT_NEAR(std::atan2(alignment->rotation.at(1, 0), alignment->rotation.at(0, 0)), 0.528695f, 1e-5f); + EXPECT_NEAR(alignment->translation[0], 0.508305f, 1e-5f); + EXPECT_NEAR(alignment->translation[1], -0.996831f, 1e-5f); + EXPECT_NEAR(alignment->rms, 0.016549f, 1e-5f); +} + +TEST_F(TestPointSetAlignment, degenerate_inputs_are_rejected) +{ + const std::array collinear{ Point3{ 0.0f, 0.0f, 0.0f }, Point3{ 1.0f, 1.0f, 1.0f }, Point3{ 2.0f, 2.0f, 2.0f } }; + const std::array single{ Point2{ 1.0f, 2.0f } }; + const std::array coincident{ Point2{ 1.0f, 2.0f }, Point2{ 1.0f, 2.0f } }; + const std::array three{ Point2{ 0.0f, 0.0f }, Point2{ 1.0f, 0.0f }, Point2{ 0.0f, 1.0f } }; + + EXPECT_FALSE(Spatial::Align(collinear, collinear).has_value()); + EXPECT_FALSE(Planar::Align(single, single).has_value()); + EXPECT_FALSE(Planar::Align(planar, three).has_value()); + EXPECT_FALSE(Planar::Align(coincident, coincident).has_value()); +} diff --git a/roadmap/README.md b/roadmap/README.md index 9ec99f0..af0b1e1 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -107,4 +107,4 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) ### `solvers` -`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) · `PointSetAlignment` (67) +`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) diff --git a/roadmap/solvers/PointSetAlignment/explanation.md b/roadmap/solvers/PointSetAlignment/explanation.md deleted file mode 100644 index cc13bde..0000000 --- a/roadmap/solvers/PointSetAlignment/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Point-Set Alignment (Kabsch / Umeyama) — Overview - -## What it is -Given two sets of matching points — the same landmarks seen in two frames, a scan and a map, a -calibration target in two poses — it finds the rotation, translation and, optionally, the uniform scale -that best overlays one set on the other in the least-squares sense. - -## Why it matters (embedded) -This is the core step of scan matching (ICP), sensor-to-sensor and hand-eye calibration, and fiducial -or motion-capture registration. The solution is closed-form: one small singular-value decomposition in -3-D, a single arctangent in 2-D. It runs in a bounded time with no iteration and no memory beyond a -few 3×3 matrices. - -## How it works (intuition) -First both point sets are shifted so their weighted centres coincide; the remaining mismatch is pure -rotation (and scale). The cross-covariance matrix of the centred sets says how directions in one set -correlate with directions in the other, and its singular-value decomposition gives the rotation that -maximises this agreement. If the best orthogonal match happens to be a mirror image, the weakest axis is -flipped so the result stays a real rotation. The scale is the ratio of the matched spread to the source -spread, and the translation moves the rotated source centre onto the target centre. - -## Key parameters -- **correspondences** — `target[i]` matches `source[i]`. -- **weights** — per-point confidence (0 drops a point). -- **estimate scale** — rigid (Kabsch) or similarity (Umeyama) fit. - -## Reference -W. Kabsch, "A solution for the best rotation to relate two sets of vectors," *Acta Crystallographica* -A32, 1976; S. Umeyama, "Least-squares estimation of transformation parameters between two point -patterns," *IEEE T-PAMI*, 13(4), 1991. - -## See also -`SingularValueDecomposition` (the 3-D solve), `SE3Transform` / `SE2Transform` (items 53, 66: wrap the result); -robotics-toolbox-cpp's ICP scan matching (E5) and hand-eye calibration (C1). diff --git a/roadmap/solvers/PointSetAlignment/implementation.md b/roadmap/solvers/PointSetAlignment/implementation.md deleted file mode 100644 index dc100f7..0000000 --- a/roadmap/solvers/PointSetAlignment/implementation.md +++ /dev/null @@ -1,93 +0,0 @@ -# Point-Set Alignment (Kabsch / Umeyama) — Implementation Pseudocode - -> Roadmap ref: #67 (Tier 2) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) - -Weighted least-squares rigid (or similarity) transform between two corresponding point sets: -find `R ∈ SO(d)`, `t` and optionally `s > 0` minimising `Σ wᵢ ‖qᵢ − (s·R·pᵢ + t)‖²`. This is the inner step -of point-to-point ICP, hand-eye and frame calibration, fiducial and motion-capture registration. In 3-D -it is one 3×3 SVD with Kabsch's reflection fix; in 2-D a closed-form `atan2`. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); static_assert(Dim == 2 || Dim == 3) -struct Alignment: - math::SquareMatrix rotation # proper: det = +1 - math::Vector translation - T scale{ 1 } # 1 unless estimated - T rms # weighted residual RMS after alignment - -template -class PointSetAlignment # stateless; static members only -``` - -## Interface - -```cpp -static std::optional> Align(std::span> source, - std::span> target, - bool estimateScale = false, - std::span weights = {}) # empty = all 1 -``` - -`target[i]` corresponds to `source[i]`. Returns `nullopt` when the sizes differ, when fewer than `Dim` -points carry positive weight, or when the geometry leaves the rotation undetermined (see below). - -## Algorithm (pseudocode) - -```text -function Align(p, q, estimateScale, w): # OPTIMIZE_FOR_SPEED - W = Σ wᵢ; μp = Σ wᵢ pᵢ / W; μq = Σ wᵢ qᵢ / W - a_i = pᵢ − μp; b_i = qᵢ − μq - σp² = Σ wᵢ ‖a_i‖² / W; σq² = Σ wᵢ ‖b_i‖² / W # spreads: σp² sets the scale - Σ = Σ wᵢ b_i a_iᵀ / W # Dim×Dim cross-covariance - - if Dim == 2: # closed form, no SVD, never a reflection - c = Σ₀₀ + Σ₁₁; s_ = Σ₁₀ − Σ₀₁ # Σ wᵢ (a·b) and Σ wᵢ (a × b), over W - if Hypot(c, s_) ≤ ε·σp·σq: return nullopt # all points coincide - θ = atan2(s_, c); R = Rot(θ); trace = Hypot(c, s_) - else: - SVD: Σ = U·diag(d)·Vᵀ # solvers::SingularValueDecomposition - if d₁ ≤ ε·d₀: return nullopt # collinear (or coincident) points: rotation about the line is free - E = diag(1, 1, sign(det U · det V)) # Kabsch: never return a reflection - R = U·E·Vᵀ; trace = d₀ + d₁ + E₂₂·d₂ - - s = estimateScale ? trace / σp² : 1 # Umeyama (1991), eq. 42 - t = μq − s·R·μp - rms = √( Σ wᵢ ‖qᵢ − (s·R·pᵢ + t)‖² / W ) - return { R, t, s, rms } -``` - -`ε = 1e-5`. `det U` and `det V` use the explicit 3×3 cofactor formula. - -## Complexity & memory - -- `O(N·Dim²)` accumulation plus one 3×3 SVD (3-D) or one `atan2` (2-D). One pass for the moments and - one for the residual. -- Stack only: a few `Dim×Dim` matrices. The point sets stay in the caller's storage. No heap. - -## Numerical / embedded notes - -- **Reflection fix.** The unconstrained Procrustes solution `U·Vᵀ` can be a reflection (`det = −1`) for - noisy or near-planar data; `E` flips the weakest axis instead. The test feeds a mirrored point set and - checks that the result is a proper rotation with a non-zero residual. -- **Degeneracy.** In 3-D a rank-1 cross-covariance (collinear points) leaves the rotation about the line - free, so `Align` refuses instead of returning an arbitrary answer. Coplanar points are fine: the sign - fix resolves them. In 2-D only coincident points are degenerate. -- **Centring first.** Accumulating `Σ` on centred coordinates avoids the `float` cancellation of the - textbook `Σ pqᵀ − N·μp μqᵀ` form when the clouds are far from the origin (scan matching in map - coordinates). -- Weights let ICP (robotics E5) down-weight or drop outliers without copying the point sets. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/solvers/PointSetAlignment.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Align`, and - `extern template class PointSetAlignment;` / `` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/solvers/PointSetAlignment.cpp` → the same instantiations. -- Test: `numerical/solvers/test/TestPointSetAlignment.cpp` -- Doc: `doc/solvers/PointSetAlignment.md` (per `doc/TEMPLATE.md`) + row in `doc/solvers/README.md` -- CMake: `.hpp` → `target_sources(numerical.solver PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.solver ...)`; test → `numerical.solvers_test`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/PointSetAlignment/tests.md b/roadmap/solvers/PointSetAlignment/tests.md deleted file mode 100644 index 9bc6d28..0000000 --- a/roadmap/solvers/PointSetAlignment/tests.md +++ /dev/null @@ -1,52 +0,0 @@ -# Point-Set Alignment — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPointSetAlignment : public ::testing::Test: - std::array, 4> planar{ (0, 0), (1, 0), (0, 2), (−1, 1) } - std::array, 5> spatial{ (0, 0, 0), (1, 0, 0), (0, 2, 0), (0, 0, 3), (1, 1, 1) } - Matrix3 rotation{ RotationAboutAxis((1, 2, 3)/‖·‖, 0.7) } # Geometry3D - Vector3 offset{ 0.2, −0.4, 1.0 } -# each case below is a TEST_F(TestPointSetAlignment, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -planar_rigid_motion_is_recovered_exactly: - Arrange: target = Rot(30°)·planar + (0.5, −1) - Assert: angle ≈ π/6, translation ≈ (0.5, −1), rms ≈ 0 -spatial_rigid_motion_is_recovered_exactly: - Arrange: target = rotation·spatial + offset - Assert: rotation ≈ the fixture rotation (1e-5), translation ≈ offset, rms ≈ 0 -umeyama_recovers_the_scale: - Arrange: target = 2.5·rotation·spatial + offset, estimateScale = true - Assert: scale ≈ 2.5, translation ≈ offset, rms ≈ 0 -mirrored_points_still_give_a_proper_rotation: - Arrange: target = spatial with z negated - Assert: det(rotation) ≈ +1, rms ≈ 0.925196 # a reflection would fit with rms 0 -zero_weight_removes_an_outlier: - Arrange: target = rotation·spatial + offset, target[4] += (5, 5, 5), weights { 1, 1, 1, 1, 0 } - Assert: rotation ≈ fixture rotation, rms ≈ 0 # with unit weights rms = 3.442415 -noisy_planar_fit_matches_the_reference: - Arrange: target as in the first case plus noise { (0.01, −0.02), (0, 0.015), (−0.01, 0), (0.02, 0.01) } - Assert: angle ≈ 0.528695, translation ≈ (0.508305, −0.996831), rms ≈ 0.016549 -degenerate_inputs_are_rejected: - Assert: collinear { (0,0,0), (1,1,1), (2,2,2) } ⇒ nullopt; one 2-D point ⇒ nullopt; - size mismatch ⇒ nullopt -``` - -## Reference vectors - -- Fixture rotation `Rot((1,2,3)/√14, 0.7)` = - `[[0.781639, −0.482929, 0.394740], [0.550117, 0.832030, −0.071392], [−0.293958, 0.272956, 0.916015]]`. -- Mirrored `spatial` (z → −z): best proper rotation leaves `rms = 0.925196`. -- Collinear set: singular values `(2, 0, 0)` ⇒ rejected. - -## Edge cases - -- Exactly `Dim` non-degenerate points (2 in 2-D, 3 non-collinear in 3-D) ⇒ exact solution. -- Translated clouds far from the origin (offset `10³`) keep `rms ≈ 0` to `1e-3`, thanks to centring.