Skip to content

Commit dc147fb

Browse files
feat(math): add SE2Transform (NT66) (#366)
* feat(math): add SE2Transform (NT66) Planar rigid transform with the rotation stored as a unit complex number: composition, closed-form inverse, adjoint and twist/wrench maps, arc-exact Exp with a series below |phi| = 0.1, singularity-free Log on (-pi, pi], short-way PoseError, renormalisation and ToSE3. Linear-first ordering as in SE3Transform. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN * docs(math): wrap long SE2Transform doc lines Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --------- Co-authored-by: Claude <noreply@anthropic.com>
1 parent ab6e9fd commit dc147fb

13 files changed

Lines changed: 682 additions & 454 deletions

File tree

‎README.md‎

Lines changed: 187 additions & 187 deletions
Large diffs are not rendered by default.

‎ROADMAP.md‎

Lines changed: 3 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,6 @@ Difficulty legend:
2626

2727
| # | Component | Target module | Difficulty |
2828
|----|-------------------------------------------------------|-------------------------|------------|
29-
| 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ |
3029
| 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ |
3130
| 74 | Residual-form Kalman update with χ² gating | `filters/active` | ★★☆☆☆ |
3231
| 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ |
@@ -36,9 +35,9 @@ Difficulty legend:
3635
| 72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` (new) | ★★★★☆ |
3736
| 73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` (new) | ★★★★★ |
3837

39-
Items 66–74 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
40-
NT66–NT74; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization
41-
families and also have non-robotics consumers. Items 66, 67, 70 and 74 have specs under
38+
Items 67–74 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
39+
NT67–NT74; NT53–NT66 are deployed). They serve its mobile, aerial, legged, marine and localization
40+
families and also have non-robotics consumers. Items 67, 70 and 74 have specs under
4241
[`roadmap/`](roadmap/README.md); items 68, 69 and 71–73 have none yet. All are *(float-first)*.
4342

4443
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
4847

4948
## Tier 2 — Easy ★★☆☆☆
5049

51-
### 66. SE(2) transform
52-
- **What:** Planar rigid transform `(R(θ), p)` with composition, closed-form inverse, adjoint, exp/log of
53-
planar twists `(vx, vy, ω)` and a pose error, ordered linear part first like item 53.
54-
- **Embedded value:** The pose type of every ground vehicle and planar mechanism — odometry, path
55-
tracking, scan matching — at a fraction of SE(3)'s cost.
56-
- **Algorithm / paper:** J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in
57-
robotics" (2018), Appendix C.
58-
- **Reuses:** Item 53 (ordering convention, `ToSE3`).
59-
- **Spec:** [roadmap/math/SE2Transform](roadmap/math/SE2Transform/implementation.md).
60-
6150
### 67. Kabsch / Umeyama alignment
6251
- **What:** Least-squares rotation, translation and optional scale between two corresponding point sets
6352
(orthogonal Procrustes), with the reflection fix, in 2-D and 3-D.

‎doc/math/README.md‎

Lines changed: 18 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -4,20 +4,21 @@ Core mathematical primitives for numerical computation.
44

55
## Algorithms
66

