docs(roadmap): add the SE(2) transform spec (item 66)

ROADMAP.md

@@ -37,7 +37,8 @@ Difficulty legend:
 
 Items 66–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
 NT66–NT73; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization
-families, also have non-robotics consumers, and have no spec yet. All are *(float-first)*.
+families and also have non-robotics consumers. Item 66 has a spec under [`roadmap/`](roadmap/README.md);
+items 67–73 have none yet. All are *(float-first)*.
 
 Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family
 unit-test reference [`TESTING.md`](TESTING.md) depends on but which
@@ -53,7 +54,8 @@ the library does not yet expose. Detailed below under
   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 54 (angle wrapping).
+- **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

roadmap/README.md

@@ -101,7 +101,7 @@ with `EXPECT_NEAR` and `math::Tolerance<float>()` (or an explicit tolerance).
 `LmsAdaptiveFilter` (21)
 
 ### `math`
-`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30)
+`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE2Transform` (66)
 
 ### `control_analysis`
 `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32)

roadmap/math/SE2Transform/explanation.md

@@ -0,0 +1,35 @@
+# 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.

roadmap/math/SE2Transform/implementation.md

@@ -0,0 +1,131 @@
+# 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<typename T> using Vector2 = math::Vector<T, 2>
+template<typename T> using Matrix2 = math::SquareMatrix<T, 2>
+# planar twist (vx, vy, ω), planar wrench (fx, fy, τ) and the pose triple (x, y, θ) are math::Vector3<T>
+
+template<typename T>                  # static_assert(std::is_floating_point_v<T>); instantiated for float
+struct SE2Transform:
+    T cosine{ 1 }                     # unit complex rotation (cos θ, sin θ)
+    T sine{ 0 }
+    Vector2<T> 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<T>   Pose() const                                    # (x, y, θ), θ = atan2(sin, cos) ∈ (−π, π]
+T            Heading() const
+Matrix2<T>   Rotation() const
+friend SE2Transform operator*(const SE2Transform& lhs, const SE2Transform& rhs)   # hot path
+SE2Transform Inverse() const                                 # closed form
+Vector2<T>   Apply(const Vector2<T>& point) const            # R·x + p; hot path
+Vector2<T>   Rotate(const Vector2<T>& direction) const       # R·x
+SquareMatrix<T, 3> Homogeneous() const
+Matrix3<T>   Adjoint() const                                 # twist map for (vx, vy, ω)
+Vector3<T>   TransformTwist(const Vector3<T>& twist) const   # Adjoint()·twist
+Vector3<T>   TransformWrench(const Vector3<T>& wrench) const # dual map, see below
+SE2Transform Normalized() const                              # rescale (cos, sin) to unit length
+SE3Transform<T> ToSE3() const                                # rotation about z, translation (x, y, 0)
+
+static SE2Transform Exp(const Vector3<T>& twist, T theta)    # e^{[ξ]θ}; θ is the elapsed time for a velocity twist; hot path
+Vector3<T>          Log() const                              # inverse of Exp with theta = 1
+static Vector3<T>   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<T>)`.
+
+## Deployment
+
+- Header: `numerical/math/SE2Transform.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on
+  `operator*`/`Apply`/`Exp`, and `extern template struct SE2Transform<float>;` under
+  `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. Includes `SE3Transform.hpp` for `ToSE3` and `Vector3`.
+- Coverage: `numerical/math/SE2Transform.cpp` → `template struct SE2Transform<float>;`
+- 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`.

roadmap/math/SE2Transform/tests.md

@@ -0,0 +1,69 @@
+# SE(2) Transform — Unit Test Plan (Pseudocode)
+
+> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap.
+
+## Fixture
+
+```cpp
+class TestSE2Transform : public ::testing::Test:
+    SE2Transform<float> a{ FromPose(1, 2, π/2) }
+    SE2Transform<float> b{ FromPose(−0.5, 0.3, −π/3) }
+    Vector3<float> twist{ 0.4, −0.2, 0.7 }
+    tolerance 1e-5
+# each case below is a TEST_F(TestSE2Transform, <name>)
+```
+
+## 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`.