The Geometry of Motion

Frames, rotations, SE(2), and the exponential map — why a pose is not a vector, and how ⊞ and ⊟ let every filter in this book pretend that it is.

The average of 179° and −179°

Rusty has two heading estimates. One says 179°, the other says −179°. They disagree by two degrees. What is the average heading?

Add and halve: $(179 + (-179))/2 = 0$. The robot is now facing due east, ninety degrees off from where either estimate said, having averaged two nearly identical opinions into their exact opposite. Nothing was noisy, nothing was miscalibrated, and no floating-point rounding was involved. The arithmetic was simply applied to objects that do not support it.

The failure is not an edge case to be patched with an if statement. It is structural: headings live on a circle, and a circle has no consistent notion of "add these two numbers." Every quantity in this book that involves an orientation — a pose, a rotation, a relative measurement between two poses — has the same property, and every operation a filter performs on such a quantity has to be defined rather than assumed.

The repair visible in the purple needle is the entire chapter compressed into one line. Do not add the angles. Instead:

1. Ask what rotation takes A to B. That is a difference, written $\bminus$, and on a circle it is the short way around: $\theta_B \bminus \theta_A = 2°$, not $-358°$.
2. Halve that rotation. Rotations can be scaled — they live in a genuine vector space, the tangent space.
3. Apply the halved rotation to A. That is $\bplus$, and the result is $180°$.

Subtract on the manifold, work in the tangent space, come back to the manifold. Chapters 7, 9, 15, and 16 are all applications of that sentence to progressively harder problems.

Building intuition: frames, and the notation that types them

Before rotations there are frames. A frame is a choice of origin and orthonormal axes; a point in physical space has one position and as many coordinate triples as there are frames looking at it. Craig's notation makes the bookkeeping explicit by decorating every symbol with the frame it is expressed in:

Read every transform name left to right as target-from-source. Then ${}^{A}_{B}T$ consumes something expressed in $\{B\}$ and produces something expressed in $\{A\}$, and the rule for chaining transforms writes itself: adjacent indices must match, and they cancel.

Two features of that widget deserve to be stated as mathematics rather than left as observations.

Derivation: rotation matrices are stacked axes

Derivation: the cancellation rule

A homogeneous transform packages rotation and translation into one matrix that composes by multiplication:

A worked composition, checkable in your head. Put Rusty at ${}^{W}_{B}T = (2, 1, 90°)$ and mount the lidar at ${}^{B}_{L}T = (0.3, 0, 0)$ — thirty centimetres forward on the robot's nose. Then

The lidar sits 30 cm north of the robot, not east, because "forward" is a body-frame word. A landmark the lidar reports at ${}^{L}p = (1, 0)$ is therefore at ${}^{W}p = (2 - 0,\; 1.3 + 1) = (2,\; 2.3)$ in the map — a metre "ahead" of the sensor is a metre north in the world, because the whole chain was rotated. The widget above runs the same product with a yawed mount, so its numbers differ; the arithmetic does not.

The mathematics

Rotations in three dimensions, and the traps

$\SOthree$ is $\SOtwo$'s harder sibling. Three facts make it harder, and all three cost real robots real accuracy.

It is three-dimensional but embedded in nine numbers. A $3\times3$ matrix has nine entries constrained by six equations ($R\T R = I$ is symmetric), leaving three degrees of freedom. Numerical drift pushes a matrix off that constraint surface, so long-running code must re-orthonormalize.

It is not commutative. $R_1 R_2 \ne R_2 R_1$ in general. Rotate this page 90° about the vertical axis, then 90° about the horizontal one; reverse the order; the page ends up somewhere else. The Frame Composer above shows the same fact in the plane by applying one increment about the body frame and about the world frame.

Every minimal parameterization is singular somewhere. Three-parameter representations of $\SOthree$ — roll-pitch-yaw and its twenty-three siblings — all have configurations where two of the three axes align and one degree of freedom silently disappears.

Unit quaternions are the standard repair: four numbers, one constraint, no singularities.

The exponential map

Here is the question that unifies everything. A rigid body moves with constant velocity for one unit of time. Where does it end up?

Now do the same for a full rigid motion. This is the derivation the rest of the book leans on.

