diff --git a/README.md b/README.md index fcaa2c0..f5df22b 100644 --- a/README.md +++ b/README.md @@ -1,187 +1,187 @@ -[![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=embedded-pro_embedded-dsp-control&metric=alert_status)](https://sonarcloud.io/summary/new_code?id=embedded-pro_embedded-dsp-control) -[![Coverage](https://sonarcloud.io/api/project_badges/measure?project=embedded-pro_embedded-dsp-control&metric=coverage)](https://sonarcloud.io/summary/new_code?id=embedded-pro_embedded-dsp-control) -[![Duplicated Lines (%)](https://sonarcloud.io/api/project_badges/measure?project=embedded-pro_embedded-dsp-control&metric=duplicated_lines_density)](https://sonarcloud.io/summary/new_code?id=embedded-pro_embedded-dsp-control) - -# Numerical Algorithms Library - -## Overview - -This library provides a comprehensive collection of digital signal processing (DSP), control algorithms, filters, optimizers, and estimators designed for robust and efficient signal analysis, manipulation, and system control. Developed to address complex technical challenges, the library offers a flexible and extensible framework for engineers, researchers, and developers working in signal processing, control systems, and related domains. - -## Getting Started - -Refer to the documentation to quickly integrate and utilize the library's signal processing and control algorithms in your projects. - -## 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, 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) | -| [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 | - -Each category page lists its algorithms with a brief description and links to the detailed documentation. - -### Booklet - -The entire documentation set is also published as a single book — read it online as a -[GitHub Pages site](https://embedded-pro.github.io/embedded-dsp-control/) or download the latest -PDF from the [Releases page](../../releases/latest). Both are generated automatically from `doc/` -(cover, Summary/table of contents, one chapter per category, consolidated references, back cover). - -Build it locally with [Pandoc](https://pandoc.org) + XeLaTeX installed: - -```bash -python scripts/build-booklet.py --format all # writes build/booklet/{NumericalToolbox.pdf,index.html} -``` - -## Simulator - -The `simulator/` directory contains interactive Qt-based GUI applications for visualizing and experimenting with the library's algorithms. These are desktop tools intended for development and exploration, separate from the core embedded-targeted library. - -### Building the Simulator - -The simulator requires Qt6 and is disabled by default. Enable it with: - -```bash -cmake --preset host # host preset enables it automatically -# or manually: -cmake -DNUMERICAL_TOOLBOX_BUILD_SIMULATOR=ON ... -``` - -Prerequisites: `qt6-base-dev` and `libgl1-mesa-dev` (Ubuntu/Debian). - -### How to Run the Simulator - -The simulator includes a Qt-based GUI for real-time visualization. Since the development environment runs inside a Dev Container, an X server on the host machine is required to display the GUI. - -#### Host Setup - -##### Windows - -1. Install VcXsrv (free) or X410 from the Microsoft Store. -2. Launch VcXsrv with the following settings: - - Multiple windows - - Start no client - - Disable access control (checked) -3. Open the project in VS Code and reopen in the Dev Container. - -##### Linux - -1. Allow Docker containers to access your X server: - -```bash -xhost +local:docker -``` - -2. Open the project in VS Code and reopen in the Dev Container. - -> **Note:** The Dev Container sets `DISPLAY=host.docker.internal:0.0` to forward GUI windows over TCP. On Linux, if you prefer Unix socket forwarding, you can override `DISPLAY` to `:0` inside the container and add a bind mount for `/tmp/.X11-unix`. - - -## Math Function Overrides - -All math functions used by the library (`Sin`, `Cos`, `Abs`, `Sqrt`, `Pow`, …) are centralised in -`numerical/math/Math.hpp` under the `math::` namespace. -Each function ships with a default `constexpr` implementation that forwards to the corresponding -`std::` counterpart, but every one of them can be replaced at compile time with a -platform-specific implementation — CORDIC, hardware intrinsics, look-up tables, etc. - -### How to override a function - -**1. Define the override macro in CMake** (suppresses the default definition): - -```cmake -target_compile_definitions(your_target PRIVATE MATH_SIN_OVERRIDE MATH_COS_OVERRIDE) -``` - -**2. Provide your own template definition** in a header included before any call site: - -```cpp -// platform/PlatformMath.hpp -#pragma once -namespace math -{ - template - constexpr T Sin(T x) { return cordic_sin(static_cast(x)); } - - template - constexpr T Cos(T x) { return cordic_cos(static_cast(x)); } -} -``` - -**3. Include your header** before `numerical/math/Math.hpp` — or use a CMake forced-include so it -applies to every translation unit automatically: - -```cmake -target_compile_options(your_target PRIVATE -include platform/PlatformMath.hpp) -``` - -### Available override macros - -| Macro | Function | -|--------------------------|------------------| -| `MATH_ABS_OVERRIDE` | `math::Abs` | -| `MATH_SQRT_OVERRIDE` | `math::Sqrt` | -| `MATH_SIN_OVERRIDE` | `math::Sin` | -| `MATH_COS_OVERRIDE` | `math::Cos` | -| `MATH_TAN_OVERRIDE` | `math::Tan` | -| `MATH_ASIN_OVERRIDE` | `math::Asin` | -| `MATH_ACOS_OVERRIDE` | `math::Acos` | -| `MATH_ATAN_OVERRIDE` | `math::Atan` | -| `MATH_ATAN2_OVERRIDE` | `math::Atan2` | -| `MATH_EXP_OVERRIDE` | `math::Exp` | -| `MATH_LOG_OVERRIDE` | `math::Log` | -| `MATH_LOG10_OVERRIDE` | `math::Log10` | -| `MATH_LOG2_OVERRIDE` | `math::Log2` | -| `MATH_POW_OVERRIDE` | `math::Pow` | -| `MATH_SINH_OVERRIDE` | `math::Sinh` | -| `MATH_COSH_OVERRIDE` | `math::Cosh` | -| `MATH_TANH_OVERRIDE` | `math::Tanh` | -| `MATH_HYPOT_OVERRIDE` | `math::Hypot` | -| `MATH_COPYSIGN_OVERRIDE` | `math::Copysign` | -| `MATH_FMOD_OVERRIDE` | `math::Fmod` | -| `MATH_CEIL_OVERRIDE` | `math::Ceil` | -| `MATH_FLOOR_OVERRIDE` | `math::Floor` | -| `MATH_ROUND_OVERRIDE` | `math::Round` | -| `MATH_ERFC_OVERRIDE` | `math::Erfc` | - -Overrides are fully compile-time: the default body is excluded from the translation unit, so the -replacement is used even for inlined calls. Non-overridden functions continue to use the `std::` -defaults unchanged. - -## Roadmap - -Planned algorithms and components are tracked in [ROADMAP.md](ROADMAP.md) — a prioritized backlog -of DSP filters, controllers, estimators, and solvers ordered by implementation difficulty. - -## Testing - -Every algorithm is validated against the **mathematical invariants of its family** — not golden -output. A low-pass filter must attenuate above cutoff, an ODE integrator must reproduce a known -analytic solution to its order, a Kalman filter's covariance must stay positive-definite. Tests are -`TEST_F` on `float`, no heap, one behaviour per test, asserted against independent reference values. - -The per-family metric strategy — which properties to assert for each algorithm type (accuracy, -frequency/transient response, stability, boundaries, invariants, convergence, statistical -consistency, conditioning) — is documented in **[TESTING.md](TESTING.md)**. Mechanics and framework -rules live in [.github/instructions/testing.instructions.md](.github/instructions/testing.instructions.md). - -## Contributing - -Contributions, issues, and feature requests are welcome. Please check the contributing guidelines before submitting pull requests. - -## License - -[Specify your license here - e.g., MIT, Apache 2.0] - +[![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=embedded-pro_embedded-dsp-control&metric=alert_status)](https://sonarcloud.io/summary/new_code?id=embedded-pro_embedded-dsp-control) +[![Coverage](https://sonarcloud.io/api/project_badges/measure?project=embedded-pro_embedded-dsp-control&metric=coverage)](https://sonarcloud.io/summary/new_code?id=embedded-pro_embedded-dsp-control) +[![Duplicated Lines (%)](https://sonarcloud.io/api/project_badges/measure?project=embedded-pro_embedded-dsp-control&metric=duplicated_lines_density)](https://sonarcloud.io/summary/new_code?id=embedded-pro_embedded-dsp-control) + +# Numerical Algorithms Library + +## Overview + +This library provides a comprehensive collection of digital signal processing (DSP), control algorithms, filters, optimizers, and estimators designed for robust and efficient signal analysis, manipulation, and system control. Developed to address complex technical challenges, the library offers a flexible and extensible framework for engineers, researchers, and developers working in signal processing, control systems, and related domains. + +## Getting Started + +Refer to the documentation to quickly integrate and utilize the library's signal processing and control algorithms in your projects. + +## 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, 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 | + +Each category page lists its algorithms with a brief description and links to the detailed documentation. + +### Booklet + +The entire documentation set is also published as a single book — read it online as a +[GitHub Pages site](https://embedded-pro.github.io/embedded-dsp-control/) or download the latest +PDF from the [Releases page](../../releases/latest). Both are generated automatically from `doc/` +(cover, Summary/table of contents, one chapter per category, consolidated references, back cover). + +Build it locally with [Pandoc](https://pandoc.org) + XeLaTeX installed: + +```bash +python scripts/build-booklet.py --format all # writes build/booklet/{NumericalToolbox.pdf,index.html} +``` + +## Simulator + +The `simulator/` directory contains interactive Qt-based GUI applications for visualizing and experimenting with the library's algorithms. These are desktop tools intended for development and exploration, separate from the core embedded-targeted library. + +### Building the Simulator + +The simulator requires Qt6 and is disabled by default. Enable it with: + +```bash +cmake --preset host # host preset enables it automatically +# or manually: +cmake -DNUMERICAL_TOOLBOX_BUILD_SIMULATOR=ON ... +``` + +Prerequisites: `qt6-base-dev` and `libgl1-mesa-dev` (Ubuntu/Debian). + +### How to Run the Simulator + +The simulator includes a Qt-based GUI for real-time visualization. Since the development environment runs inside a Dev Container, an X server on the host machine is required to display the GUI. + +#### Host Setup + +##### Windows + +1. Install VcXsrv (free) or X410 from the Microsoft Store. +2. Launch VcXsrv with the following settings: + - Multiple windows + - Start no client + - Disable access control (checked) +3. Open the project in VS Code and reopen in the Dev Container. + +##### Linux + +1. Allow Docker containers to access your X server: + +```bash +xhost +local:docker +``` + +2. Open the project in VS Code and reopen in the Dev Container. + +> **Note:** The Dev Container sets `DISPLAY=host.docker.internal:0.0` to forward GUI windows over TCP. On Linux, if you prefer Unix socket forwarding, you can override `DISPLAY` to `:0` inside the container and add a bind mount for `/tmp/.X11-unix`. + + +## Math Function Overrides + +All math functions used by the library (`Sin`, `Cos`, `Abs`, `Sqrt`, `Pow`, …) are centralised in +`numerical/math/Math.hpp` under the `math::` namespace. +Each function ships with a default `constexpr` implementation that forwards to the corresponding +`std::` counterpart, but every one of them can be replaced at compile time with a +platform-specific implementation — CORDIC, hardware intrinsics, look-up tables, etc. + +### How to override a function + +**1. Define the override macro in CMake** (suppresses the default definition): + +```cmake +target_compile_definitions(your_target PRIVATE MATH_SIN_OVERRIDE MATH_COS_OVERRIDE) +``` + +**2. Provide your own template definition** in a header included before any call site: + +```cpp +// platform/PlatformMath.hpp +#pragma once +namespace math +{ + template + constexpr T Sin(T x) { return cordic_sin(static_cast(x)); } + + template + constexpr T Cos(T x) { return cordic_cos(static_cast(x)); } +} +``` + +**3. Include your header** before `numerical/math/Math.hpp` — or use a CMake forced-include so it +applies to every translation unit automatically: + +```cmake +target_compile_options(your_target PRIVATE -include platform/PlatformMath.hpp) +``` + +### Available override macros + +| Macro | Function | +|--------------------------|------------------| +| `MATH_ABS_OVERRIDE` | `math::Abs` | +| `MATH_SQRT_OVERRIDE` | `math::Sqrt` | +| `MATH_SIN_OVERRIDE` | `math::Sin` | +| `MATH_COS_OVERRIDE` | `math::Cos` | +| `MATH_TAN_OVERRIDE` | `math::Tan` | +| `MATH_ASIN_OVERRIDE` | `math::Asin` | +| `MATH_ACOS_OVERRIDE` | `math::Acos` | +| `MATH_ATAN_OVERRIDE` | `math::Atan` | +| `MATH_ATAN2_OVERRIDE` | `math::Atan2` | +| `MATH_EXP_OVERRIDE` | `math::Exp` | +| `MATH_LOG_OVERRIDE` | `math::Log` | +| `MATH_LOG10_OVERRIDE` | `math::Log10` | +| `MATH_LOG2_OVERRIDE` | `math::Log2` | +| `MATH_POW_OVERRIDE` | `math::Pow` | +| `MATH_SINH_OVERRIDE` | `math::Sinh` | +| `MATH_COSH_OVERRIDE` | `math::Cosh` | +| `MATH_TANH_OVERRIDE` | `math::Tanh` | +| `MATH_HYPOT_OVERRIDE` | `math::Hypot` | +| `MATH_COPYSIGN_OVERRIDE` | `math::Copysign` | +| `MATH_FMOD_OVERRIDE` | `math::Fmod` | +| `MATH_CEIL_OVERRIDE` | `math::Ceil` | +| `MATH_FLOOR_OVERRIDE` | `math::Floor` | +| `MATH_ROUND_OVERRIDE` | `math::Round` | +| `MATH_ERFC_OVERRIDE` | `math::Erfc` | + +Overrides are fully compile-time: the default body is excluded from the translation unit, so the +replacement is used even for inlined calls. Non-overridden functions continue to use the `std::` +defaults unchanged. + +## Roadmap + +Planned algorithms and components are tracked in [ROADMAP.md](ROADMAP.md) — a prioritized backlog +of DSP filters, controllers, estimators, and solvers ordered by implementation difficulty. + +## Testing + +Every algorithm is validated against the **mathematical invariants of its family** — not golden +output. A low-pass filter must attenuate above cutoff, an ODE integrator must reproduce a known +analytic solution to its order, a Kalman filter's covariance must stay positive-definite. Tests are +`TEST_F` on `float`, no heap, one behaviour per test, asserted against independent reference values. + +The per-family metric strategy — which properties to assert for each algorithm type (accuracy, +frequency/transient response, stability, boundaries, invariants, convergence, statistical +consistency, conditioning) — is documented in **[TESTING.md](TESTING.md)**. Mechanics and framework +rules live in [.github/instructions/testing.instructions.md](.github/instructions/testing.instructions.md). + +## Contributing + +Contributions, issues, and feature requests are welcome. Please check the contributing guidelines before submitting pull requests. + +## License + +[Specify your license here - e.g., MIT, Apache 2.0] + diff --git a/ROADMAP.md b/ROADMAP.md index 2728561..add2919 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,7 +26,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| -| 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | | 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | | 74 | Residual-form Kalman update with χ² gating | `filters/active` | ★★☆☆☆ | | 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ | @@ -36,9 +35,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 66–74 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as -NT66–NT74; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization -families and also have non-robotics consumers. Items 66, 67, 70 and 74 have specs under +Items 67–74 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as +NT67–NT74; NT53–NT66 are deployed). They serve its mobile, aerial, legged, marine and localization +families and also have non-robotics consumers. Items 67, 70 and 74 have specs 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 @@ -48,16 +47,6 @@ the library does not yet expose. Detailed below under ## Tier 2 — Easy ★★☆☆☆ -### 66. SE(2) transform -- **What:** Planar rigid transform `(R(θ), p)` with composition, closed-form inverse, adjoint, exp/log of - planar twists `(vx, vy, ω)` and a pose error, ordered linear part first like item 53. -- **Embedded value:** The pose type of every ground vehicle and planar mechanism — odometry, path - tracking, scan matching — at a fraction of SE(3)'s cost. -- **Algorithm / paper:** J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in - robotics" (2018), Appendix C. -- **Reuses:** Item 53 (ordering convention, `ToSE3`). -- **Spec:** [roadmap/math/SE2Transform](roadmap/math/SE2Transform/implementation.md). - ### 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. diff --git a/doc/math/README.md b/doc/math/README.md index ca563f5..5579965 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -4,20 +4,21 @@ Core mathematical primitives for numerical computation. ## Algorithms -| Algorithm | Description | -|----------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------| -| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | -| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | -| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | -| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | -| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | -| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | -| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | -| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | -| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | -| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | -| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | -| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | -| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | -| [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization | -| [SE(3) Transform](SE3Transform.md) | Rigid transforms with twist/wrench adjoints and an accurate exp/log (linear-first $(v;\omega)$ ordering) | +| Algorithm | Description | +|----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | +| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | +| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | +| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | +| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | +| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | +| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | +| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | +| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | +| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | +| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | +| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | +| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | +| [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization | +| [SE(3) Transform](SE3Transform.md) | Rigid transforms with twist/wrench adjoints and an accurate exp/log (linear-first $(v;\omega)$ ordering) | +| [SE(2) Transform](SE2Transform.md) | Planar rigid transforms as unit complex rotations with twist/wrench adjoints, arc-exact exp/log and short-way pose error (linear-first $(v_x, v_y, \omega)$ ordering) | diff --git a/doc/math/SE2Transform.md b/doc/math/SE2Transform.md new file mode 100644 index 0000000..d816da8 --- /dev/null +++ b/doc/math/SE2Transform.md @@ -0,0 +1,117 @@ +# SE(2) Transform, Planar Twists and Wrenches + +## Overview & Motivation + +Ground robots, warehouse vehicles, mobile bases and planar mechanisms all work with poses in a plane. Odometry integrates wheel speeds into a pose, path trackers compare the vehicle pose with a reference, and landmark and scan-matching localization correct the pose from measurements. +Every one of them needs the same few operations: compose two poses, invert a pose, move a velocity or a force into another frame, and turn a constant velocity held for some time into the finite motion it produces. + +The planar special Euclidean group SE(2) provides exactly these. A planar pose costs four numbers instead of the twelve of a spatial transform. Composition and inversion need no trigonometry when the heading is stored as its cosine and sine. +Using the same linear-first ordering as the spatial transform keeps planar and spatial code consistent, for example when a mobile base carries an arm. + +## Mathematical Theory + +### Planar Transform + +A transform $T = (R(\theta), p)$ with $p \in \mathbb{R}^2$ maps coordinates in frame B to frame A, $x_A = R\,x_B + p$, where + +$$R(\theta) = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix}, \qquad T = \begin{pmatrix} R & p \\ 0 & 1 \end{pmatrix}.$$ + +The rotation is stored as the unit complex number $z = \cos\theta + i\sin\theta$. Composition multiplies the complex numbers and rotates the second offset, and the inverse conjugates: + +$$T_1 T_2 = (z_1 z_2,\; R_1 p_2 + p_1), \qquad T^{-1} = (\bar{z},\; -R^T p).$$ + +The heading is never stored. It is recovered as $\theta = \operatorname{atan2}(\sin, \cos) \in (-\pi, \pi]$, so it is always wrapped. + +### Ordering Convention (normative) + +All 3-vectors are ordered **linear part first**: planar twists $(v_x, v_y, \omega)$, planar wrenches $(f_x, f_y, \tau)$, poses $(x, y, \theta)$, logarithms and pose errors. This matches the spatial transform's $(v;\,\omega)$ ordering. + +### Adjoint, Twists and Wrenches + +A twist in frame B is re-expressed in frame A by + +$$V_A = \mathrm{Ad}_T V_B, \qquad \mathrm{Ad}_T = \begin{pmatrix} \cos\theta & -\sin\theta & p_y \\ \sin\theta & \cos\theta & -p_x \\ 0 & 0 & 1 \end{pmatrix}.$$ + +The rotation turns the linear velocity, and the offset converts the turn rate into a lever-arm velocity. The wrench map is the power-consistent dual: + +$$f_A = R f_B, \qquad \tau_A = \tau_B + p_x f_{A,y} - p_y f_{A,x}.$$ + +Power $W \cdot V$ is then the same in every frame, and the adjoint is a homomorphism, $\mathrm{Ad}_{T_1 T_2} = \mathrm{Ad}_{T_1}\mathrm{Ad}_{T_2}$. + +### Exponential + +A constant twist $\xi = (v_x, v_y, \omega)$ held for a time $t$ turns by $\varphi = \omega t$ and moves along a circular arc: + +$$\exp(\xi t) = \left(z(\varphi),\; V(\varphi)\,u\right), \qquad u = (v_x t, v_y t), \qquad V(\varphi) = \begin{pmatrix} A & -B \\ B & A \end{pmatrix},$$ + +with $A = \sin\varphi / \varphi$ and $B = (1 - \cos\varphi)/\varphi$. For $|\varphi| < 0.1$ both are replaced by their Taylor series, $A \approx 1 - \varphi^2/6 + \varphi^4/120$ and $B \approx \varphi/2 - \varphi^3/24 + \varphi^5/720$. This removes the $0/0$ at $\varphi = 0$ and the cancellation in $1 - \cos\varphi$. The truncation error at the switch is below single-precision resolution. +Away from it, $B$ uses the half-angle form $2\sin^2(\varphi/2)/\varphi$. + +### Logarithm + +The inverse of the exponential with $t = 1$ returns the twist that carries the identity to $T$: + +$$\log T = \left(V(\varphi)^{-1} p,\; \varphi\right), \qquad V(\varphi)^{-1} = \begin{pmatrix} \alpha & h \\ -h & \alpha \end{pmatrix}, \qquad h = \varphi/2, \quad \alpha = h\cot h.$$ + +On the branch $\varphi \in (-\pi, \pi]$, $\alpha$ is finite everywhere and reaches $0$ at $\pm\pi$, so the logarithm has no singular point. Near zero it uses $\alpha \approx 1 - \varphi^2/12 - \varphi^4/720$. Because the angle is wrapped, $\log(\exp(\xi t)) = \xi t$ only while $|\omega t| < \pi$. + +### Pose Error + +The base-frame pose error between a target and the current pose is + +$$e = \left(p_{\text{target}} - p_{\text{current}},\; \operatorname{atan2}(\sin(\theta_t - \theta_c),\, \cos(\theta_t - \theta_c))\right).$$ + +The heading term is computed from the complex product $z_t \bar{z}_c$, so it always takes the short way round. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------------------|--------|--------|-----------------------------------------------| +| Compose, invert, apply | $O(1)$ | $O(1)$ | At most 8 multiplications, no trigonometry | +| Adjoint, twist and wrench map | $O(1)$ | $O(1)$ | One rotation and one lever-arm term | +| Exponential | $O(1)$ | $O(1)$ | One sine/cosine pair plus one half-angle sine | +| Logarithm | $O(1)$ | $O(1)$ | One `atan2` and one sine/cosine pair | +| Renormalisation | $O(1)$ | $O(1)$ | One hypotenuse | + +## Step-by-Step Walkthrough + +1. **Apply**: the pose $(1, 2, \pi/2)$ maps the point $(1, 0)$ to $(1, 3)$, a quarter turn followed by the offset. Its inverse is $(-2, 1, -\pi/2)$. +2. **Compose**: $(1, 2, \pi/2)$ composed with $(-0.5, 0.3, -\pi/3)$ gives $(0.7, 1.5, \pi/6)$. The right operand applies first. +3. **Exponential of an arc**: the unicycle twist $(1, 0, 1)$ held for $\pi/2$ traces a quarter circle of radius 1 and ends at $(1, 1, \pi/2)$. With zero turn rate, $(0.3, -0.4, 0)$ for 2 s is a straight move to $(0.6, -0.8, 0)$. +4. **Logarithm**: the pose $(1, 2, \pi/2)$ has logarithm $(3\pi/4, \pi/4, \pi/2)$. At a half turn $(1, 0, \pi)$ the logarithm stays finite at $(0, -\pi/2, \pi)$. +5. **Pose error**: from heading $0.2$ to $0.2 + 190°$, the heading error is $-170°$ ($-2.967$ rad), not $+190°$. +6. **Power invariance**: the wrench $(1.5, -0.7, 0.3)$ and the twist $(0.4, -0.2, 0.7)$ have power $0.95$ in both frames. + +## Pitfalls & Edge Cases + +- **Drift of the unit rotation**: each composition in single precision lets $\cos^2 + \sin^2$ drift by about one unit in the last place. A pose integrated over many steps, such as odometry, should be renormalised every step. Single compositions need no correction. +- **Wrapped logarithm**: the logarithm returns the wrapped angle. Integrators that must count full turns should integrate the exponential directly instead of round-tripping through the logarithm. +- **Half turn**: at a heading of exactly $\pi$ the branch returns $+\pi$. A sine that rounds to a tiny negative value instead yields an angle just above $-\pi$, which describes the same rotation. +- **Ordering**: twists, wrenches, logarithms and pose errors are linear first. Formulas from sources that put the angular component first must be permuted. +- **Constructed transforms**: a transform built directly from a cosine and sine must be given a unit pair. Construction from a heading always is. + +## Variants & Generalizations + +- **Lie-group Jacobians**: left and right Jacobians of SE(2), and the $\oplus$/$\ominus$ operators for on-manifold estimation. +- **Error-state filters**: Kalman filters that estimate a small pose perturbation in the tangent space. +- **SE(3)**: the spatial counterpart with the same conventions. A planar pose lifts to a rotation about the vertical axis. +- **Similarity group Sim(2)**: adds a scale factor for monocular mapping and image registration. + +## Applications + +- Wheel odometry with exact arc integration. +- Path tracking controllers that need a pose error in the vehicle frame. +- Landmark and scan-matching localization on planar maps. +- Planar mechanisms and the base pose of mobile manipulators. + +## Connections to Other Algorithms + +- **SE(3) Transform**: the spatial version. The planar transform lifts into it as a rotation about the vertical axis, with zero height. +- **Angle Utilities**: the shortest signed angle difference is the scalar analogue of the short-way heading error. +- **Matrix Exponential**: the general matrix exponential of the $3\times3$ twist matrix is the closed form's reference. + +## References & Further Reading + +- J. Solà, J. Deray, and D. Atchuthan, "A micro Lie theory for state estimation in robotics," arXiv:1812.01537, 2018 — Appendix C. +- K. M. Lynch and F. C. Park, *Modern Robotics: Mechanics, Planning, and Control*, Cambridge University Press, 2017 — Chapters 3 and 13. +- T. D. Barfoot, *State Estimation for Robotics*, Cambridge University Press, 2017 — Chapter 7. diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index a48155f..ac941b7 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -34,6 +34,7 @@ target_sources(numerical.math PRIVATE QNumber.hpp Quaternion.hpp RecursiveBuffer.hpp + SE2Transform.hpp SE3Transform.hpp Statistics.hpp StepResponseMetrics.hpp @@ -57,6 +58,7 @@ numerical_add_coverage_sources(numerical.math QNumber.cpp Quaternion.cpp RecursiveBuffer.cpp + SE2Transform.cpp SE3Transform.cpp Toeplitz.cpp TridiagonalSolver.cpp diff --git a/numerical/math/SE2Transform.cpp b/numerical/math/SE2Transform.cpp new file mode 100644 index 0000000..6f141e7 --- /dev/null +++ b/numerical/math/SE2Transform.cpp @@ -0,0 +1,6 @@ +#include "numerical/math/SE2Transform.hpp" + +namespace math +{ + template struct SE2Transform; +} diff --git a/numerical/math/SE2Transform.hpp b/numerical/math/SE2Transform.hpp new file mode 100644 index 0000000..066a540 --- /dev/null +++ b/numerical/math/SE2Transform.hpp @@ -0,0 +1,187 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include + +namespace math +{ + template + using Vector2 = Vector; + + template + using Matrix2 = SquareMatrix; + + template + struct SE2Transform + { + static_assert(std::is_floating_point_v, "SE2Transform supports floating-point types only"); + + T cosine{ 1 }; + T sine{ 0 }; + Vector2 translation{}; + + static SE2Transform Identity(); + static SE2Transform FromPose(T x, T y, T theta); + + Vector3 Pose() const; + T Heading() const; + Matrix2 Rotation() const; + + [[nodiscard]] OPTIMIZE_FOR_SPEED friend SE2Transform operator*(const SE2Transform& lhs, const SE2Transform& rhs) + { + return { lhs.cosine * rhs.cosine - lhs.sine * rhs.sine, lhs.sine * rhs.cosine + lhs.cosine * rhs.sine, lhs.Apply(rhs.translation) }; + } + + SE2Transform Inverse() const; + OPTIMIZE_FOR_SPEED Vector2 Apply(const Vector2& point) const; + Vector2 Rotate(const Vector2& direction) const; + SquareMatrix Homogeneous() const; + Matrix3 Adjoint() const; + Vector3 TransformTwist(const Vector3& twist) const; + Vector3 TransformWrench(const Vector3& wrench) const; + SE2Transform Normalized() const; + SE3Transform ToSE3() const; + + OPTIMIZE_FOR_SPEED static SE2Transform Exp(const Vector3& twist, T theta); + Vector3 Log() const; + static Vector3 PoseError(const SE2Transform& target, const SE2Transform& current); + + private: + static constexpr T seriesThreshold{ T{ 0.1f } }; + }; + + template + SE2Transform SE2Transform::Identity() + { + return {}; + } + + template + SE2Transform SE2Transform::FromPose(T x, T y, T theta) + { + return { math::Cos(theta), math::Sin(theta), Vector2{ x, y } }; + } + + template + Vector3 SE2Transform::Pose() const + { + return Vector3{ translation[0], translation[1], Heading() }; + } + + template + T SE2Transform::Heading() const + { + return math::Atan2(sine, cosine); + } + + template + Matrix2 SE2Transform::Rotation() const + { + return Matrix2{ { cosine, -sine }, { sine, cosine } }; + } + + template + SE2Transform SE2Transform::Inverse() const + { + return { cosine, -sine, Vector2{ -(cosine * translation[0] + sine * translation[1]), sine * translation[0] - cosine * translation[1] } }; + } + + template + OPTIMIZE_FOR_SPEED Vector2 SE2Transform::Apply(const Vector2& point) const + { + return Vector2{ cosine * point[0] - sine * point[1] + translation[0], sine * point[0] + cosine * point[1] + translation[1] }; + } + + template + Vector2 SE2Transform::Rotate(const Vector2& direction) const + { + return Vector2{ cosine * direction[0] - sine * direction[1], sine * direction[0] + cosine * direction[1] }; + } + + template + SquareMatrix SE2Transform::Homogeneous() const + { + return SquareMatrix{ { cosine, -sine, translation[0] }, { sine, cosine, translation[1] }, { T{}, T{}, T{ 1 } } }; + } + + template + Matrix3 SE2Transform::Adjoint() const + { + return Matrix3{ { cosine, -sine, translation[1] }, { sine, cosine, -translation[0] }, { T{}, T{}, T{ 1 } } }; + } + + template + Vector3 SE2Transform::TransformTwist(const Vector3& twist) const + { + return Adjoint() * twist; + } + + template + Vector3 SE2Transform::TransformWrench(const Vector3& wrench) const + { + const Vector2 force{ Rotate(Vector2{ wrench[0], wrench[1] }) }; + return Vector3{ force[0], force[1], wrench[2] + translation[0] * force[1] - translation[1] * force[0] }; + } + + template + SE2Transform SE2Transform::Normalized() const + { + const T scale{ T{ 1 } / math::Hypot(cosine, sine) }; + return { cosine * scale, sine * scale, translation }; + } + + template + SE3Transform SE2Transform::ToSE3() const + { + return SE3Transform{ Matrix3{ { cosine, -sine, T{} }, { sine, cosine, T{} }, { T{}, T{}, T{ 1 } } }, Vector3{ translation[0], translation[1], T{} } }; + } + + template + OPTIMIZE_FOR_SPEED SE2Transform SE2Transform::Exp(const Vector3& twist, T theta) + { + const T phi{ twist[2] * theta }; + const T phi2{ phi * phi }; + + T sinc{ T{ 1 } - phi2 / T{ 6 } + phi2 * phi2 / T{ 120 } }; + T versinc{ phi / T{ 2 } - phi * phi2 / T{ 24 } + phi * phi2 * phi2 / T{ 720 } }; + if (math::Abs(phi) >= seriesThreshold) + { + const T halfSin{ math::Sin(phi / T{ 2 }) }; + sinc = math::Sin(phi) / phi; + versinc = T{ 2 } * halfSin * halfSin / phi; + } + + const T ux{ twist[0] * theta }; + const T uy{ twist[1] * theta }; + return { math::Cos(phi), math::Sin(phi), Vector2{ sinc * ux - versinc * uy, versinc * ux + sinc * uy } }; + } + + template + Vector3 SE2Transform::Log() const + { + const T phi{ Heading() }; + const T half{ phi / T{ 2 } }; + const T phi2{ phi * phi }; + + T halfCot{ T{ 1 } - phi2 / T{ 12 } - phi2 * phi2 / T{ 720 } }; + if (math::Abs(phi) >= seriesThreshold) + halfCot = half * math::Cos(half) / math::Sin(half); + + return Vector3{ halfCot * translation[0] + half * translation[1], -half * translation[0] + halfCot * translation[1], phi }; + } + + template + Vector3 SE2Transform::PoseError(const SE2Transform& target, const SE2Transform& current) + { + const T heading{ math::Atan2(target.sine * current.cosine - target.cosine * current.sine, target.cosine * current.cosine + target.sine * current.sine) }; + return Vector3{ target.translation[0] - current.translation[0], target.translation[1] - current.translation[1], heading }; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template struct SE2Transform; +#endif +} diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index e100ccf..bb406c0 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -30,6 +30,7 @@ target_sources(numerical.math_test PRIVATE TestMatrix.cpp TestQuaternion.cpp TestRecursiveBuffer.cpp + TestSE2Transform.cpp TestSE3Transform.cpp TestStatistics.cpp TestStepResponseMetrics.cpp diff --git a/numerical/math/test/TestSE2Transform.cpp b/numerical/math/test/TestSE2Transform.cpp new file mode 100644 index 0000000..50a2fab --- /dev/null +++ b/numerical/math/test/TestSE2Transform.cpp @@ -0,0 +1,160 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Vector2 = math::Vector2; + using Vector3 = math::Vector3; + + class TestSE2Transform : public ::testing::Test + { + protected: + static constexpr float pi{ std::numbers::pi_v }; + static constexpr float tolerance{ 1e-5f }; + const Transform a{ Transform::FromPose(1.0f, 2.0f, pi / 2.0f) }; + const Transform b{ Transform::FromPose(-0.5f, 0.3f, -pi / 3.0f) }; + const Vector3 twist{ 0.4f, -0.2f, 0.7f }; + + template + static void ExpectNear(const math::Matrix& actual, const math::Matrix& expected, float eps = tolerance) + { + for (std::size_t r = 0; r < Rows; ++r) + for (std::size_t c = 0; c < Cols; ++c) + EXPECT_NEAR(actual.at(r, c), expected.at(r, c), eps) << "(" << r << ", " << c << ")"; + } + + static void ExpectNear(const Transform& actual, const Transform& expected, float eps = tolerance) + { + EXPECT_NEAR(actual.cosine, expected.cosine, eps); + EXPECT_NEAR(actual.sine, expected.sine, eps); + ExpectNear(actual.translation, expected.translation, eps); + } + + static float Dot(const Vector3& lhs, const Vector3& rhs) + { + return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2]; + } + + static Vector2 ClosedFormTranslation(float phi, const Vector2& u) + { + const float sinc{ std::sin(phi) / phi }; + const float versinc{ (1.0f - std::cos(phi)) / phi }; + return Vector2{ sinc * u[0] - versinc * u[1], versinc * u[0] + sinc * u[1] }; + } + }; +} + +TEST_F(TestSE2Transform, apply_rotates_then_translates) +{ + ExpectNear(a.Apply(Vector2{ 1.0f, 0.0f }), Vector2{ 1.0f, 3.0f }); +} + +TEST_F(TestSE2Transform, composition_applies_right_operand_first) +{ + const Vector2 x{ 0.3f, -0.2f }; + + ExpectNear((a * b).Apply(x), a.Apply(b.Apply(x))); + ExpectNear((a * b).Pose(), Vector3{ 0.7f, 1.5f, pi / 6.0f }); +} + +TEST_F(TestSE2Transform, inverse_composes_to_identity) +{ + ExpectNear(a.Inverse().Pose(), Vector3{ -2.0f, 1.0f, -pi / 2.0f }); + ExpectNear(a * a.Inverse(), Transform::Identity()); + ExpectNear(a.Inverse() * a, Transform::Identity()); +} + +TEST_F(TestSE2Transform, pose_returns_wrapped_heading) +{ + EXPECT_NEAR(Transform::FromPose(0.0f, 0.0f, 3.0f * pi / 2.0f).Heading(), -pi / 2.0f, tolerance); + ExpectNear(b.Pose(), Vector3{ -0.5f, 0.3f, -pi / 3.0f }); +} + +TEST_F(TestSE2Transform, homogeneous_matches_block_form) +{ + ExpectNear(a.Homogeneous(), math::SquareMatrix{ { 0.0f, -1.0f, 1.0f }, { 1.0f, 0.0f, 2.0f }, { 0.0f, 0.0f, 1.0f } }); + ExpectNear(a.Rotation(), math::Matrix2{ { 0.0f, -1.0f }, { 1.0f, 0.0f } }); +} + +TEST_F(TestSE2Transform, adjoint_maps_twists_consistently_with_composition) +{ + for (const float t : { 0.4f, 2.0f }) + ExpectNear(Transform::Exp(a.TransformTwist(twist), t), a * Transform::Exp(twist, t) * a.Inverse()); +} + +TEST_F(TestSE2Transform, adjoint_of_product_is_product_of_adjoints) +{ + ExpectNear((a * b).Adjoint(), math::Matrix3{ a.Adjoint() * b.Adjoint() }); +} + +TEST_F(TestSE2Transform, wrench_and_twist_power_is_frame_invariant) +{ + const Vector3 wrench{ 1.5f, -0.7f, 0.3f }; + + EXPECT_NEAR(Dot(a.TransformWrench(wrench), a.TransformTwist(twist)), Dot(wrench, twist), tolerance); + EXPECT_NEAR(Dot(wrench, twist), 0.95f, tolerance); +} + +TEST_F(TestSE2Transform, exp_of_unicycle_arc) +{ + ExpectNear(Transform::Exp(Vector3{ 1.0f, 0.0f, 1.0f }, pi / 2.0f).Pose(), Vector3{ 1.0f, 1.0f, pi / 2.0f }); +} + +TEST_F(TestSE2Transform, exp_of_pure_translation) +{ + ExpectNear(Transform::Exp(Vector3{ 0.3f, -0.4f, 0.0f }, 2.0f).Pose(), Vector3{ 0.6f, -0.8f, 0.0f }); +} + +TEST_F(TestSE2Transform, exp_is_continuous_across_series_threshold) +{ + const Vector2 linear{ 0.8f, 0.3f }; + + for (const float phi : { 0.1f - 1e-4f, 0.1f + 1e-4f }) + ExpectNear(Transform::Exp(Vector3{ linear[0], linear[1], phi }, 1.0f).translation, ClosedFormTranslation(phi, linear), 1e-6f); +} + +TEST_F(TestSE2Transform, log_inverts_exp) +{ + ExpectNear(a.Log(), Vector3{ 3.0f * pi / 4.0f, pi / 4.0f, pi / 2.0f }); + for (const auto& transform : { a, b, Transform::FromPose(0.2f, -0.4f, 179.9f * pi / 180.0f) }) + ExpectNear(Transform::Exp(transform.Log(), 1.0f), transform); +} + +TEST_F(TestSE2Transform, log_at_half_turn_is_finite) +{ + const Transform halfTurn{ -1.0f, 0.0f, Vector2{ 1.0f, 0.0f } }; + + ExpectNear(halfTurn.Log(), Vector3{ 0.0f, -pi / 2.0f, pi }); +} + +TEST_F(TestSE2Transform, pose_error_takes_the_short_way) +{ + const Transform current{ Transform::FromPose(1.0f, 1.0f, 0.2f) }; + const Transform target{ Transform::FromPose(2.0f, 0.5f, 0.2f + 190.0f * pi / 180.0f) }; + + ExpectNear(Transform::PoseError(target, current), Vector3{ 1.0f, -0.5f, -2.967060f }); +} + +TEST_F(TestSE2Transform, normalized_restores_unit_rotation) +{ + const Transform scaled{ 1.001f * std::cos(0.3f), 1.001f * std::sin(0.3f), Vector2{ 1.0f, 2.0f } }; + + const auto normalized{ scaled.Normalized() }; + + EXPECT_NEAR(normalized.cosine * normalized.cosine + normalized.sine * normalized.sine, 1.0f, tolerance); + EXPECT_NEAR(normalized.Heading(), 0.3f, tolerance); + ExpectNear(normalized.translation, Vector2{ 1.0f, 2.0f }); +} + +TEST_F(TestSE2Transform, to_se3_rotates_about_z) +{ + const auto lifted{ a.ToSE3() }; + + ExpectNear(lifted.rotation, math::RotationAboutAxis(Vector3{ 0.0f, 0.0f, 1.0f }, pi / 2.0f)); + ExpectNear(lifted.translation, Vector3{ 1.0f, 2.0f, 0.0f }); +} diff --git a/roadmap/README.md b/roadmap/README.md index 3a36914..1a97c3f 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -101,7 +101,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `LmsAdaptiveFilter` (21) ### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE2Transform` (66) +`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) ### `control_analysis` `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) diff --git a/roadmap/math/SE2Transform/explanation.md b/roadmap/math/SE2Transform/explanation.md deleted file mode 100644 index f16bc81..0000000 --- a/roadmap/math/SE2Transform/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# SE(2) Transform, Planar Twists and Wrenches — Overview - -## What it is -The algebra of motion in a plane: a pose is a heading plus a 2-D position; a *planar twist* is a -body's instantaneous velocity (forward, sideways, turn rate); a *planar wrench* is a force in the plane -together with a moment about the vertical axis. The adjoint moves twists and wrenches between frames, and -the exponential and logarithm convert between a constant twist held for some time and the finite motion -it produces. - -## Why it matters (embedded) -Ground robots, mobile bases, warehouse vehicles and planar mechanisms work with poses in the plane. -Odometry, path tracking, landmark and scan-matching localization all need the same few operations. -SE(2) costs a fraction of SE(3): four numbers per pose and no trigonometry in composition. Writing it -once, with item 53's ordering, keeps planar and spatial code consistent when a mobile base carries an arm. - -## How it works (intuition) -The rotation is stored as the cosine and sine of the heading, so composing two poses multiplies two unit -complex numbers and rotates one offset — no angle arithmetic and no wrap-around bugs. A constant twist -held for a time moves the body along a circular arc (a straight line when the turn rate is zero). The -exponential computes the end of that arc exactly, which is why it is the right way to integrate wheel -odometry. The logarithm runs the other way: it returns the constant twist that would carry one pose to -another in unit time, and so gives a pose difference that respects the vehicle's geometry. - -## Key parameters -- **Ordering convention** — linear part first for twists `(vx, vy, ω)` and wrenches `(fx, fy, τ)`. -- **Heading branch** — headings and `Log` angles are reported in `(−π, π]`. -- **Unit rotation** — `(cos, sin)` kept unit length; renormalised when a pose is integrated over time. - -## Reference -J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics," -arXiv:1812.01537 (2018), Appendix C; K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 3 and 13. - -## See also -`SE3Transform` (item 53, the spatial version and `ToSE3`'s target), `AngleUtilities` (item 54, heading -wrap and differences); robotics-toolbox-cpp's wheeled-robot models and planar odometry. diff --git a/roadmap/math/SE2Transform/implementation.md b/roadmap/math/SE2Transform/implementation.md deleted file mode 100644 index 53a4ab7..0000000 --- a/roadmap/math/SE2Transform/implementation.md +++ /dev/null @@ -1,131 +0,0 @@ -# SE(2) Transform, Planar Twists and Wrenches — Implementation Pseudocode - -> Roadmap ref: #66 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) - -Planar rigid transform with the twist/wrench algebra of item 53, reduced to the plane: the pose type of -ground vehicles and planar mechanisms (odometry, path tracking, scan matching). It follows item 53's -convention: 3-vectors are ordered **linear part first**, twists `(vx, vy, ω)` and wrenches `(fx, fy, τ)`. -The rotation is stored as a unit complex number `(cos θ, sin θ)`, as Sophus and manif do. Composition, -`Apply` and `Inverse` then need no trigonometry; only the conversion to and from a heading does. - -## Data structures - -```cpp -template using Vector2 = math::Vector -template using Matrix2 = math::SquareMatrix -# planar twist (vx, vy, ω), planar wrench (fx, fy, τ) and the pose triple (x, y, θ) are math::Vector3 - -template # static_assert(std::is_floating_point_v); instantiated for float -struct SE2Transform: - T cosine{ 1 } # unit complex rotation (cos θ, sin θ) - T sine{ 0 } - Vector2 translation{} - # equivalently the 3×3 homogeneous matrix [[R(θ), p],[0 0 1]] -``` - -## Interface - -```text -static SE2Transform Identity() -static SE2Transform FromPose(T x, T y, T theta) # the only trig on construction -Vector3 Pose() const # (x, y, θ), θ = atan2(sin, cos) ∈ (−π, π] -T Heading() const -Matrix2 Rotation() const -friend SE2Transform operator*(const SE2Transform& lhs, const SE2Transform& rhs) # hot path -SE2Transform Inverse() const # closed form -Vector2 Apply(const Vector2& point) const # R·x + p; hot path -Vector2 Rotate(const Vector2& direction) const # R·x -SquareMatrix Homogeneous() const -Matrix3 Adjoint() const # twist map for (vx, vy, ω) -Vector3 TransformTwist(const Vector3& twist) const # Adjoint()·twist -Vector3 TransformWrench(const Vector3& wrench) const # dual map, see below -SE2Transform Normalized() const # rescale (cos, sin) to unit length -SE3Transform ToSE3() const # rotation about z, translation (x, y, 0) - -static SE2Transform Exp(const Vector3& twist, T theta) # e^{[ξ]θ}; θ is the elapsed time for a velocity twist; hot path -Vector3 Log() const # inverse of Exp with theta = 1 -static Vector3 PoseError(const SE2Transform& target, const SE2Transform& current) -``` - -`operator*` is a hidden friend, as in item 53. The heading is never stored, so `Pose` always returns it -wrapped. Only `FromPose` checks for a unit rotation: an aggregate-initialised transform must be given a -unit `(cosine, sine)`. - -## Algorithm (pseudocode) - -```text -function FromPose(x, y, θ): return { cos θ, sin θ, (x, y) } -function Pose(): return (p.x, p.y, atan2(sine, cosine)) -function Rotation(): return [[c, −s], [s, c]] - -function operator*(a, b): # complex product + rotated offset - return { a.c·b.c − a.s·b.s, a.s·b.c + a.c·b.s, a.R·b.p + a.p } -function Inverse(): return { c, −s, −Rᵀ·p } - -function Adjoint(): # V_A = Ad_T · V_B for V = (vx, vy, ω) - return [[ c, −s, p.y ], - [ s, c, −p.x ], - [ 0, 0, 1 ]] - -function TransformWrench(w = (fx, fy, τ)): # power-consistent dual of Adjoint - f_A = R·(fx, fy) - return (f_A; τ + p.x·f_A.y − p.y·f_A.x) - -function Normalized(): - k = 1 / Hypot(c, s); return { c·k, s·k, p } - -function Exp(ξ = (vx, vy, ω), θ): # OPTIMIZE_FOR_SPEED - φ = ω·θ - if |φ| < 0.1: # series, no 0/0 and no cancellation - A = 1 − φ²/6 + φ⁴/120 # sin φ / φ - B = φ/2 − φ³/24 + φ⁵/720 # (1 − cos φ) / φ - else: - A = sin φ / φ - B = 2·sin²(φ/2) / φ # half-angle form of (1 − cos φ)/φ - (ux, uy) = (vx·θ, vy·θ) - return { cos φ, sin φ, (A·ux − B·uy, B·ux + A·uy) } # p = V(φ)·u, V = [[A, −B], [B, A]] - -function Log(): # returns (vx, vy, ω) with ω = Heading() ∈ (−π, π] - φ = atan2(s, c); h = φ/2 - if |φ| < 0.1: α = 1 − φ²/12 − φ⁴/720 # h·cot h - else: α = h·cos h / sin h # finite on the whole branch: α(±π) = 0 - return (α·p.x + h·p.y, −h·p.x + α·p.y, φ) # V(φ)⁻¹·p - -function PoseError(target, current): # base-frame error, like item 53's PoseError - eθ = atan2(target.s·current.c − target.c·current.s, - target.c·current.c + target.s·current.s) # heading of target·currentᵀ, short way - return (target.p − current.p; eθ) -``` - -## Complexity & memory - -- Compose / inverse / apply / rotate / adjoint: `O(1)`, at most 8 multiplies, no trigonometry. -- `Exp`: one `sin`/`cos` pair plus one `sin` (half angle); `Log`: one `atan2` and one `sin`/`cos` pair. -- Memory: 4 scalars per transform. No heap. - -## Numerical / embedded notes - -- **Convention (normative):** twists `(vx, vy, ω)`, wrenches `(fx, fy, τ)`, `PoseError` and `Log` return - linear part first. robotics-toolbox-cpp's wheeled models, odometry and trackers depend on this ordering. -- `Log` has no singular point: on the branch `φ ∈ (−π, π]` the coefficient `(φ/2)·cot(φ/2)` stays finite - and reaches 0 at `±π`. It does return the **wrapped** angle, so `Log(Exp(ξ, θ)) = ξ·θ` only while - `|ω·θ| < π`. Odometry that must count full turns integrates `Exp` and does not round-trip through `Log`. -- The series threshold `|φ| < 0.1` mirrors item 53. The truncation error there is `≈1.6e-10`, below - `float` resolution, so `Exp` is continuous across the switch. -- Composing in `float` lets `cos² + sin²` drift from 1 by roughly one ulp per product. A transform that is - integrated over time (odometry) calls `Normalized` every step, which costs one `Hypot`. Single compositions - need no renormalisation. -- `FromPose` accepts any angle; no wrapping is needed because only `cos θ` and `sin θ` are stored. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/math/SE2Transform.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `operator*`/`Apply`/`Exp`, and `extern template struct SE2Transform;` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. Includes `SE3Transform.hpp` for `ToSE3` and `Vector3`. -- Coverage: `numerical/math/SE2Transform.cpp` → `template struct SE2Transform;` -- Test: `numerical/math/test/TestSE2Transform.cpp` -- Doc: `doc/math/SE2Transform.md` (per `doc/TEMPLATE.md`) + row in `doc/math/README.md` -- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.math ...)`; `TestSE2Transform.cpp` → `numerical.math_test`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/SE2Transform/tests.md b/roadmap/math/SE2Transform/tests.md deleted file mode 100644 index 027d2da..0000000 --- a/roadmap/math/SE2Transform/tests.md +++ /dev/null @@ -1,69 +0,0 @@ -# SE(2) Transform — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSE2Transform : public ::testing::Test: - SE2Transform a{ FromPose(1, 2, π/2) } - SE2Transform b{ FromPose(−0.5, 0.3, −π/3) } - Vector3 twist{ 0.4, −0.2, 0.7 } - tolerance 1e-5 -# each case below is a TEST_F(TestSE2Transform, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -apply_rotates_then_translates: - Assert: a.Apply((1, 0)) ≈ (1, 3) -composition_applies_right_operand_first: - Assert: (a * b).Apply(x) ≈ a.Apply(b.Apply(x)) for x = (0.3, −0.2); (a * b).Pose() ≈ (0.7, 1.5, π/6) -inverse_composes_to_identity: - Assert: a.Inverse().Pose() ≈ (−2, 1, −π/2); a * a.Inverse() ≈ Identity and a.Inverse() * a ≈ Identity -pose_returns_wrapped_heading: - Assert: FromPose(0, 0, 3π/2).Heading() ≈ −π/2; b.Pose() ≈ (−0.5, 0.3, −π/3) -homogeneous_matches_block_form: - Assert: a.Homogeneous() == [[0, −1, 1], [1, 0, 2], [0, 0, 1]] -adjoint_maps_twists_consistently_with_composition: - Assert: Exp(a.TransformTwist(twist), t) ≈ a * Exp(twist, t) * a.Inverse() for t = 0.4 and t = 2 -adjoint_of_product_is_product_of_adjoints: - Assert: (a * b).Adjoint() ≈ a.Adjoint() · b.Adjoint() -wrench_and_twist_power_is_frame_invariant: - Arrange: wrench W = (1.5, −0.7, 0.3) - Assert: a.TransformWrench(W) · a.TransformTwist(twist) ≈ W · twist = 0.95 -exp_of_unicycle_arc: - Assert: Exp((1, 0, 1), π/2).Pose() ≈ (1, 1, π/2) # quarter circle of radius 1 -exp_of_pure_translation: - Assert: Exp((0.3, −0.4, 0), 2).Pose() ≈ (0.6, −0.8, 0) -exp_is_continuous_across_series_threshold: - Arrange: ξ = (0.8, 0.3, φ), θ = 1, φ ∈ {0.1 − 1e-4, 0.1 + 1e-4} - Assert: both translations agree with the closed form V(φ)·(0.8, 0.3) to 1e-6 -log_inverts_exp: - Assert: a.Log() ≈ (3π/4, π/4, π/2); Exp(T.Log(), 1) ≈ T for T = a, b, FromPose(0.2, −0.4, 179.9°) -log_at_half_turn_is_finite: - Assert: FromPose(1, 0, π).Log() ≈ (0, −π/2, π) -pose_error_takes_the_short_way: - Arrange: current = FromPose(1, 1, 0.2), target = FromPose(2, 0.5, 0.2 + 190°) - Assert: PoseError(target, current) ≈ (1, −0.5, −170°) # −2.967060 rad -normalized_restores_unit_rotation: - Arrange: T = { 1.001·cos 0.3, 1.001·sin 0.3, (1, 2) } - Assert: T.Normalized() has cos² + sin² ≈ 1, heading 0.3, translation (1, 2) -to_se3_rotates_about_z: - Assert: a.ToSE3() ≈ { RotationAboutAxis(ẑ, π/2), (1, 2, 0) } -``` - -## Reference vectors - -- `FromPose(1, 2, π/2)` applied to `(1, 0)` ⇒ `(1, 3)`; its inverse is `(−2, 1, −π/2)`. -- `a * b` ⇒ `(0.7, 1.5, π/6)`. -- `a.Log()` ⇒ `(3π/4, π/4, π/2) = (2.356194, 0.785398, 1.570796)`; `b.Log()` ⇒ `(−0.610529, 0.010271, −π/3)`. -- `Exp` agrees with a double-precision 3×3 matrix exponential of `[[0, −ω, vx], [ω, 0, vy], [0, 0, 0]]·θ` - to `8e-10` over 2000 random twists; `Exp(Log(T))` round-trips to `6e-10`. - -## Edge cases - -- `ω = 0` (pure translation) and `|ω·θ| > π` in `Exp` (the result is still exact; only `Log` wraps). -- `Log` at heading exactly `π`: the branch returns `+π`, never `−π`. -- 10⁵ compositions of a small step with `Normalized` after each keep `cos² + sin² = 1` to `1e-6`.