7-
| Algorithm | Description |
8-
|----------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------|
9-
| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier |
10-
| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions |
11-
| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize |
12-
| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive |
13-
| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive |
14-
| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR |
15-
| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU |
16-
| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) |
17-
| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector |
18-
| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine |
19-
| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared |
20-
| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements |
21-
| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems |
22-
| [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization |
23-
| [SE(3) Transform](SE3Transform.md) | Rigid transforms with twist/wrench adjoints and an accurate exp/log (linear-first $(v;\omega)$ ordering) |
7+
| Algorithm | Description |
8+
|----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------|
9+
| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier |
10+
| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions |
11+
| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize |
12+
| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive |
13+
| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive |
14+
| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR |
15+
| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU |
16+
| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) |
17+
| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector |
18+
| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine |
19+
| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared |
20+
| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements |
21+
| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems |
22+
| [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization |
23+
| [SE(3) Transform](SE3Transform.md) | Rigid transforms with twist/wrench adjoints and an accurate exp/log (linear-first $(v;\omega)$ ordering) |
24+
| [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) |

‎doc/math/SE2Transform.md‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
# SE(2) Transform, Planar Twists and Wrenches
2+
3+
## Overview & Motivation
4+
5+
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.
6+
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.
7+
8+
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.
9+
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.
10+
11+
## Mathematical Theory
12+
13+
### Planar Transform
14+
15+
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
16+
17+
$$R(\theta) = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix}, \qquad T = \begin{pmatrix} R & p \\ 0 & 1 \end{pmatrix}.$$
18+
19+
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:
20+
21+
$$T_1 T_2 = (z_1 z_2,\; R_1 p_2 + p_1), \qquad T^{-1} = (\bar{z},\; -R^T p).$$
22+
23+
The heading is never stored. It is recovered as $\theta = \operatorname{atan2}(\sin, \cos) \in (-\pi, \pi]$, so it is always wrapped.
24+
25+
### Ordering Convention (normative)
26+
27+
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.
28+
29+
### Adjoint, Twists and Wrenches
30+
31+
A twist in frame B is re-expressed in frame A by
32+
33+
$$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}.$$
34+
35+
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:
36+
37+
$$f_A = R f_B, \qquad \tau_A = \tau_B + p_x f_{A,y} - p_y f_{A,x}.$$
38+
39+
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}$.
40+
41+
### Exponential
42+
43+
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:
44+
45+
$$\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},$$
46+
47+
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.
48+
Away from it, $B$ uses the half-angle form $2\sin^2(\varphi/2)/\varphi$.
49+
50+
### Logarithm
51+
52+
The inverse of the exponential with $t = 1$ returns the twist that carries the identity to $T$:
53+
54+
$$\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.$$
55+
56+
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$.
57+
58+
### Pose Error
59+
60+
The base-frame pose error between a target and the current pose is
61+
62+
$$e = \left(p_{\text{target}} - p_{\text{current}},\; \operatorname{atan2}(\sin(\theta_t - \theta_c),\, \cos(\theta_t - \theta_c))\right).$$
63+
64+
The heading term is computed from the complex product $z_t \bar{z}_c$, so it always takes the short way round.
65+
66+
## Complexity Analysis
67+
68+
| Operation | Time | Space | Notes |
69+
|-------------------------------|--------|--------|-----------------------------------------------|
70+
| Compose, invert, apply | $O(1)$ | $O(1)$ | At most 8 multiplications, no trigonometry |
71+
| Adjoint, twist and wrench map | $O(1)$ | $O(1)$ | One rotation and one lever-arm term |
72+
| Exponential | $O(1)$ | $O(1)$ | One sine/cosine pair plus one half-angle sine |
73+
| Logarithm | $O(1)$ | $O(1)$ | One `atan2` and one sine/cosine pair |
74+
| Renormalisation | $O(1)$ | $O(1)$ | One hypotenuse |
75+
76+
## Step-by-Step Walkthrough
77+
78+
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)$.
79+
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.
80+
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)$.
81+
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)$.
82+
5. **Pose error**: from heading $0.2$ to $0.2 + 190°$, the heading error is $-170°$ ($-2.967$ rad), not $+190°$.
83+
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.
84+
85+
## Pitfalls & Edge Cases
86+
87+
- **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.
88+
- **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.
89+
- **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.
90+
- **Ordering**: twists, wrenches, logarithms and pose errors are linear first. Formulas from sources that put the angular component first must be permuted.
91+
- **Constructed transforms**: a transform built directly from a cosine and sine must be given a unit pair. Construction from a heading always is.
92+
93+
## Variants & Generalizations
94+
95+
- **Lie-group Jacobians**: left and right Jacobians of SE(2), and the $\oplus$/$\ominus$ operators for on-manifold estimation.
96+
- **Error-state filters**: Kalman filters that estimate a small pose perturbation in the tangent space.
97+
- **SE(3)**: the spatial counterpart with the same conventions. A planar pose lifts to a rotation about the vertical axis.
98+
- **Similarity group Sim(2)**: adds a scale factor for monocular mapping and image registration.
99+
100+
## Applications
101+
102+
- Wheel odometry with exact arc integration.
103+
- Path tracking controllers that need a pose error in the vehicle frame.
104+
- Landmark and scan-matching localization on planar maps.
105+
- Planar mechanisms and the base pose of mobile manipulators.
106+
107+
## Connections to Other Algorithms
108+
109+
- **SE(3) Transform**: the spatial version. The planar transform lifts into it as a rotation about the vertical axis, with zero height.
110+
- **Angle Utilities**: the shortest signed angle difference is the scalar analogue of the short-way heading error.
111+
- **Matrix Exponential**: the general matrix exponential of the $3\times3$ twist matrix is the closed form's reference.
112+
113+
## References & Further Reading
114+
115+
- J. Solà, J. Deray, and D. Atchuthan, "A micro Lie theory for state estimation in robotics," arXiv:1812.01537, 2018 — Appendix C.
116+
- K. M. Lynch and F. C. Park, *Modern Robotics: Mechanics, Planning, and Control*, Cambridge University Press, 2017 — Chapters 3 and 13.
117+
- T. D. Barfoot, *State Estimation for Robotics*, Cambridge University Press, 2017 — Chapter 7.

‎numerical/math/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ target_sources(numerical.math PRIVATE
3434
QNumber.hpp
3535
Quaternion.hpp
3636
RecursiveBuffer.hpp
37+
SE2Transform.hpp
3738
SE3Transform.hpp
3839
Statistics.hpp
3940
StepResponseMetrics.hpp
@@ -57,6 +58,7 @@ numerical_add_coverage_sources(numerical.math
5758
QNumber.cpp
5859
Quaternion.cpp
5960
RecursiveBuffer.cpp
61+
SE2Transform.cpp
6062
SE3Transform.cpp
6163
Toeplitz.cpp
6264
TridiagonalSolver.cpp

‎numerical/math/SE2Transform.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
#include "numerical/math/SE2Transform.hpp"
2+
3+
namespace math
4+
{
5+
template struct SE2Transform<float>;
6+
}

0 commit comments

Comments
 (0)