The geometric content of $V(\omega)$ is worth saying in words, because it is the payoff. A constant twist does not drive a body along a straight line; it sweeps the body around a fixed point — the instantaneous center of rotation, at $(-v_y/\omega,\ v_x/\omega)$ in body coordinates — at constant angular rate. This is the planar case of the Chasles–Mozzi theorem, the one Lynch and Park state as a screw: every rigid displacement is a rotation about some axis combined with a translation along it, and in the plane the translation vanishes, leaving a rotation about a point or a pure translation. $V$ is the bookkeeping that converts "how far I drove along the arc" into "where I ended up".

The adjoint

Twists, like points, are frame-dependent. The object that moves them between frames is the adjoint.

The adjoint is how a covariance changes frames. If a body-frame uncertainty is $\Sigma_{\text{body}}$, the same uncertainty in the world frame is $\Ad_T\,\Sigma_{\text{body}}\,\Ad_T\T$ — the manifold version of the familiar $A\Sigma A\T$, and the reason Chapter 9 can compose odometry uncertainty at all.

⊞ and ⊟: a legitimate local vector space

Every estimator in this book wants to write $x + \delta$: the Kalman update adds a correction to a mean, Gauss–Newton adds a step to an iterate, a particle filter adds noise to a sample. On $\SEtwo$ that expression is undefined. The fix is to define it, once, with the two operators already used informally above:

Vectors are manifolds too, with $\bplus = +$ and $\bminus = -$. That is not a technicality: it means one generic filter implementation covers a Euclidean state, a pose, and a pose-plus-bias state, with the compiler picking the right operators.

The algorithms

Geometry has no Thrun table names, so these are the book's Rust API, stated once and never re-derived.

Everything else is one line on top of these: $\text{compose}(a,b) = ab$, $\text{inverse}(T) = (R\T, -R\T t)$, $x \bplus \delta = x\cdot\texttt{se2\_exp}(\delta)$, and $y \bminus x = \texttt{se2\_log}(x^{-1}y)$.

Uncertainty on a manifold

A short section now, because it is the bridge to Parts II and III; the full treatment is Chapter 9.

If a pose is not a vector, a distribution over poses is not the Gaussian on $\R^3$ that Chapter 2 built. Writing $\Normal\big((x,y,\theta); \mu, \Sigma\big)$ is already wrong: it assigns different densities to the same rotation depending on which branch of $\theta$ you wrote down. The standard repair is a concentrated Gaussian: put an ordinary Gaussian in the tangent space, where it is at home, and push it onto the group through the retraction,

The mean $\bar x$ lives on the manifold, the covariance lives in the tangent space at $\bar x$, and every operation a filter performs happens in that tangent space. What the construction produces in $(x, y)$ coordinates is not an ellipse.

Two consequences, both of which get proper treatment later. First, $\E[\exp(\tau)] \ne \exp(\E[\tau])$: the sample mean of the cloud is not the commanded endpoint, and the gap grows with $\sigma_\omega$ and with distance travelled. Second, an ellipse fitted to the cloud claims mass in places the robot cannot be. A Gaussian filter tracking a pose is making a bet that the banana is thin enough to pass for an ellipse over one time step — which is usually true at 60 Hz and disastrously false over a ten-metre dead-reckoned stretch. Chapter 7 names that bet and prices it.

Implementation in Rust

The type is small enough to read in one sitting and central enough that it is worth reading carefully. Two design decisions are load-bearing. The rotation is stored as a UnitComplex, not as an f64 angle, so no arithmetic can ever produce an unwrapped heading. And the tangent type is a fixed-size SVector<f64, 3>, so a dimension mismatch is a compile error rather than a panic.

use nalgebra::{Point2, SMatrix, SVector, UnitComplex, Vector2};

/// Tangent coordinates τ = (vₓ, v_y, ω)ᵀ — **translation first** (Solà order).
///
/// Lynch & Park write (ω, v). We do not, because the reader already holds the
/// pose tuple (x, y, θ) and because `factrs` and `sophus` order it this way.
pub type Tangent2 = SVector<f64, 3>;

/// Below this |ω| the closed forms are 0/0 and we switch to Taylor series.
const SMALL_ANGLE: f64 = 1e-8;

