feat(solvers): support wide matrices in SVD; add roadmap items 53–65

ROADMAP.md

@@ -24,14 +24,162 @@ Difficulty legend:
 
 ## Master list (by priority)
 
-| #  | Component                                            | Target module             | Difficulty |
-|----|------------------------------------------------------|---------------------------|------------|
+| #  | Component                                            | Target module           | Difficulty |
+|----|------------------------------------------------------|-------------------------|------------|
+| 54 | Angle utilities (wrap, shortest difference, unwrap)  | `math`                  | ★☆☆☆☆      |
+| 55 | Polynomial (cubic/quintic) point-to-point profile    | `motion_profiles` (new) | ★☆☆☆☆      |
+| 56 | Trapezoidal (LSPB) profile + duration sync           | `motion_profiles` (new) | ★☆☆☆☆      |
+| 57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers`           | ★☆☆☆☆      |
+| 58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers`               | ★☆☆☆☆      |
+| 59 | Tridiagonal (Thomas) solver                          | `math`                  | ★☆☆☆☆      |
+| 53 | SE(3) transform, twists, wrenches, adjoint, exp/log  | `math`                  | ★★☆☆☆      |
+| 60 | S-curve (jerk-limited) profile                       | `motion_profiles` (new) | ★★☆☆☆      |
+| 61 | Cubic spline interpolation (vector-valued)           | `math`                  | ★★☆☆☆      |
+| 62 | Damped pseudo-inverse + null-space projector         | `solvers`               | ★★☆☆☆      |
+| 63 | Incremental QR least squares (rank-revealing)        | `estimators/offline`    | ★★★☆☆      |
+| 64 | Levenberg–Marquardt (retraction, pluggable damping)  | `optimization`          | ★★★☆☆      |
+| 65 | Dense convex QP (Goldfarb–Idnani dual active set)    | `solvers`               | ★★★★☆      |
+
+Items 53–65 are the generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
+NT53–NT65); each also serves non-robotics consumers (motor control, navigation, calibration). 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
 the library does not yet expose. Detailed below under
 [Evaluation & metrics primitives](#evaluation--metrics-primitives).
 
+## Tier 1 — Trivial primitives ★☆☆☆☆
+
+### 54. Angle utilities
+- **What:** `WrapToPi` `(−π, π]`, `WrapToTwoPi` `[0, 2π)`, shortest signed `AngleDifference`, and
+  `UnwrapAngle` for continuous signals.
+- **Embedded value:** One convention for every angle consumer — heading controllers, odometry, encoder
+  unwrapping, analytical IK; replaces `ComplementaryFilter`'s private wrap.
+- **Algorithm / paper:** range reduction with `fmod`; D. Goldberg, *ACM Computing Surveys*, 23(1), 1991.
+- **Reuses:** `math::Fmod`.
+- **Spec:** [roadmap/math/AngleUtilities](roadmap/math/AngleUtilities/implementation.md).
+
+### 55. Polynomial point-to-point profile
+- **What:** Cubic (position + velocity) or quintic (+ acceleration) time law between two states over a
+  fixed duration; defines the shared `TrajectoryState` / `MotionLimits` types of `motion_profiles`.
+- **Embedded value:** Coefficients solved once in closed form; each servo tick is a Horner evaluation.
+- **Algorithm / paper:** M. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 5.
+- **Reuses:** Scalar templates.
+- **Spec:** [roadmap/motion_profiles/PolynomialProfile](roadmap/motion_profiles/PolynomialProfile/implementation.md).
+
+### 56. Trapezoidal (LSPB) profile
+- **What:** Minimum-time velocity/acceleration-limited rest-to-rest move, triangular for short moves, plus
+  a fixed-duration plan for multi-axis synchronization.
+- **Embedded value:** The standard servo-axis move (robot joints, CNC, conveyors, motor position loops).
+- **Algorithm / paper:** L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and
+  Robots* (2008), Ch. 3.
+- **Reuses:** Item 55 types.
+- **Spec:** [roadmap/motion_profiles/TrapezoidalProfile](roadmap/motion_profiles/TrapezoidalProfile/implementation.md).
+
+### 57. Friction compensation
+- **What:** Feedforward Coulomb + viscous + Stribeck friction per axis with a `tanh` zero-crossing and an
+  output clamp.
+- **Embedded value:** Removes stick-slip and tracking lag in geared drives and joints for one `exp` and one
+  `tanh` per axis.
+- **Algorithm / paper:** B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, *Automatica*, 30(7), 1994.
+- **Reuses:** `math::Vector`.
+- **Spec:** [roadmap/controllers/FrictionCompensation](roadmap/controllers/FrictionCompensation/implementation.md).
+
+### 58. Symplectic integrators
+- **What:** Semi-implicit Euler and velocity Verlet for second-order systems `q̈ = a(q, q̇, u)`.
+- **Embedded value:** Bounded energy error for simulated mechanical plants at one or two evaluations per
+  step (RK4 needs four and still drifts).
+- **Algorithm / paper:** E. Hairer, C. Lubich, G. Wanner, *Geometric Numerical Integration* (2006).
+- **Reuses:** `math::Vector`; complements `RungeKuttaIntegrators`.
+- **Spec:** [roadmap/solvers/SymplecticIntegrators](roadmap/solvers/SymplecticIntegrators/implementation.md).
+
+### 59. Tridiagonal solver
+- **What:** Thomas algorithm, factor once / solve many, pivot-failure reporting.
+- **Embedded value:** `O(n)` instead of `O(n³)` for splines and implicit difference schemes.
+- **Algorithm / paper:** L. H. Thomas (1949); G. Golub, C. Van Loan, *Matrix Computations*, §4.3.
+- **Reuses:** Bounded `std::array` bands.
+- **Spec:** [roadmap/math/TridiagonalSolver](roadmap/math/TridiagonalSolver/implementation.md).
+
+---
+
+## Tier 2 — Easy ★★☆☆☆
+
+### 53. SE(3) transform, twists and wrenches
+- **What:** Rigid transform (rotation + translation) with closed-form inverse, adjoint, twist/wrench
+  transforms, exponential/logarithm (SO(3) and SE(3)) and a log-map pose error. 6-vectors are ordered
+  linear part first: twists `(v; ω)`, wrenches `(f; n)`.
+- **Embedded value:** The algebra every pose-level consumer shares — robot kinematics, Jacobians and
+  task-space control in robotics-toolbox-cpp, drone attitude/position control, visual odometry. One
+  type and one ordering convention prevent silent block-order mismatches between modules.
+- **Algorithm / paper:** K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 3; R. Murray, Z. Li,
+  S. Sastry (1994), Ch. 2; J. Solà et al., "A micro Lie theory for state estimation in robotics" (2018).
+- **Reuses:** `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), `Quaternion`.
+- **Spec:** [roadmap/math/SE3Transform](roadmap/math/SE3Transform/implementation.md).
+
+### 60. S-curve (jerk-limited) profile
+- **What:** Seven-segment jerk-bounded rest-to-rest move with all short-move cases in closed form.
+- **Embedded value:** Continuous acceleration — far less vibration than a trapezoid for precision axes.
+- **Algorithm / paper:** L. Biagiotti, C. Melchiorri (2008), §3.4.
+- **Reuses:** Items 55 (types) and 56 (synchronization scheme).
+- **Spec:** [roadmap/motion_profiles/SCurveProfile](roadmap/motion_profiles/SCurveProfile/implementation.md).
+
+### 61. Cubic spline interpolation
+- **What:** C² vector-valued spline through `K` knots, clamped or natural ends, shared factorization across
+  channels.
+- **Embedded value:** Smooth interpolation of waypoint lists and calibration tables in linear time on fixed
+  storage.
+- **Algorithm / paper:** C. de Boor, *A Practical Guide to Splines* (2001), Ch. IV.
+- **Reuses:** Item 59.
+- **Spec:** [roadmap/math/CubicSpline](roadmap/math/CubicSpline/implementation.md).
+
+### 62. Damped pseudo-inverse and null-space projector
+- **What:** `A⁺_λ·b` and the exact projector `I − A⁺A` for any `Rows × Cols` matrix from one SVD.
+- **Embedded value:** Singularity-robust inversion for redundant robots, over-actuated control allocation
+  and regularized calibration; the projector exposes spare actuation to a secondary objective.
+- **Algorithm / paper:** C. Wampler, *IEEE Trans. SMC*, 16(1), 1986; A. Liégeois, *IEEE Trans. SMC*,
+  7(12), 1977.
+- **Reuses:** `SingularValueDecomposition` (tall and wide).
+- **Spec:** [roadmap/solvers/DampedPseudoInverse](roadmap/solvers/DampedPseudoInverse/implementation.md).
+
+---
+
+## Tier 3 — Moderate ★★★☆☆
+
+### 63. Incremental QR least squares
+- **What:** Streaming Givens-QR accumulation of an unbounded number of rows into a `P×P` factor, with a
+  rank-revealing SVD solve (minimum-norm solution, identifiable directions, residual RMS).
+- **Embedded value:** Parameter identification from long logs in fixed memory without squaring the
+  condition number.
+- **Algorithm / paper:** G. Golub, C. Van Loan, §5.1.8 and §5.5; Å. Björck (1996), Ch. 2.
+- **Reuses:** `GivensRotation`, `SingularValueDecomposition`.
+- **Spec:** [roadmap/estimators/offline/IncrementalQrLeastSquares](roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md).
+
+### 64. Levenberg–Marquardt
+- **What:** Damped Gauss–Newton for small nonlinear least squares with a problem-supplied retraction
+  (vectors, bounds, rotations) and a pluggable damping policy.
+- **Embedded value:** One bounded, heap-free solver for IK, parallel-robot forward kinematics, calibration
+  and scan matching.
+- **Algorithm / paper:** K. Levenberg (1944); D. Marquardt (1963); J. Nocedal, S. Wright (2006), Ch. 10.
+- **Reuses:** `LuDecomposition`.
+- **Spec:** [roadmap/optimization/LevenbergMarquardt](roadmap/optimization/LevenbergMarquardt/implementation.md).
+
+---
+
+## Tier 4 — Advanced ★★★★☆
+
+### 65. Dense convex QP
+- **What:** Goldfarb–Idnani dual active-set solver for strictly convex QPs with equality and inequality
+  constraints; finite, bounded iterations.
+- **Embedded value:** Exact constrained allocation (cable tensions, thrusters), task-priority control and
+  constrained MPC without heuristics.
+- **Algorithm / paper:** D. Goldfarb, A. Idnani, *Mathematical Programming*, 27(1), 1983; M. J. D. Powell
+  (1985).
+- **Reuses:** `CholeskyDecomposition`, `TriangularSolve`, `GivensRotation`.
+- **Spec:** [roadmap/solvers/ActiveSetQp](roadmap/solvers/ActiveSetQp/implementation.md).
+
+---
+
 ## Evaluation & metrics primitives
 
 Reusable quantities that quantify *how well* an algorithm behaves — the properties the per-family

doc/solvers/SingularValueDecomposition.md

@@ -2,9 +2,9 @@
 
 ## Overview & Motivation
 
-The singular value decomposition (SVD) factors any $m \times n$ matrix $A$ ($m \geq n$) into
+The singular value decomposition (SVD) factors any $m \times n$ matrix $A$ into
 two orthonormal rotation factors and a non-negative diagonal, revealing the fundamental geometric
-action of $A$ as a stretch along $n$ orthogonal directions. It is the most numerically stable
+action of $A$ as a stretch along $k = \min(m, n)$ orthogonal directions. It is the most numerically stable
 matrix factorization available and exposes rank, condition number, and the pseudo-inverse directly
 from its output. In embedded systems, SVD powers robust least-squares solvers for sensor calibration,
 redundant-actuator control allocation, and low-rank model reduction — all applications where the
@@ -14,16 +14,23 @@ matrix $A$ may be ill-conditioned or rank-deficient and normal-equation approach
 
 ### Factorization
 
-For $A \in \mathbb{R}^{m \times n}$ with $m \geq n$, the thin SVD is
+For $A \in \mathbb{R}^{m \times n}$ and $k = \min(m, n)$, the thin SVD is
 
 $$A = U \Sigma V^\top,$$
 
-where $U \in \mathbb{R}^{m \times n}$ has orthonormal columns ($U^\top U = I_n$),
-$\Sigma = \mathrm{diag}(\sigma_1, \ldots, \sigma_n)$ with $\sigma_1 \geq \cdots \geq \sigma_n \geq 0$,
-and $V \in \mathbb{R}^{n \times n}$ is orthogonal ($V^\top V = V V^\top = I_n$).
+where $U \in \mathbb{R}^{m \times k}$ and $V \in \mathbb{R}^{n \times k}$ have orthonormal columns
+($U^\top U = V^\top V = I_k$) and $\Sigma = \mathrm{diag}(\sigma_1, \ldots, \sigma_k)$ with
+$\sigma_1 \geq \cdots \geq \sigma_k \geq 0$. For a tall or square matrix ($m \geq n$) $V$ is square
+and orthogonal; for a wide matrix ($m < n$) $U$ is.
 The scalars $\sigma_i$ are the **singular values** of $A$; the columns of $U$ and $V$ are
 the left and right **singular vectors**.
 
+### Wide Matrices
+
+The Golub–Kahan bidiagonalization below runs on a tall matrix. A wide $A$ is handled through its
+transpose: factor $A^\top = U' \Sigma V'^\top$ (tall), then $A = V' \Sigma U'^\top$, so $U = V'$ and
+$V = U'$. The singular values are shared, and no extra storage is needed beyond the tall factors.
+
 ### Connection to the Symmetric Eigenproblem
 
 The singular values satisfy $\sigma_i^2 = \lambda_i(A^\top A)$, where $\lambda_i$ denotes the
@@ -43,7 +50,7 @@ or numerical noise.
 
 ### Rank and Condition Number
 
-$$\mathrm{rank}(A) = \#\{i : \sigma_i > \tau\}, \quad \kappa(A) = \sigma_1 / \sigma_n.$$
+$$\mathrm{rank}(A) = \#\{i : \sigma_i > \tau\}, \quad \kappa(A) = \sigma_1 / \sigma_k.$$
 
 The condition number $\kappa$ measures sensitivity: a system with $\kappa \gg 1$ amplifies
 input perturbations by a factor of $\kappa$ in the least-squares solution.
@@ -52,10 +59,12 @@ input perturbations by a factor of $\kappa$ in the least-squares solution.
 
 | Phase             | Time                                            | Space    | Notes                                        |
 |-------------------|-------------------------------------------------|----------|----------------------------------------------|
-| Bidiagonalization | $O(mn^2 - n^3/3)$                               | $O(mn)$  | Two-sided Householder reflectors             |
-| QR sweeps         | $O(n^2)$ per iteration, $O(n)$ total iterations | $O(n^2)$ | Golub-Reinsch implicit-shift; converges fast |
-| Pseudo-inverse    | $O(n^2 m)$                                      | $O(nm)$  | Matrix triple product $V \Sigma^+ U^\top$    |
-| Total             | $O(mn^2 + n^3)$                                 | $O(mn)$  | Dominated by bidiagonalization               |
+| Bidiagonalization | $O(Mk^2 - k^3/3)$                               | $O(mn)$  | Two-sided Householder reflectors             |
+| QR sweeps         | $O(k^2)$ per iteration, $O(k)$ total iterations | $O(k^2)$ | Golub-Reinsch implicit-shift; converges fast |
+| Pseudo-inverse    | $O(k\,mn)$                                      | $O(nm)$  | Matrix triple product $V \Sigma^+ U^\top$    |
+| Total             | $O(Mk^2 + k^3)$                                 | $O(mn)$  | Dominated by bidiagonalization               |
+
+Here $M = \max(m, n)$ and $k = \min(m, n)$: a wide matrix costs the same as its transpose.
 
 All storage is stack-allocated; no heap is used. The bidiagonal form is stored implicitly in
 the working copy of $A$ alongside the Householder reflector scalars.
@@ -86,13 +95,14 @@ negated. After descending sort: $\sigma = (3, 2, 1)$.
   diagonal entries into the sign of the corresponding $U$ column so that all $\sigma_i \geq 0$.
 - **Repeated singular values:** the corresponding singular-vector subspaces are correct but the
   individual vectors are not unique; tests should check subspace properties, not individual vectors.
-- **Condition number with zero $\sigma_{\min}$:** a rank-deficient matrix has $\sigma_n = 0$;
+- **Condition number with zero $\sigma_{\min}$:** a rank-deficient matrix has $\sigma_k = 0$;
   `ConditionNumber()` returns zero as a sentinel in this case rather than dividing by zero.
 
 ## Variants & Generalizations
 
-- **Full SVD:** $U \in \mathbb{R}^{m \times m}$ (all $m$ left singular vectors); the extra
-  $m - n$ columns span the left null space of $A$. The thin SVD suffices for least squares.
+- **Full SVD:** $U \in \mathbb{R}^{m \times m}$ and $V \in \mathbb{R}^{n \times n}$; the extra columns
+  span the left and right null spaces of $A$. The thin SVD suffices for least squares; a null-space
+  projector $I - V V^\top$ needs only the thin $V$.
 - **Truncated SVD:** retain only the $k$ largest singular values for low-rank approximation
   $A \approx U_k \Sigma_k V_k^\top$; the approximation error in Frobenius norm equals
   $\sqrt{\sigma_{k+1}^2 + \cdots + \sigma_n^2}$ (Eckart-Young theorem).
@@ -105,7 +115,8 @@ negated. After descending sort: $\sigma = (3, 2, 1)$.
 ## Applications
 
 - **Least-squares regression:** $x = A^+ b$ minimises $\|Ax - b\|_2$ and is unique among
-  minimum-norm solutions when $A$ is rank-deficient.
+  minimum-norm solutions when $A$ is rank-deficient. For a wide, full-row-rank $A$ (an
+  underdetermined system) it is the minimum-norm exact solution $A^\top (A A^\top)^{-1} b$.
 - **Control allocation:** maps actuator space to output space; the pseudo-inverse distributes
   effort across redundant actuators while respecting the null space.
 - **System identification:** SVD of a Hankel matrix (Ho-Kalman) recovers system order (rank)

numerical/solvers/SingularValueDecomposition.cpp

@@ -4,4 +4,5 @@ namespace solvers
 {
     template class SingularValueDecomposition<float, 4, 3>;
     template class SingularValueDecomposition<float, 3, 3>;
+    template class SingularValueDecomposition<float, 3, 4>;
 }