Rigid-body transforms (SE(3) poses)

Deep dive

How it works

In: poses (q, t), twists xi, dt, interpolation parameter s · Out: composed, inverted, integrated or interpolated pose; pose covariance in another frame · State: none (pose = 7 floats; the caller keeps the pose) · Cost: O(1), order of 30-50 flops per compose, invert or rotate

1. Convention. T_a_b = (R, t) maps b-coordinates to a-coordinates; t is the origin of b expressed in a. Compose T_a_c = T_a_b T_b_c; invert T_b_a = (R^T, -R^T t); relative pose T_a_b = T_w_a^-1 T_w_b.
2. Rotation as unit quaternion q = (w, u), w = cos(theta/2), u = axis*sin(theta/2). Product is the composition, rotating a vector is v + w t + u x t with t = 2 u x v (see quaternion rotations); the 4x4 matrix form is in homogeneous transforms.
3. Exp/log (SO(3)). Rotation vector w (axis*angle): Exp(w) = (cos(|w|/2), w/|w| sin(|w|/2)). Log inverts it; flip sign if q.w < 0 so the angle is in [0, pi]. Use a first-order series below |w| ~ 1e-8.
4. Exp (SE(3)) of twist (v, w): R = Exp(w), t = V v, V = I + (1-cos th)/th^2 [w]x + (th - sin th)/th^3 [w]x^2. Constant body-frame velocity traces a screw (a circular arc when v and w are perpendicular).
5. Integrate: T_{k+1} = T_k Exp(xi dt) for a body-frame twist (IMU, wheel odometry). For a world-frame twist multiply on the left.
6. Interpolate: slerp (q0 Exp(s Log(q0^-1 q1))) plus lerp. Not the constant-twist screw path, but what tf2 does.
7. Uncertainty: a pose covariance is over a 6-vector xi with T Exp(xi). Moving it to another frame uses the adjoint Ad_T = [[R, [t]x R], [0, R]] (ordering v then w): S_a = Ad S_b Ad^T.

world --T_w_base--> base --T_base_cam--> cam      p_w = T_w_base * T_base_cam * p_cam
T_cam_base = T_base_cam^-1   (cheap, closed form, never a general 4x4 solve)

Code

pub type V3 = [f64; 3];