/// A planar rigid-body transform: the book's most-used type.
///
/// Storing the rotation as a `UnitComplex` rather than an angle is not a
/// micro-optimization — it makes an unwrapped heading *unrepresentable*.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SE2 {
    pub rot: UnitComplex<f64>,
    pub trans: Vector2<f64>,
}

impl SE2 {
    pub fn identity() -> Self {
        Self { rot: UnitComplex::identity(), trans: Vector2::zeros() }
    }

    pub fn new(x: f64, y: f64, theta: f64) -> Self {
        Self { rot: UnitComplex::new(theta), trans: Vector2::new(x, y) }
    }

    /// exp: 𝔰𝔢(2) → SE(2). Derivation 4, with the guard from its Step 6.
    pub fn exp(tau: &Tangent2) -> Self {
        let (vx, vy, w) = (tau[0], tau[1], tau[2]);
        let (a, b) = if w.abs() < SMALL_ANGLE {
            (1.0 - w * w / 6.0, w / 2.0 - w * w * w / 24.0)
        } else {
            // (1 − cos ω) written as 2 sin²(ω/2): the literal form loses about
            // eight digits to cancellation at the ω a 60 Hz loop produces.
            let sh = (0.5 * w).sin();
            (w.sin() / w, 2.0 * sh * sh / w)
        };
        Self {
            rot: UnitComplex::new(w),
            trans: Vector2::new(a * vx - b * vy, b * vx + a * vy),
        }
    }

    /// log: SE(2) → 𝔰𝔢(2). Exact inverse of `exp` for |ω| < π.
    pub fn log(&self) -> Tangent2 {
        let w = self.rot.angle(); // UnitComplex::angle is already in (−π, π]
        let h = 0.5 * w;
        // c = (ω/2)·cot(ω/2), the cancellation-free form of ω sin ω / 2(1−cos ω).
        let c = if w.abs() < SMALL_ANGLE { 1.0 - w * w / 12.0 } else { h / h.tan() };
        let (x, y) = (self.trans.x, self.trans.y);
        Tangent2::new(c * x + h * y, -h * x + c * y, w)
    }

    /// (Rᵀ, −Rᵀt). Note that the translation is rotated *before* it is negated.
    pub fn inverse(&self) -> Self {
        let r_inv = self.rot.inverse();
        Self { rot: r_inv, trans: -(r_inv * self.trans) }
    }

    /// Ad_T, satisfying T·exp(τ)·T⁻¹ = exp(Ad_T τ). Derivation 6.
    pub fn adjoint(&self) -> SMatrix<f64, 3, 3> {
        let r = self.rot.to_rotation_matrix();
        let m = r.matrix();
        SMatrix::<f64, 3, 3>::new(
            m[(0, 0)], m[(0, 1)],  self.trans.y,
            m[(1, 0)], m[(1, 1)], -self.trans.x,
            0.0,       0.0,        1.0,
        )
    }

    /// Map a point from this frame's coordinates into the parent frame.
    pub fn act(&self, p: &Point2<f64>) -> Point2<f64> {
        self.rot.transform_point(p) + self.trans
    }

    /// Display only. Never do arithmetic on the tuple — that is what ⊞ is for.
    pub fn xytheta(&self) -> (f64, f64, f64) {
        (self.trans.x, self.trans.y, self.rot.angle())
    }
}

impl std::ops::Mul for SE2 {
    type Output = SE2;
    fn mul(self, rhs: SE2) -> SE2 {
        SE2 { rot: self.rot * rhs.rot, trans: self.trans + self.rot * rhs.trans }
    }
}

The retraction goes in its own trait, because that is the interface every later estimator is generic over. D is the tangent dimension, and it is a const generic so that the compiler knows the size of every Jacobian in Chapter 7.

use nalgebra::SVector;
use crate::geom::se2::{Tangent2, SE2};

/// The interface the book's filters are written against.
///
/// `x ⊞ δ` moves `x` by a tangent-space increment; `y ⊟ x` recovers the
/// increment that would do it. Implementations must satisfy the Hertzberg
/// axioms, which `tests/geom.rs` checks by property test rather than by trust.
pub trait Manifold<const D: usize>: Copy {
    fn boxplus(&self, delta: &SVector<f64, D>) -> Self;
    fn boxminus(&self, other: &Self) -> SVector<f64, D>;
}

impl Manifold<3> for SE2 {
    /// Right (local) convention: the increment is applied in the body frame.
    fn boxplus(&self, delta: &Tangent2) -> Self {
        *self * SE2::exp(delta)
    }
    fn boxminus(&self, other: &Self) -> Tangent2 {
        (other.inverse() * *self).log()
    }
}

/// Vectors are manifolds too — flat ones. This impl is why one generic EKF
/// serves both a Euclidean state and a pose without a line of special-casing.
impl<const N: usize> Manifold<N> for SVector<f64, N> {
    fn boxplus(&self, delta: &SVector<f64, N>) -> Self { self + delta }
    fn boxminus(&self, other: &Self) -> SVector<f64, N> { self - other }
}

Craig's notation, enforced by rustc

Craig invented a type system in 1986 and wrote it in superscripts. Rust can check it. The markers are zero-sized, PhantomData occupies nothing, and the whole thing compiles down to the same machine code as bare SE2.

use core::marker::PhantomData;
use core::ops::Mul;
use crate::geom::se2::SE2;

pub trait Frame: 'static {}

pub struct World;
pub struct Body;
pub struct LidarF;
impl Frame for World {}
impl Frame for Body {}
impl Frame for LidarF {}

/// `Pose<A, B>` is Craig's ᴬ_BT: "A from B". Zero runtime cost, and the index
/// bookkeeping becomes the compiler's problem instead of yours.
#[derive(Clone, Copy, Debug)]
pub struct Pose<A: Frame, B: Frame>(pub SE2, PhantomData<fn() -> (A, B)>);

impl<A: Frame, B: Frame> Pose<A, B> {
    pub fn new(t: SE2) -> Self { Self(t, PhantomData) }
    pub fn inverse(self) -> Pose<B, A> { Pose::new(self.0.inverse()) }
}

/// The cancellation rule, as a trait impl: the inner frames must agree, and
/// they vanish from the output type.
impl<A: Frame, B: Frame, C: Frame> Mul<Pose<B, C>> for Pose<A, B> {
    type Output = Pose<A, C>;
    fn mul(self, rhs: Pose<B, C>) -> Pose<A, C> { Pose::new(self.0 * rhs.0) }
}

Now the payoff. This is a deliberate compile error, and it is a feature of the chapter:

let world_from_body: Pose<World, Body> = Pose::new(SE2::new(2.0, 1.0, FRAC_PI_2));
let world_from_lidar: Pose<World, LidarF> = Pose::new(SE2::new(2.0, 1.3, FRAC_PI_2));

// Wrong: the inner indices are W and W. They do not cancel.
let oops = world_from_body * world_from_lidar;

error[E0308]: mismatched types
  --> demos/ch03-geometry/examples/frame_safety.rs:6:31
   |
 6 |     let oops = world_from_body * world_from_lidar;
   |                                  ^^^^^^^^^^^^^^^^ expected `Pose<Body, _>`,
   |                                                   found `Pose<World, LidarF>`

What you meant was world_from_body.inverse() * world_from_lidar, whose type is Pose<Body, LidarF> — the sensor extrinsics. The convention for the rest of the book: bare SE2 for state inside an estimator, where everything is in one frame by construction, and Pose<_, _> at subsystem boundaries — extrinsics, map anchoring, anything crossing a module line.

Putting it together: the square dance

Here is the numeric example to check by hand. It is the smallest computation that exercises the whole chapter, and a test pins it.

Drive a quarter circle. Command $\boldsymbol\tau = (1, 0, \tfrac{\pi}{2})\T$: one metre of arc while turning ninety degrees. From Derivation 4,

because $\sin\frac{\pi}{2} = 1$ and $1 - \cos\frac{\pi}{2} = 1$, and $1/\omega = 2/\pi$. So

Sanity check the geometry: the turn radius is $|v|/\omega = 2/\pi$, the center of the arc is at $(0, 2/\pi)$, and rotating the start point $90°$ about that center lands exactly on $(2/\pi, 2/\pi)$.

Go back. $\log$ of that pose returns $\omega = \pi/2$, so $h = \pi/4$ and $c = \frac{\pi}{4}\cot\frac{\pi}{4} = \frac{\pi}{4}$ — the two entries of $V^{-1}$ are equal, and