fn cross(a: V3, b: V3) -> V3 {
    [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
}
fn dot(a: V3, b: V3) -> f64 { a[0] * b[0] + a[1] * b[1] + a[2] * b[2] }
fn add(a: V3, b: V3) -> V3 { [a[0] + b[0], a[1] + b[1], a[2] + b[2]] }
fn scale(a: V3, s: f64) -> V3 { [a[0] * s, a[1] * s, a[2] * s] }

/// Unit quaternion, Hamilton convention, w first. q and -q are the same rotation.
#[derive(Clone, Copy, Debug)]
pub struct Quat { pub w: f64, pub v: V3 }

impl Quat {
    pub const IDENTITY: Quat = Quat { w: 1.0, v: [0.0; 3] };
    pub fn mul(self, o: Quat) -> Quat {
        Quat { w: self.w * o.w - dot(self.v, o.v),
               v: add(add(scale(o.v, self.w), scale(self.v, o.w)), cross(self.v, o.v)) }
    }
    pub fn conj(self) -> Quat { Quat { w: self.w, v: scale(self.v, -1.0) } }
    pub fn normalize(self) -> Quat {
        let n = (self.w * self.w + dot(self.v, self.v)).sqrt();
        Quat { w: self.w / n, v: scale(self.v, 1.0 / n) }
    }
    /// v' = v + w t + u x t with t = 2 (u x v), u = vector part: two cross products, no matrix.
    pub fn rotate(self, p: V3) -> V3 {
        let t = scale(cross(self.v, p), 2.0);
        add(add(p, scale(t, self.w)), cross(self.v, t))
    }
    /// SO(3) exp: rotation vector (axis * angle) -> quaternion.
    pub fn exp(w: V3) -> Quat {
        let th = dot(w, w).sqrt();
        if th < 1e-8 { return Quat { w: 1.0, v: scale(w, 0.5) }.normalize(); }
        Quat { w: (th / 2.0).cos(), v: scale(w, (th / 2.0).sin() / th) }
    }
    /// SO(3) log: quaternion -> rotation vector with angle in [0, pi].
    pub fn log(self) -> V3 {
        let q = if self.w < 0.0 { Quat { w: -self.w, v: scale(self.v, -1.0) } } else { self };
        let s = dot(q.v, q.v).sqrt();
        if s < 1e-8 { return scale(q.v, 2.0); }
        scale(q.v, 2.0 * s.atan2(q.w) / s)
    }
    pub fn slerp(self, mut o: Quat, t: f64) -> Quat {
        if self.w * o.w + dot(self.v, o.v) < 0.0 { o = Quat { w: -o.w, v: scale(o.v, -1.0) }; } // short way
        // q0 * exp(t * log(q0^-1 q1)) is slerp, and stable near 0 angle
        self.mul(Quat::exp(scale(self.conj().mul(o).log(), t)))
    }
}

/// T_a_b: maps points expressed in frame b to frame a (pose of b in a).
#[derive(Clone, Copy, Debug)]
pub struct Pose { pub r: Quat, pub t: V3 }

impl Pose {
    pub const IDENTITY: Pose = Pose { r: Quat::IDENTITY, t: [0.0; 3] };
    pub fn act(&self, p: V3) -> V3 { add(self.r.rotate(p), self.t) }
    /// T_a_c = T_a_b * T_b_c  (frame labels cancel in the middle).
    pub fn mul(&self, o: &Pose) -> Pose {
        Pose { r: self.r.mul(o.r).normalize(), t: self.act(o.t) }
    }
    /// T_b_a = (R^T, -R^T t): no matrix inverse needed.
    pub fn inverse(&self) -> Pose {
        let rc = self.r.conj();
        Pose { r: rc, t: scale(rc.rotate(self.t), -1.0) }
    }
    /// Relative pose of `o` as seen from self: self^-1 * o.
    pub fn between(&self, o: &Pose) -> Pose { self.inverse().mul(o) }
    /// SE(3) exp of a twist (v, w): v is the translational part, w the rotation vector.
    pub fn exp(v: V3, w: V3) -> Pose {
        let th2 = dot(w, w);
        let (a, b) = if th2 < 1e-12 { (0.5, 1.0 / 6.0) } else {
            let th = th2.sqrt();
            ((1.0 - th.cos()) / th2, (th - th.sin()) / (th2 * th))
        };
        let wv = cross(w, v);
        // V v = v + a (w x v) + b (w x (w x v))
        let t = add(add(v, scale(wv, a)), scale(cross(w, wv), b));
        Pose { r: Quat::exp(w), t }
    }
    /// Move by a twist expressed in the body frame (what an IMU / wheel odometry gives).
    pub fn integrate(&self, v: V3, w: V3, dt: f64) -> Pose { self.mul(&Pose::exp(scale(v, dt), scale(w, dt))) }
    pub fn interpolate(&self, o: &Pose, s: f64) -> Pose {
        Pose { r: self.r.slerp(o.r, s), t: add(scale(self.t, 1.0 - s), scale(o.t, s)) }
    }
}

Asserted: inverse round trip, composition equals sequential application, a * between(a,b) == b, a twist (v=pi/2 along x, yaw rate pi/2 for 1 s) ends at (1, 1, 0), 1000 integrate steps agree with one exp, q and -q interpolate identically. Translation is lerp, not a screw path.

Complexity and constants

• Quaternion product: 16 mul + 12 add. Rotate a vector: two cross products, order of 30 flops (a 3x3 matrix: 9 mul + 6 add). Compose pose: product + rotate + add + renormalise, order of 50 flops. 3x3 matrix product: 27 mul + 18 add.
• Measured on an Apple M4 Pro (f64, scalar, dependent chain, so latency not throughput): compose with normalise 18.8 ns, inverse 11 ns, rotate+translate one point 14 ns.
• Measured batch of 1M points (f64): quaternion path 2.0 ns/point, same pose pre-converted to a 3x3 matrix 0.83 ns/point.
• Memory: f32 pose 28 B (pad to 32), 3x4 matrix 48 B, 4x4 matrix 64 B = one cache line.

Efficiency

• Bound: memory traffic over many points, and trig in exp/log at high rates.
• Pre-convert for batches: build the 3x3 matrix once per frame (2.4x faster per point than the quaternion in the measurement above).
• Collapse chains once: multiply fixed extrinsics into one pose per sensor, not per measurement; cache inverses used in loops.
• Avoid trig for small steps: Exp(w) ~ (1, w/2) normalised has relative error about |w|^2/24 (4e-6 at |w| = 1e-2).
• Layout: struct-of-arrays (x[], y[], z[]) lets SIMD transform several points per instruction.
• Cheaper alternative: ground robots use SE(2): three numbers, no renormalisation.

Implementation notes

• Renormalise every composition (one sqrt); re-orthonormalising a matrix costs far more, so keep quaternions as state.
• f32 caps accuracy by magnitude: at 65,536-131,072 m a float has about 8 mm resolution. Keep poses relative to a local origin (see floating-origin rebasing) or use f64 for world poses.
• Fix the component order once. Hamilton, scalar-last storage (x,y,z,w) is what ROS messages, glam and Eigen's coeffs() use; the Eigen and nalgebra constructors take w first. Convert at the boundary, never inside.
• Khronos Group, glTF 2.0 Specification, nodes and transforms. spec
• Khronos Group, OpenXR Specification, XrPosef. spec
• ROS REP-103: right-handed, body x forward, y left, z up, radians; geographic frames ENU.
• Optimisers (SLAM, bundle adjustment) update T <- T Exp(delta) with a 6-vector: minimal, singularity-free.
• Time-stamped transforms: interpolate between the two nearest samples, don't extrapolate; see transform tree.

Pitfalls

• Mixing T_a_b and T_b_a, the most common bug: T_a_b moves point coordinates from b to a, yet places frame b in frame a.
• Wrong multiplication side: body-frame twist on the right, world-frame on the left; covariance must use the matching perturbation side.
• Quaternion order wxyz vs xyzw between libraries and file formats; silent, gives plausible-looking wrong rotations.
• Euler angles: gimbal lock at pitch ±90 degrees and many axis-order conventions; use only at the UI boundary.
• Averaging quaternions component-wise without sign alignment (q and -q); averaging poses by averaging matrices.
• Unnormalised quaternion after deserialising or many products: rotation then scales the vector.
• Dividing by sin(theta) near zero in log/exp: use a series fallback.

Variants and alternatives

In production

ROS tf2 and geometry_msgs (translation + xyzw quaternion), Drake and Pinocchio for robot kinematics, GTSAM and manif for factor-graph and filter state, game engines for scene graphs. Other fields: graphics (a glTF node carries translation, unit-quaternion rotation and scale: spec); AR/VR (OpenXR poses are an orientation quaternion plus a position: XrPosef); aerial vehicles and vision (camera and vehicle poses in pose graphs, see SLAM).

Implementations

Sources

• Richard M. Murray, Zexiang Li, S. Shankar Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press 1994 (ch. 2, rigid motion, twists, exponential coordinates). pdf
• Joan Sola, Jeremie Deray, Dinesh Atchuthan, A micro Lie theory for state estimation in robotics, 2018. arXiv · code
• Ken Shoemake, Animating rotation with quaternion curves, SIGGRAPH 1985. paper
• ROS REP-103, Standard Units of Measure and Coordinate Conventions. spec