Compare with Euler integration. The classical update $x \mathrel{+}= \Delta s\cos\theta$, $y \mathrel{+}= \Delta s\sin\theta$, $\theta \mathrel{+}= \Delta\theta$ applied in $K$ equal substeps gives

The error falls like $1/K$, which is first-order convergence, which is what "Euler" means. The exponential is exact at $K = 1$, because it is not an approximation of the arc — it is the arc.

Close a square. Alternate exp((1,0,0)) and exp((0,0,π/2)) four times. Rusty returns to the origin, and the closure error measured on the manifold, $\norm{\,\text{final} \bminus \mathrm{SE2::identity()}\,}$, is at the level of floating-point round-off — around $10^{-16}$, not $10^{-3}$. That is the difference between arithmetic that respects the group and arithmetic that approximates it.

use approx::assert_relative_eq;
use nalgebra::Vector3;
use proptest::prelude::*;
use rand::{rngs::SmallRng, Rng, SeedableRng};
use std::f64::consts::{FRAC_PI_2, PI};

use pr_core::geom::{manifold::Manifold, se2::{Tangent2, SE2}};

/// The chapter's worked example, pinned to 1e-12.
#[test]
fn worked_example_ch03_quarter_arc() {
    let tau = Tangent2::new(1.0, 0.0, FRAC_PI_2);
    let t = SE2::exp(&tau);
    let (x, y, theta) = t.xytheta();

    assert_relative_eq!(x, 2.0 / PI, epsilon = 1e-12); // 0.636619772...
    assert_relative_eq!(y, 2.0 / PI, epsilon = 1e-12);
    assert_relative_eq!(theta, FRAC_PI_2, epsilon = 1e-12);

    // …and log takes us straight back.
    assert_relative_eq!(t.log(), tau, epsilon = 1e-12);

    // One Euler step is off by 73 centimetres on a one-metre arc.
    let euler = (1.0 - x).hypot(0.0 - y);
    assert_relative_eq!(euler, 0.733028, epsilon = 1e-6);
}

/// Four sides and four right-angle turns must return exactly to the start.
#[test]
fn square_dance_closes() {
    let forward = SE2::exp(&Tangent2::new(1.0, 0.0, 0.0));
    let turn = SE2::exp(&Tangent2::new(0.0, 0.0, FRAC_PI_2));

    let mut pose = SE2::identity();
    for _ in 0..4 {
        pose = pose * forward * turn;
    }

    // Measured on the manifold, not on the tuple: ‖final ⊟ identity‖.
    let closure = pose.boxminus(&SE2::identity()).norm();
    assert!(closure < 1e-14, "closure error {closure:e} — the group is not closing");
}

fn twist() -> impl Strategy<Value = Tangent2> {
    // |ω| < π keeps us inside log's injectivity radius, per Derivation 4 Step 5.
    (-5.0f64..5.0, -5.0f64..5.0, -3.14f64..3.14)
        .prop_map(|(a, b, c)| Tangent2::new(a, b, c))
}

proptest! {
    #[test]
    fn exp_log_roundtrip(tau in twist()) {
        prop_assert!((SE2::exp(&tau).log() - tau).norm() < 1e-12);
    }

    #[test]
    fn group_axioms(a in twist(), b in twist(), c in twist()) {
        let (ta, tb, tc) = (SE2::exp(&a), SE2::exp(&b), SE2::exp(&c));
        let lhs = (ta * tb) * tc;
        let rhs = ta * (tb * tc);
        prop_assert!(lhs.boxminus(&rhs).norm() < 1e-12);          // associativity
        prop_assert!((ta * ta.inverse()).log().norm() < 1e-12);   // inverse
    }

    /// The authority on the adjoint's sign convention. Not memory — this.
    #[test]
    fn adjoint_conjugation(t in twist(), tau in twist()) {
        let big_t = SE2::exp(&t);
        let lhs = big_t * SE2::exp(&tau) * big_t.inverse();
        let rhs = SE2::exp(&(big_t.adjoint() * tau));
        prop_assert!(lhs.boxminus(&rhs).norm() < 1e-10);
    }

    #[test]
    fn boxplus_axioms(x in twist(), d in twist()) {
        let big_x = SE2::exp(&x);
        prop_assert!(big_x.boxplus(&Tangent2::zeros()).boxminus(&big_x).norm() < 1e-14);
        let y = big_x.boxplus(&d);
        prop_assert!((y.boxminus(&big_x) - d).norm() < 1e-12);
        prop_assert!(big_x.boxplus(&y.boxminus(&big_x)).boxminus(&y).norm() < 1e-12);
    }
}

/// log stays accurate right up to the injectivity boundary — the place where
/// a naive implementation quietly returns the wrong branch.
#[test]
fn log_is_stable_near_pi() {
    let mut rng = SmallRng::seed_from_u64(0xC0FFEE); // never thread_rng() in this book
    for _ in 0..1000 {
        let theta = PI - rng.random_range(1e-9..1e-6);
        let ours = SE2::new(0.3, -0.2, theta).log();
        assert_relative_eq!(ours[2], theta, epsilon = 1e-12);
        assert_relative_eq!(SE2::exp(&ours).log(), ours, epsilon = 1e-9);
    }
}

The three-dimensional case is delegated, not skipped. sophus provides $\SOthree$ and $\SEthree$ with the same exp/log/adjoint surface, nalgebra's UnitQuaternion handles the parameterization, and Chapter 18 uses both. The reason to hand-roll $\SEtwo$ and only $\SEtwo$ is that every structure in the general case is visible here, in two dimensions, where you can check the answer with a protractor.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Derive V and its inverse

   Evaluate $V(\omega) = \int_0^1 R(s\omega)\,ds$ entrywise to reproduce Derivation 4's closed form, then derive $V(\omega)^{-1}$ by inverting the $2\times2$ matrix directly. Show that both tend to $I$ as $\omega \to 0$, and compute the first two nonzero terms of the Taylor series of each entry — these are the coefficients the code branches to.

2. Foundation exerciseFDifficulty 2 of 3The adjoint is a homomorphism

   Prove the $\SEtwo$ adjoint formula of Derivation 6 by block computation, then show that $\Ad_{T_1T_2} = \Ad_{T_1}\Ad_{T_2}$ and $\Ad_{T^{-1}} = (\Ad_T)^{-1}$. Explain why the first identity says "changing frames twice is one change of frame", and why that is exactly what makes covariance propagation across a kinematic chain composable.

3. Foundation exerciseFDifficulty 3 of 3Right increments are body-frame quantities

   Let $x' = x \bplus \delta$ with $\delta = (\rho, \omega_\delta)$. Show that the world-frame displacement of the origin is $R_x V(\omega_\delta)\rho$, and hence $R_x\rho$ to first order. Conclude why wheel odometry naturally produces right increments, and derive the conversion $\delta_{\text{left}} = \Ad_x \delta_{\text{right}}$ to the left convention.

4. Conceptual exerciseCDifficulty 2 of 3Predict, then verify with the Exp/Log Lens

   Two poses differ by a pure $180°$ rotation with no translation. Predict, before touching the widget: where does the geodesic interpolation put the robot at $s = 0.5$, and why does $\log$ fail to pick a unique answer for this pair? Now drag the arrowhead in w3.2 to the origin so $v \approx 0$, and slide $\omega$ toward $\pm 3.1$. The slider deliberately stops short of $\pi$ — say why, in terms of Derivation 4, Step 5, and describe what happens to the arc and to the ICR as you approach the limit from either side.

5. Conceptual exerciseCDifficulty 1 of 3Squeeze the banana

   In w3.4, predict what happens to the cloud as $\sigma_\omega \to 0$ with $\sigma_v$ fixed, and what happens to the 2σ coverage readout. Verify. Then state, in one sentence, the condition under which the fitted ellipse is approximately honest — you have just described the bet the EKF makes in Chapter 7.

6. Practical exercisePDifficulty 2 of 3Implement SE2 and pass the suite

   Implement SE2 with exp, log, inverse, adjoint, and Mul, plus the Manifold<3> impl, so that every test in crates/pr-core/tests/geom.rs above passes. Then deliberately break the small-angle guard by deleting the Taylor branch, and find the value of $|\omega|$ below which exp_log_roundtrip starts failing. Report it, and name the subtraction that lost the digits.

7. Practical exercisePDifficulty 3 of 3SO3 under the same trait

   Implement SO3 as a newtype over nalgebra::UnitQuaternion<f64> with exp, log, and Manifold<3>. Handle $\theta \approx \pi$ with the numerically stable branch (the naive $\log$ divides by $\sin(\theta/2)$, which vanishes at $\theta = 0$, and loses precision as $\theta \to \pi$ from the other side). Property-test the round trip against sophus at a thousand random rotations including the near-$\pi$ band, and plot the round-trip error against $\theta$.

8. Practical exercisePDifficulty 2 of 3Make the frame bug uncompilable

   Take a working piece of code that composes transforms with bare SE2 — the widget's lidar chain is a fine target — and port it to Pose<A, B>. Introduce a realistic frame bug (compose the extrinsics in the wrong order) and show the compiler catching it. Then measure: does the newtype cost anything? Compare the generated assembly for Pose<World, Body> * Pose<Body, LidarF> with plain SE2 * SE2.

References

1. Lynch, K. M. and Park, F. C. (2017) Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press. ISBN 978-1107156302.link to Modern Robotics: Mechanics, Planning, and Control (opens in a new tab)

   Chapter 3 is this chapter's computational spine: planar rigid motions, angular velocity, exponential coordinates, twists, and screws. The epigraph is from its opening pages. Note the tangent ordering differs from ours — they write (ω, v).

2. Craig, J. J. (2005) Introduction to Robotics: Mechanics and Control. Pearson, 3rd edition. ISBN 978-0201543612.link to Introduction to Robotics: Mechanics and Control (opens in a new tab)

   The source of the leading super/subscript notation and the cancellation rule. Sections 2.2–2.7 are the clearest treatment of frames as a discipline anywhere; the Rust newtypes in this chapter are that discipline, mechanized.

3. Spong, M. W., Hutchinson, S., and Vidyasagar, M. (2020) Robot Modeling and Control. Wiley, 2nd edition. ISBN 978-1119523994.link to Robot Modeling and Control (opens in a new tab)

   Section 2.4 states the fixed-frame versus current-frame composition rule — pre- versus post-multiplication — which is exactly the two ghost frames in the Frame Composer, and the reason this book fixes the right convention.

4. Hertzberg, C., Wagner, R., Frese, U., and Schröder, L. (2013) Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds. Information Fusion 14(1), 57–77.doi:10.1016/j.inffus.2011.08.003 (opens in a new tab)

   Where ⊞ and ⊟ come from, with the axioms proved in Derivation 7. The paper's thesis is this chapter's thesis: encapsulate the manifold and every estimator downstream can be written as if the state were a vector.

5. Solà, J., Deray, J., and Atchuthan, D. (2018) A micro Lie theory for state estimation in robotics. arXiv:1812.01537.link to A micro Lie theory for state estimation in robotics (opens in a new tab)

   The modernization spine of this chapter: the ⊞/⊟ interface, the adjoint, and the Jacobian bookkeeping that Part II needs. This book follows its translation-first tangent ordering and its right-⊞ convention.

6. Mangelson, J. G., Ghaffari, M., Vasudevan, R., and Eustice, R. M. (2020) Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra. IEEE Transactions on Robotics 36(5), 1371–1388.doi:10.1109/TRO.2020.2994457 (opens in a new tab)

   The banana, taken seriously: what a concentrated Gaussian on SE(2)/SE(3) actually looks like, and why the independence assumption between poses in a graph is usually false. Chapter 9 builds on its compounding operations.

7. Potokar, E. R., Beard, R. W., and Mangelson, J. G. (2024) An Introduction to the Invariant Extended Kalman Filter [Lecture Notes]. IEEE Control Systems 44(6).doi:10.1109/MCS.2024.3466488 (opens in a new tab)

   The most readable recent account of why the choice between left and right ⊞ has consequences for convergence, not just for bookkeeping. Read it after Chapter 7.

8. Ge, Y., van Goor, P., and Mahony, R. (2025) The Geometry of Extended Kalman Filters on Manifolds with Affine Connection. arXiv:2506.05728.link to The Geometry of Extended Kalman Filters on Manifolds with Affine Connection (opens in a new tab)

   Where the field is now: the retraction of this chapter is one choice among many, and connection, parallel transport, and curvature all show up in the filter equations once you look. The frontier version of Chapter 7.