Coordinate Frames &
Rigid-Body Math

Chapter 1: SE(3) and SO(3) — Rebuilding the 4×4 From Two Requirements

"Where does the homogeneous transform come from?" is the right first question for this whole subject, and it is not a memory question. The goal is to produce the 4×4 as a consequence of two requirements — not to draw it from memory and then be unable to say why the bottom row is what it is.

Here is the derivation, built so that every step can defend itself.

CONCEPT — two requirements, one matrix

Requirement one: rigid means distances do not change. A rigid body is a set of points whose mutual distances are fixed. So whatever matrix R represents a rotation, it must preserve the length of every vector:

Square both sides and write the norm as a dot product: (Rv)^⊤(Rv) = v^⊤v, so v^⊤(R^⊤R)v = v^⊤v for every v. Call M = R^⊤R. The claim is that M must be the identity. This is the step most write-ups hand-wave — "same quadratic form, therefore same matrix" — and it is exactly the step worth doing properly. Do it in two moves.

Move 1 — M is symmetric. Transpose it, using (AB)^⊤ = B^⊤A^⊤ and (A^⊤)^⊤ = A:

Symmetry is not decoration here; it is what makes the whole argument legal. A quadratic form v^⊤Mv only ever sees the symmetric part of M, because any antisymmetric part contributes v^⊤Av = 0 for every v. So if M could carry an antisymmetric component, knowing the form everywhere would not pin the matrix down and the argument would prove nothing. M is symmetric, so the form determines it completely.

Move 2 — read the entries straight off the form. We know v^⊤Mv = v^⊤v for every v, so choose vs that expose one entry at a time.

• Set v = e_i, the i-th basis vector (a 1 in slot i, zeros elsewhere). Then v^⊤Mv picks out a single entry, M_ii, and v^⊤v = 1. So M_ii = 1 for i = 1, 2, 3 — every diagonal entry is 1.
• Now set v = e_i + e_j with i ≠ j. Expanding the form gives four terms: v^⊤Mv = M_ii + M_jj + M_ij + M_ji. Symmetry says M_ij = M_ji, so the middle pair collapses to 2M_ij — that is where Move 1 gets spent. Meanwhile v^⊤v = 1 + 1 = 2. So 1 + 1 + 2M_ij = 2, and M_ij = 0.

Every diagonal entry is 1 and every off-diagonal entry is 0. That is the identity matrix, recovered entry by entry from the constraint — not asserted, and not borrowed from a theorem you would have to name under pressure:

That is the definition of an orthogonal matrix, and the set of them is the orthogonal group O(3). You just got the inverse for free — a transpose, no division, no solve. That is the practical payoff of the constraint.

Requirement two: no mirrors. Take determinants of R^⊤R = I: det(R^⊤)det(R) = 1, and det(R^⊤) = det(R), so det(R)² = 1 and det(R) = ±1. The −1 branch is a reflection — it preserves lengths but turns a right hand into a left hand. A physical rigid body cannot do that. Throwing that branch away leaves the special orthogonal group SO(3):

How many numbers is a rotation really? Count: R has 9 entries. The constraint R^⊤R = I is a 3×3 matrix equation, but R^⊤R is symmetric, so it only carries 6 independent scalar equations — three saying each column is a unit vector, three saying each pair of columns is perpendicular. So 9 − 6 = 3 degrees of freedom. Every rotation representation is an attempt to package those 3 numbers, and Chapter 3 is about the fact that none of them does it perfectly.

Do not say "six" without being able to list them. The (i, j) entry of R^⊤R is exactly c_i·c_j, the dot product of columns i and j of R. Nine entries, but the matrix is symmetric, so the lower triangle repeats the upper: only the six on-or-above the diagonal are independent. Here they are, each mapped to the column pair it constrains:

Three "no stretch" equations, three "no shear" equations. Together they say the transform carries an orthonormal triad to another orthonormal triad, which is what "rigid" means in coordinates. The three redundant equations — (2,1), (3,1), (3,2) — are not extra information; they are the same three perpendicularity statements written a second time, which is precisely why the count is 6 and not 9. Get that count wrong and your DOF answer comes out as 0 (if you say 9 constraints) or 6 (if you forget the constraints entirely), and both are visibly absurd for a rotation.

Now translation. Rotation is linear, so a matrix handles it. Translation is not: p → p + t sends 0 to t, and every linear map sends 0 to 0. You cannot write a translation as a 3×3 matrix. Two options: carry the pair (R, t) around and remember the rule, or find a bigger space where translation is linear.

The trick is homogeneous coordinates: embed the 3D point p = (x, y, z) as the 4-vector (x, y, z, 1). Now look at what this block matrix does:

The rotation and the translation both happen, in one matrix multiply, and the fourth component comes out as 1 again — so the result is a valid homogeneous point and you can multiply again. That last fact is the whole reason for the bottom row.

Put it together. The special Euclidean group SE(3) is the set of those block matrices, and it is a group: closed under multiplication, has an identity, every element has an inverse. Six degrees of freedom — three of rotation, three of translation.

DESIGN — the transform tree is a system, and it has a bill

Deriving the 4×4 gets you to the second question, which is where the engineering actually starts: where do these live on a real robot, and what do they cost?

On a ROS 2 humanoid, poses live in a tf tree: a directed tree of frames where every non-root frame has exactly one parent and one transform to it. Two channels feed it:

Size the buffer, out loud — and split the frame count first. The table above just said static frames are published once and latched, so they store one sample each, not 2,000. Multiplying all 40 frames by 200 Hz is the exact sloppiness this section is warning you about, and anyone who has actually sized a buffer will catch it. Take a humanoid with 40 frames, of which say 12 are static (both camera links, the IMU mount, the LiDAR mount, the bumper and tool frames) and 28 are dynamic (the joints, odom→base_link, map→odom), with the tf2 default 10-second buffer:

Now the bytes, counted honestly. A stored entry is a translation (3 doubles) plus a quaternion (4 doubles) = 7 doubles = 56 bytes of pose, plus the timestamp, which is another 8 bytes — so 64 bytes of payload, not 56. Around that sits the deque node and the frame-id bookkeeping, which takes the real per-entry cost to roughly 100 bytes:

Ten nodes each running their own buffer is 56 MB, which is how a Jetson runs out of memory in a way nobody expected — and why the fix is a shared buffer or a shorter horizon (drop to 2 s and you are at 1.1 MB per node), not more RAM. Note also what the split bought you: the naive 40-frame figure overstates the buffer by 43%, which in a design review is the difference between "fits" and "does not".

Size the wire, out loud. Only the dynamic frames ride /tf, so one message carrying 28 TransformStamped entries is about 28 × 100 B = 2.8 kB. At 200 Hz that is 560 kB/s ≈ 4.5 Mbit/s of pure transform traffic, multiplied by every subscriber if the transport is not shared. On a 100 Mbit link that is 4.5% of the pipe doing nothing but bookkeeping — and it is why teams split the fast joint broadcaster from the slow odometry broadcaster and drop the rate of anything that does not need 200 Hz. The 12 static frames cost 12 × 100 B = 1.2 kB once, which is the entire argument for latching them.

Size the lookup, out loud. Asking for camera_optical → base_link at time t does three things: a binary search in each link's sorted sample deque (2,000 samples at 200 Hz over 10 s, so about 11 comparisons), a slerp/lerp interpolation between the two bracketing samples, and a compose up and back down the tree. For an 8-link chain that is roughly 8 interpolations and 8 quaternion products — on the order of 2–5 µs. Cheap per call, and murderous if a 200 Hz control loop calls it 40 times per cycle without caching.

Data flow, with shapes, for one 200 Hz control cycle on a 7-DOF arm:

CODE — from scratch, then the library

Write this yourself at least once, because it is 15 lines and it exposes everything: whether you know the inverse, whether you get the composition order right, whether you handle the translation block.

python — from scratch
import numpy as np

def se3(R, t):
    """Pack a rotation and a translation into a 4x4."""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def se3_inv(T):
    """Inverse WITHOUT a linear solve: exploit R^-1 = R^T."""
    R, t = T[:3, :3], T[:3, 3]
    Ti = np.eye(4)
    Ti[:3, :3] = R.T
    Ti[:3, 3]  = -R.T @ t      # NOT -t. Chapter 2 explains why.
    return Ti

def apply_to_points(T, P):
    """P is (N, 3). Returns (N, 3). No 4-vector allocation."""
    return P @ T[:3, :3].T + T[:3, 3]

def is_valid(T, tol=1e-6):
    """The four checks worth running on any transform you did not build."""
    R = T[:3, :3]
    return (np.abs(R @ R.T - np.eye(3)).max() < tol      # orthogonal
            and abs(np.linalg.det(R) - 1.0) < tol           # right-handed
            and np.allclose(T[3], [0, 0, 0, 1], atol=tol)     # affine, not projective
            and np.isfinite(T).all())                       # no NaN from a bad solve

Two details that separate a clean implementation from a slow one. First, apply_to_points uses P @ R.T rather than building 4-vectors — for a 300,000-point LiDAR sweep that avoids allocating a 300,000×4 array and roughly halves the memory traffic. Second, se3_inv never calls np.linalg.inv. Inverting a general 4×4 is an LU factorisation, about 25× slower than a transpose and two products, and it silently produces a matrix that is not exactly in SE(3) when the input has drifted.

The production form. Nobody ships hand-rolled SE(3) in a control loop, so know the ecosystem:

DEBUG — the rotation that stops being a rotation

Failure mode: accumulated rotation drift. A node integrates gyro increments by repeatedly multiplying its stored rotation matrix: R = R @ dR, 200 times a second, for a whole shift. Nothing crashes. Nothing looks wrong for the first hour.

Every product carries floating-point rounding, and there is nothing in the multiply that pushes the result back onto SO(3). The matrix slowly stops being orthogonal — its columns stop being unit length and stop being perpendicular. Geometrically, your "rotation" acquires a scale and a shear.

Observable symptom: a rigid object in the map slowly grows or skews. Two LiDAR sweeps of the same wall, taken an hour apart, no longer have the same length. Grasps get worse over a shift and are fixed by restarting the node — which is the tell everyone misreads as "a memory leak".

The metric that reveals it, and you should be publishing it as a diagnostic:

Put numbers on it. Single precision has a machine epsilon of about 1.2×10⁻⁷. Rounding errors accumulate like a random walk, so after N products the orthogonality error grows roughly as √N · ε_mach. Over an 8-hour shift at 200 Hz, N = 200 × 3600 × 8 = 5.76×10⁶, and √N = 2400:

That is a 0.029% scale-and-skew error. At a 1.5 m arm reach it is 1.5 × 2.9×10⁻⁴ = 0.43 mm — small, but it is a monotonically growing bias, and it stacks on top of everything else. Redo the same sum in double precision (ε_mach = 2.2×10⁻¹⁶): 2400 × 2.2×10⁻¹⁶ = 5.3×10⁻¹³, which is 0.8 nanometres at the same reach. Precision choice is the fix, not a detail.

The second failure mode, same family: the silent det = −1. A calibration solver returns a matrix from an unconstrained least-squares fit. It is orthogonal to 10⁻⁹, so the usual check passes. Its determinant is −1. Symptom: the scene is mirrored — the robot consistently reaches to the wrong side, and the error is not small, it is a reflection. Metric: np.linalg.det(R), one line, and it should be in every transform validator you write. Fix: enforce det = +1 inside the solver by flipping the sign of the last column of V in the SVD when the determinant comes out negative — the standard Kabsch/Umeyama correction.

FRONTIER — from matrices to manifolds

The 4×4 is not going anywhere — it is the interchange format. What has changed in the last decade is how estimators optimise over it.

The problem: SE(3) is a curved 6-dimensional manifold sitting inside a 16-dimensional matrix space. Gradient descent on the 16 entries wanders off the manifold immediately. The old workaround was to parameterise with Euler angles or a constrained quaternion and accept the singularities and the Lagrange multipliers. The modern answer is to keep the state on the manifold and take the increment in the tangent space, using the exponential map to get back:

Three references worth knowing by title:

• Solà, Deray & Atchuthan, "A micro Lie theory for state estimation in robotics" (2018, revised 2021). The paper that made this vocabulary standard on robotics teams. If you name one reference for on-manifold estimation, name this one — it is also the theory behind the manif library.
• Teed & Deng, "Tangent Space Backpropagation for 3D Transformation Groups" (CVPR 2021) — the LieTorch library. This is the frontier answer: it makes SE(3) a differentiable type inside PyTorch, computing gradients in the tangent space rather than through a matrix parameterisation. It is what lets DROID-SLAM backpropagate through a bundle-adjustment layer, and it is the reason "learned SLAM" stopped meaning "regress a pose vector".
• Deray & Solà, "Manif: a micro Lie theory library for state estimation in robotics applications" (JOSS, 2020) — the practical C++ answer, with analytic Jacobians returned alongside every operation instead of being finite-differenced.

Chapter 2: Composition Order and Inverses — Where Most Frame Bugs Live

It is Friday afternoon and you have just shipped a calibration node. All week the arm grasped correctly on the bench: you jogged the torso a few degrees each way, the gripper closed on the part, the reprojection residual sat at 0.4 px. On Monday the cell runs its real cycle — a 90° torso turn to reach the far bin — and the gripper closes on empty air, 0.7 m from the part.

Nothing in the log is red. Every node is publishing at its rated rate. The tf tree is connected end to end, the calibration residual is still 0.4 px, and the joint encoders agree with the commanded angles to a hundredth of a degree. There is no error to grep for, because from the software's point of view nothing went wrong.

The cause is one of exactly two one-character mistakes, and this chapter is about both: multiplying two transforms in the wrong order, and inverting one incorrectly. Between them they account for the majority of frame bugs that reach production robots.

Both survive code review, because both lines look entirely reasonable on the page. Both are invisible in the unit test, because unit tests use small angles and short offsets — exactly the regime where the wrong answer is nearly the right answer. By the end of this chapter you will derive that 0.7 m from first principles, you will know why the bench test could never have caught it, and you will know the ten-second command that would have.

CONCEPT — the subscript rule, and why order is physical

Composition is matrix multiplication, and matrix multiplication is not commutative. That is not a mathematical inconvenience; it is a statement about the world. Rotating a book 90° about the vertical axis and then 90° about the axis pointing at you does not land in the same place as doing it the other way round. Try it with an actual book — a physical object settles the argument faster than any diagram.

The bookkeeping device that makes order errors impossible is the subscript-cancellation rule:

Write every transform with both frames on it, always. Then check that adjacent subscripts touch. T_world_base @ T_base_grip has base next to base: valid, and the result is T_world_grip. T_base_grip @ T_world_base has grip next to world: invalid, and the compiler will not tell you, but the rule will.

Reading a chain out loud. A three-link chain composes right to left in the algebra and, confusingly, that is also the physical order of application to a point:

Apply that to a point p expressed in the tool frame. The rightmost matrix hits p first, lifting it into the torso frame; then into the base; then into the world. So the matrix nearest the point is the transform nearest the point in the physical chain. That is the sanity check: the innermost matrix is the innermost frame.

Before the arithmetic: how to score a rotation error with one number. Two rotations differ by a third rotation. If the right answer is R₁ and you shipped R₂, the residual is R_rel = R₁^⊤R₂ — the rotation you would have to apply to undo your mistake. The honest scalar for "how wrong is that" is the rotation angle of R_rel: the single angle that Euler's theorem says every rotation is. You do not need to extract an axis to get it. The trace hands it to you — and "where does that come from?" deserves an answer, so derive it rather than reciting it.

Rodrigues' formula writes any rotation by angle θ about a unit axis n as a three-term sum:

Take the trace of both sides. Trace is linear, so it distributes over the three terms, and three small facts finish it:

• tr(I) = 3. Three ones on the diagonal.
• tr(K) = 0. A skew matrix has a zero diagonal by construction: K = [[0, −n₃, n₂], [n₃, 0, −n₁], [−n₂, n₁, 0]]. So the whole sinθ term contributes nothing to the trace — which is exactly why the trace cannot tell you the sign of the rotation, only its magnitude.
• tr(K²) = −2. Because K² = nn^⊤ − |n|²I, and n is a unit vector, so K² = nn^⊤ − I. Then tr(nn^⊤) = n·n = 1 and tr(I) = 3, giving 1 − 3 = −2.

Substituting all three:

and solving for θ gives the identity used throughout this chapter and the rest of the lesson:

Check it on two rotations you already know. R = I has trace 3, so θ = arccos((3−1)/2) = arccos(1) = 0° ✓. A 180° rotation about z is diag(−1, −1, +1), trace −1, so θ = arccos(−1) = 180° ✓. And the maximum possible trace is 3 and the minimum is −1, which is the algebraic statement that no rotation is ever more than 180° from another one.

So order errors at large angles are catastrophic and obvious. The dangerous regime is the opposite one — and to say anything quantitative about it we need to know how badly two small rotations fail to commute. That is a derivation, not a lookup, and it is the load-bearing step of this chapter.

Step 1 — multiply two exponentials by hand. Write each small rotation as a matrix exponential, R = exp(A) where A is the skew matrix of the rotation vector. Expand each exponential to second order — we only care about terms up to quadratic, because that is where the difference will live:

Multiply the two series and keep every term of total degree 2 or lower:

Step 2 — ask what single exponential this equals. Guess that the product is exp(C) for some C = A + B + D, where D is itself second order (so D² is fourth order and can be dropped). Expanding that guess:

Step 3 — subtract. The I, A, B, ½A² and ½B² terms are identical on both sides and cancel. What is left is AB on one side against D + ½AB + ½BA on the other:

That is the second-order Baker–Campbell–Hausdorff formula, derived rather than quoted. Read what it says physically: the only thing distinguishing the two orders is the commutator. Swap A and B and every term is unchanged except [A, B], which flips sign. So the discrepancy between the two orders is the full commutator [A, B], and its half-size correction is what each order carries.

Step 4 — the commutator of two skew matrices is a cross product. This is the step that turns algebra into geometry, and it is the one most people cannot reproduce. Claim: for any two 3-vectors u and v, [u^∧, v^∧] = (u × v)^∧. Prove it by hand on the cleanest possible case, a = ẑ and b = x̂.

Write both skew matrices out. Using K = [[0, −n₃, n₂], [n₃, 0, −n₁], [−n₂, n₁, 0]]:

Compute AB, all nine entries. Rows of A dotted against columns of B, where the columns of B are (0,0,0), (0,0,1) and (0,−1,0):

• Row 1 of A = (0,−1,0):   ·(0,0,0) = 0  |  ·(0,0,1) = 0  |  ·(0,−1,0) = +1
• Row 2 of A = (1,0,0):   ·(0,0,0) = 0  |  ·(0,0,1) = 0  |  ·(0,−1,0) = 0
• Row 3 of A = (0,0,0):   0  |  0  |  0

Now BA. The columns of A are (0,1,0), (−1,0,0) and (0,0,0) — note that third column is all zeros, which is where most hand-calculations of this go wrong:

• Row 1 of B = (0,0,0):   0  |  0  |  0
• Row 2 of B = (0,0,−1):   ·(0,1,0) = 0  |  ·(−1,0,0) = 0  |  ·(0,0,0) = 0
• Row 3 of B = (0,1,0):   ·(0,1,0) = +1  |  ·(−1,0,0) = 0  |  ·(0,0,0) = 0

Subtract entry by entry:

Is that the skew matrix of something? Match it against the template: the (1,3) entry is n₂, so n₂ = 1; the (3,1) entry is −n₂ = −1 ✓; the (1,2) entry is −n₃ = 0 and the (2,3) entry is −n₁ = 0, so n₁ = n₃ = 0. It is exactly (0, 1, 0)^∧ = ŷ^∧. And the cross product? ẑ × x̂ = ŷ. The commutator of the two skew matrices is the skew matrix of the cross product of their axes — verified, by hand, entry by entry.

Put the pieces together. Two rotations by α about axis a and β about axis b, taken in the two orders, differ by the full commutator [αa^∧, βb^∧] = (αa × βb)^∧. The rotation angle of a small rotation is the magnitude of its rotation vector, so the order error is |αa × βb| = αβ sinφ, with φ the angle between the axes. Now the formula in the next box is earned.

Two points do not make a curve. Worked example 2 asserts a quadratic law from a 5° sample and a 90° sample. That is a claim you should be able to defend across the whole range, so here is the exact order error — angle of (AB)^⊤(BA), computed from the trace identity — for R_z(α) against R_x(α), tabulated against the small-angle prediction α²:

Read the table in three passes. First the growth column: the quadratic law is not decoration. Going 1° → 5° multiplies the angle by 5 and the error by 24.9 — call it 25, which is 5². Going 5° → 15° multiplies by 3 and the error by 8.95, which is 3² = 9 to within half a percent. Halving both angles quarters the error. That is the whole argument for why a gentle bench test is worthless as a detector: cut your test motion in half to be "safe" and you cut your detection signal by four. The widget below prints this ratio live — drag both sliders to 5° and it reads 4.00×.

Second, the "exact ÷ pred" column: the small-angle prediction is exact to four digits below 5°, still good to half a percent at 15°, and starts breaking down past 45° where the neglected third-order BCH terms bite. It under-predicts nothing — it over-predicts, saturating at the geometric ceiling of 120°, because a rotation error can never exceed 180° and the residual axis itself rotates as the angles grow.

Third, the practical reading. Suppose your acceptance test is "grasp error under 5 mm at a 0.5 m tool offset", which is a 0.57° rotation tolerance. From the table that trips at roughly α = 5.7° of test motion — so a rig that jogs ±5° sits just under the threshold and passes. The test does not merely fail to catch the bug; it is calibrated, by accident, to sit exactly on the boundary where it cannot. The fix is not a tighter tolerance, it is a bigger motion: add one 90° sweep to the acceptance script and the error becomes 120°, which nothing can hide.

CONCEPT — deriving the inverse instead of memorising it

The inverse is the other half of the chapter, and it earns its place precisely because the wrong answer is so tempting. Do not recall it — derive it in three lines, every time.

We want the T′ with T T′ = I. Write both in block form and multiply:

For that to be the identity, two things must hold. The rotation block gives R R′ = I, so R′ = R⁻¹ = R^⊤. The translation block gives R t′ + t = 0, so t′ = −R⁻¹t = −R^⊤t. Done:

The general residual. If you use −t instead of −R^⊤t, then T · T_wrong⁻¹ is not the identity — its translation block is exactly (I − R)t. That is the same expression as Chapter 0's bug, which is not a coincidence: both are "a translation that was never rotated into the frame it is being used in". It is one disease with two presentations.

Confirm that on the numbers you just computed. R_z(30°)t = (0.86603×2 − 0.5×1,  0.5×2 + 0.86603×1,  0) = (1.23205, 1.86603, 0), so (I − R)t = (2 − 1.23205,  1 − 1.86603,  0) = (0.76795, −0.86603, 0), whose norm is √(0.58975 + 0.75001) = 1.15747 m. Same 1.157 m as the gap measured in the child frame — which it must be, because the two vectors differ only by a rotation, and rotations preserve length. Two independent routes to the same number is the check.

CONCEPT — composing the whole transform, translations included

Worked examples 1 and 2 kept the translations at zero so the rotation story stayed clean. Real chains never do. Composing two full SE(3) elements in block form gives

and the whole lesson of this chapter lives in that top-right block. The translations do not simply add. The child's offset t₂ is written in the child's axes, so it has to be rotated into the parent's axes by R₁ before the parent's own offset is added to it. Get that wrong — write t1 + t2 — and you have made Chapter 0's mistake again, in a third costume.

CONCEPT — active vs passive, the other order confusion

There is a second reason people get order wrong, and it is not about matrices at all. The same matrix R can mean two different things:

The 2θ claim, proved in one line. Suppose the correct passive matrix is R^⊤ and you shipped R. Score it the way we score everything in this chapter: the residual is (correct)⁻¹(shipped). The inverse of R^⊤ is R, so

and because both factors are rotations about the same fixed axis n, they commute and their angles simply add: R(θ)R(θ) = R(2θ). (Check it with the trace identity if you like: for R = R_z(30°), R² = R_z(60°) has trace 1 + 2cos60° = 2, giving arccos(0.5) = 60° = 2×30° ✓.) So the residual is not "some rotation" — it is exactly double the true angle, about exactly the true axis.

That precision is the diagnostic. A random sign error, a wrong axis convention, or a bad calibration gives you an error with no particular relationship to the mounting angle. An active/passive mix-up gives you an error that is 2θ to within your measurement noise, on the same axis. So when a technician reports "the camera thinks the pallet is at 60° and it is really at 30°", you do not go looking for a calibration problem — you go looking for a missing transpose, and you find it in one grep. If instead the error were θ itself, or 90° − θ, or an axis swap, it would be a different bug entirely.

They are inverses of each other, which is why mixing them produces an error of exactly 2θ rather than something random. If your calibration is off by precisely twice the mounting angle, this is your bug. In the T_A←B notation the ambiguity disappears — T_A←B is always the passive reading, and the active reading is "T_A←B is where the body ends up if it starts at A's origin and moves by that pose" — but you have to pick one convention and put it in the header comment.

DESIGN — the API that gets the direction wrong for you

Here is where the chapter turns from algebra to systems. In ROS, the transform library already handles composition and inversion — and the number one source of production frame bugs is not doing the math wrong, it is calling the API with the arguments in the wrong order.

The cost model for a chain. Composing two SE(3) matrices is a 3×3 product (27 multiply–adds) plus a matrix–vector product (9) plus 3 adds — about 39 flops. Composing two (q, t) pairs is a quaternion product (16 multiplies, 12 adds) plus a quaternion rotation of a vector (about 30 flops) — roughly 60 flops but 56 bytes instead of 128. On an 8-link chain either is under a microsecond, so the choice is driven by memory and drift, not speed.

The lookup you should not do 40 times per cycle. A 200 Hz controller that calls lookupTransform once per link per cycle spends 40 × 3 µs = 120 µs per cycle, which is 2.4% of a 5 ms budget on lookups alone — and each call takes the buffer mutex, so it also serialises against the tf listener thread. The fix is to look up the one transform you need at the top of the cycle and pass it down. Being able to say "and it takes the buffer lock" is what makes this a design answer rather than a micro-optimisation.

CODE — a chain, a fold, and a guard

python — from scratch
import numpy as np
from functools import reduce

def se3_inv(T):
    """Inverse of a 4x4 rigid transform, derived not recalled.
    [[R, t], [0, 1]]^-1 == [[R.T, -R.T @ t], [0, 1]]
    NOT T.T  (that dumps t into the bottom row -- not even affine)
    NOT -t   (t lives in the PARENT axes; the inverse needs it in the CHILD's)"""
    R, t = T[:3, :3], T[:3, 3]
    out = np.eye(4)
    out[:3, :3] = R.T          # orthogonal => transpose IS the inverse
    out[:3, 3]  = -R.T @ t     # <-- the one line people get wrong
    return out

def chain(*transforms):
    """chain(T_w_b, T_b_t, T_t_g) -> T_w_g.  Left to right, subscripts cancel."""
    return reduce(lambda a, b: a @ b, transforms, np.eye(4))

def relative(T_w_a, T_w_b):
    """Pose of b as seen from a, when both are known in the world.
    T_a_b = inv(T_w_a) @ T_w_b   -- the TARGET side is the one inverted."""
    return se3_inv(T_w_a) @ T_w_b

def chain_checked(links):
    """links: [(('world','base'), T), (('base','tool'), T), ...]
    Refuses to compose a chain whose subscripts do not cancel."""
    (a, b), T = links[0]
    for (c, d), U in links[1:]:
        if c != b:
            raise ValueError(f"cannot compose {a}<-{b} with {c}<-{d}")
        T, b = T @ U, d
    return (a, b), T

chain_checked is 8 lines and it is the difference between a bug that costs two days and an exception on the first run. In C++ the same idea with template parameters costs zero runtime and fails at compile time; in Rust the newtype pattern does it too. When a postmortem asks "how do we prevent this from happening again?", this is a much better answer than "more careful code review".

The test that would have caught the Friday deploy. A test that only asserts the right answer is half a test — it passes just as happily against an implementation that is right for the wrong reason. Assert both directions: the correct inverse must round-trip to the identity, and the tempting wrong one must not. Six lines, and it pins the exact number from Worked example 3 so a future refactor cannot quietly regress it.

python — the six-line guard
def Rz(th):
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

T = np.eye(4)
T[:3, :3] = Rz(np.deg2rad(30.0))
T[:3, 3]  = [2.0, 1.0, 0.0]        # Worked example 3, exactly

def se3_inv_wrong(T):                # the -t version, kept for contrast
    out = np.eye(4); out[:3, :3] = T[:3, :3].T; out[:3, 3] = -T[:3, 3]
    return out

# leg 1 -- the right one round-trips to machine precision
assert np.allclose(T @ se3_inv(T), np.eye(4), atol=1e-12)
# leg 2 -- the tempting one MUST fail, or the test proves nothing
resid = T @ se3_inv_wrong(T)
assert not np.allclose(resid, np.eye(4), atol=1e-12)
# leg 3 -- pin the magnitude so a refactor cannot silently shrink it
err = np.linalg.norm(resid[:3, 3])          # this is |(I - R) t|
assert abs(err - 1.157) < 1e-3, err
print(f"-t inverse leaves a {err:.3f} m residual")   # -> 1.157 m

Three things worth understanding about that test. One: the second assert is the one that matters — a test suite full of positive assertions cannot distinguish "my code is correct" from "my code and my reference are wrong the same way". Two: the tolerance is 1e-12, not 1e-6, because an exact algebraic identity in float64 should close to about 10⁻¹⁶; a loose tolerance here would let a genuinely broken inverse through. Three: the residual translation is (I − R)t, so the test degenerates when R is near identity — which is exactly why you pin a 30° rotation and not the 1° one a lazy fixture would use. That last sentence is the same lesson as the order-error table, arriving through the code instead of the algebra.

python — the production form
# ROS 2. Note the argument order: TARGET first, SOURCE second.
T = buf.lookup_transform('base_link', 'lidar', stamp, timeout=Duration(seconds=0.05))
p_base = tf2_geometry_msgs.do_transform_point(p_lidar_stamped, T)

# Sophus / manif (C++): composition and inverse are operators, and the
# library keeps the result on the manifold for you.
#   Sophus::SE3d T_w_g = T_w_b * T_b_g;
#   Sophus::SE3d T_g_w = T_w_g.inverse();          // R^T and -R^T t
#   Eigen::Vector3d p_w = T_w_g * p_g;             // no 4-vector needed

DEBUG — the reversed chain, and how to catch it in one command

Failure mode: a reversed composition inside a calibration publisher. A team writes a node that fuses two extrinsic calibrations to produce lidar → camera from base → lidar and base → camera. Someone writes T_lidar_cam = T_base_lidar @ T_base_cam instead of se3_inv(T_base_lidar) @ T_base_cam.

Observable symptom: the projected LiDAR points land on the image, but rotated and offset — the wall is there, it is just leaning. Detection recall drops from 94% to 71% and everyone blames the detector. Nobody blames the transform, because the point cloud looks plausible in RViz.

The metric that reveals it, and it takes ten seconds. A valid relative transform between two rigidly mounted sensors has a translation whose magnitude equals the physically measured baseline. Print it:

With the reversed composition the translation becomes R_base,lidart_base,cam + t_base,lidar, whose magnitude is generally close to the sum of the two mount distances rather than their difference. If the LiDAR is 0.35 m from base and the camera is 0.42 m from base and they are 0.15 m apart on the rig, a reported 0.71 m baseline is not a subtle discrepancy — it is a shout. Every extrinsic your system computes should be compared against a tape measure on first publish. One assertion, one time.

Second failure mode: extrapolation, which looks exactly like a composition bug. lookupTransform with a timestamp newer than the last published sample either throws or, with some settings, extrapolates. A torso moving at 60°/s with a 180 ms stale transform gives 60 × 0.18 = 10.8° of error — and because it depends on how fast the joint is moving, it produces an error that looks configuration-dependent, which is the same signature as a rotation bug. Discriminator: a composition bug is deterministic and repeatable at a given configuration; an extrapolation bug varies with joint velocity. Hold the configuration still and command the same grasp: if the error vanishes, it is timing, not algebra. That is Lesson 2's territory.

FRONTIER — composing increments instead of poses

Composition order is settled mathematics, but where the composition happens has moved. Two developments worth naming:

• Forster, Carlone, Dellaert & Scaramuzza, "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry" (IEEE T-RO, 2017). Classical VIO composed hundreds of IMU increments into a pose every time the optimiser changed its estimate of the initial state — recomposing the whole chain per iteration. Preintegration composes the increments once, in a frame that does not depend on the state being estimated, and carries a correction Jacobian. It is a composition-order insight that bought a 10–50× speedup and is now in essentially every VIO system shipped.
• The adjoint as an engineering tool. When you move a velocity or a wrench between frames, the rule is not T v — it is Ad_T v, a 6×6 built from R and the skew of t. Teams that get this wrong publish end-effector velocities that are correct in direction and wrong in magnitude by the moment arm. VNAV 02 — Lie Groups derives it; the point to carry is that poses, velocities and wrenches transform by three different rules, and you have to name which is which.

Three objects, three rules — write them down and keep them. Given T = T_A←B = [[R, t], [0, 1]]:

Chapter 3: Four Ways to Say the Same Rotation

A rotation is three numbers' worth of information. There are four common ways to package it, they all appear in the same codebase, and the conversions between them are where the sign errors live.

The question that matters in practice is never "what is a quaternion". It is which representation would you store, which would you compute in, and why are those different answers?

CONCEPT — why there are four, and why there cannot be a perfect one

Start with the fact that makes this chapter necessary. The set of rotations SO(3) is a curved, closed 3-dimensional space — it is not flat ℝ³. Specifically it is topologically the 3-sphere with antipodal points identified. This has a hard consequence:

Here is each one, what it costs, and what it breaks.

1. The rotation matrix R. Nine numbers, six constraints, three free. No singularities anywhere — it is the only representation that is globally clean. Rotating a vector is one matrix–vector product. The cost is memory (72 bytes in double precision) and the fact that six constraints are six things to drift away from, as Chapter 1's failure mode showed.

2. Axis–angle, and its cousin the rotation vector. Euler's rotation theorem says any rotation is a single rotation by θ about a single unit axis n. Store them separately as (n, θ) — four numbers, one constraint — or multiply them into one 3-vector ω = θn, the rotation vector, which is exactly three numbers. Rodrigues' formula turns it into a matrix:

where n^∧ is the skew-symmetric matrix of n. The derivation is in VNAV 02 — Lie Groups; here what matters is the failure: the axis is undefined at θ = 0 (you cannot normalise a zero vector), and the map wraps at θ = 2π. The rotation vector is exactly the Lie algebra element, which is why estimators use it for the increment — increments are small, so you are always far from the singularity.

3. Euler angles. Three sequential rotations about coordinate axes. Three numbers, no constraints, and human-readable — which is the entire reason they refuse to die. There are 24 conventions: 12 axis sequences (zyx, zyz, xyz, …) times two interpretations (intrinsic, where each rotation is about the already-rotated body axis, vs extrinsic, about the fixed world axes). The robotics default is intrinsic z–y–x, spoken as yaw–pitch–roll. The failure is gimbal lock at pitch = ±90°, and Chapter 4 is entirely about it.

4. The unit quaternion. Four numbers, one constraint (‖q‖ = 1), three free. Built directly from axis–angle:

The half-angle is not decoration — it is the whole design. Because everything is a half-angle, rotating a vector is a sandwich product q v q⁻¹ where the two halves each contribute θ/2. And because cos and sin of θ/2 are 2π-periodic while the rotation is not, q and −q are the same rotation. That is the double cover, and Chapter 4 will show you what it costs.

The matrix built from a quaternion — the conversion you will be asked to write:

Do not memorise those nine entries — derive them. There is a one-line closed form that produces every one of them, signs included, and it is the thing to write down first:

where v = (x, y, z) is the vector part and [v]_× is the skew-symmetric matrix of v — the matrix that satisfies [v]_×u = v × u for every u. Write it down explicitly, because this is the object that carries all six off-diagonal signs:

Three terms, three distinct jobs, and each has a name worth knowing:

• (w² − ‖v‖²) I — the isotropic part. It is what survives of a vector under the sandwich regardless of direction. Note it equals cosθ once you substitute the half-angles: w² − ‖v‖² = cos²(θ/2) − sin²(θ/2) = cosθ. That is Rodrigues' cosθ term.
• 2vv^⊤ — symmetric, rank 1, and it rebuilds the component along the axis. Its magnitude is 2sin²(θ/2) = 1 − cosθ times the axis outer product nn^⊤. That is Rodrigues' (1 − cosθ) term.
• 2w[v]_× — antisymmetric, and the only term that knows which way the rotation turns. Its magnitude is 2cos(θ/2)sin(θ/2) = sinθ. That is Rodrigues' sinθ term. Flip its sign and you have built the inverse rotation.

So the quaternion→matrix formula and Rodrigues' formula are the same three terms wearing different clothes, and the half-angle identities are the translation. Substituting the unit-norm constraint w² + x² + y² + z² = 1 collapses the diagonal to the familiar form — entry (1,1) is (w² − x² − y² − z²) + 2x² = w² + x² − y² − z² = (1 − y² − z²) − y² − z² = 1 − 2(y² + z²) ✓.

Now the rule for the off-diagonals, stated correctly. The w-term of entry (i, j) is 2w times entry (i, j) of [v]_× — nothing more. Because [v]_× is not a checkerboard (its entries cycle x → y → z → x), the signs do not simply alternate across the diagonal. All six, written out:

Two structural facts fall out of the decomposition that you will use later in this chapter:

• Every entry of R is quadratic in the quaternion components — each is a product of exactly two of w, x, y, z. That is exactly why the sign of q cancels: (−a)(−b) = ab. The double cover is not a convention, it is arithmetic.
• The antisymmetric part of R is the whole w-term: (R − R^⊤)/2 = 2w[v]_×. So R₃₂ − R₂₃ = 4wx, R₁₃ − R₃₁ = 4wy, R₂₁ − R₁₂ = 4wz. Those three differences are literally the first branch of the matrix→quaternion code you will write in the CODE section — the algorithm is not a magic trick, it is this identity divided by 4w.

CONCEPT — deriving the quaternion product instead of memorising it

Composition is the operation you will be asked to perform live, and the formula for it is usually handed to students as a fact. Do not accept it that way — the derivation is four lines, it explains where the dot and cross products come from, and it is the only way the Hamilton-vs-JPL split later in this chapter will make sense rather than being a thing you assert.

Everything comes from the single line Hamilton carved into Broom Bridge in Dublin in 1843:

Step 1 — recover the six basis products. From k² = −1 we get k⁻¹ = −k. Right-multiply ijk = −1 by k:

Left-multiply ijk = −1 by i⁻¹ = −i and you get jk = i; the same move on the other side gives ki = j. For the reversed products, start from jk = i and substitute k = ij: j(ij) = i, so (ji)j = i; right-multiply by j⁻¹ = −j and (ji)(−j²) = −ij, i.e. ji = −k. Identically, kj = −i and ik = −j.

Step 2 — expand the sixteen terms. Multiply out, keeping the order of every factor because this algebra is not commutative:

Step 3 — sort the sixteen terms by which basis element they land on.

Real part — four terms. The only products that land on the reals are w₁w₂ and the three squares:

The minus sign in front of the dot product is not a convention someone chose. It is i² = −1, three times.

i part — four terms. Two come from multiplying by a scalar, two from the mixed pair jk = i and kj = −i:

and y₁z₂ − z₁y₂ is exactly (v₁ × v₂)_x.

j part. w₁y₂ + w₂y₁ + z₁x₂(ki) + x₁z₂(ik) = w₁y₂ + w₂y₁ + (z₁x₂ − x₁z₂) — and that bracket is (v₁ × v₂)_y.

k part. w₁z₂ + w₂z₁ + x₁y₂(ij) + y₁x₂(ji) = w₁z₂ + w₂z₁ + (x₁y₂ − y₁x₂) — and that bracket is (v₁ × v₂)_z.

Step 4 — stack the three vector components back up. The three brackets are the three components of one cross product, so:

CONCEPT — the comparison table you should be able to redraw

DESIGN — the numbers that decide the representation

"Store quaternions, rotate with matrices" sounds like taste. It is not. It is forced by the rates, shapes and byte counts on a real robot — "which representation?" is really a question about whether you can produce those numbers. So produce them.

Take a concrete stack: a ground robot with an IMU-driven attitude filter and a 10 Hz spinning LiDAR, on ROS 2. Every boundary, annotated:

Two details from that table worth saying out loud. First, the message types are sensor_msgs/Imu, geometry_msgs/TransformStamped and sensor_msgs/PointCloud2; the IMU message is ≈330 B on the wire even though the orientation is only 32 B of it, because two 9×float64 covariance blocks dominate it — the quaternion is not what costs you. Second, 14.4 MB/s is per subscriber: three nodes that each take their own copy of the cloud will saturate a gigabit link on the point cloud alone.

Now cash in the flop counts from the comparison table. De-warping means rotating the cloud, and the table above says a matrix rotate is 15 flop per point while the quaternion sandwich is ~30. Convert both into time:

• Matrix path. 120,000 × 15 = 1.8 Mflop. Done as one (120000,3) @ (3,3) BLAS call it reads 1.44 MB and writes 1.44 MB, so it moves 2.88 MB for 1.8 Mflop — 0.63 flop per byte, which is not compute-bound by any margin, it is bandwidth-bound. At the ~12 GB/s a single core can stream, that is ≈0.24 ms. Against a 100 ms budget: 0.24%. (The 100k-point row in the table is the same story: 1.5 Mflop, 2.4 MB, ~0.2 ms.)
• Quaternion-sandwich path. 120,000 × 30 = 3.6 Mflop — only 2× the flops, which is why people wrongly expect it to cost 2× the time. It does not, because it never touches BLAS. v + w·t + u×t with t = 2(u×v) is five separate numpy kernels, each allocating and writing a full (120000,3) temporary. That is ~5 × 2.88 MB ≈ 14 MB of traffic instead of 2.88 MB, plus a silent float32 → float64 promotion the moment your (4,) float64 quaternion meets the float32 cloud, which doubles every one of those temporaries again. Measured cost lands around 1.6–2.0 ms — roughly 8× the matrix path for 2× the arithmetic.

But the sweep is not one rotation — and that is why you still store quaternions. A 10 Hz spinning LiDAR paints its 120,000 points across the full 100 ms. The robot moves during that time, so each point was captured in a slightly different frame; de-warping means applying the attitude at that point's capture time. The attitude arrives at 200 Hz, so a 100 ms sweep is bracketed by 20 attitude samples, one every 5 ms. Two ways to spend that:

Unpacking the two rows. A slerp costs three transcendental evaluations per call — one acos for the angle between the endpoints and two sin for the weights — so 120,000 of them is ~360k transcendentals, and even vectorised those run at roughly 20–30 ns each. The bracketed version instead does 20 slerps, 20 quaternion→matrix conversions (20 × ~30 flop = 600 flop total, which is free), and 20 BLAS calls of (6000,3) @ (3,3) — 6000 because 120,000 points spread over 20 brackets is 6000 points each.

Is 20 brackets accurate enough? Do the error bound rather than guessing. Within a 5 ms bracket, slerp is exact for constant angular rate, so the residual is second-order in the angular acceleration α: roughly ⅛αΔt². For an aggressive α = 10 rad/s² and Δt = 5 ms, that is ⅛ × 10 × (0.005)² = 3.1 × 10⁻⁵ rad = 31 µrad. At 30 m range: 0.9 mm of lateral error — comfortably below the LiDAR's own ~2 cm range noise. The 8–11 ms per-point slerp buys nothing measurable.

DESIGN — the component order that will bite you

This is the highest-value paragraph in the chapter, because it is a real bug that ships constantly and it takes ninety seconds to understand.

A quaternion has four components. Two orderings are in wide use, (w, x, y, z) scalar-first and (x, y, z, w) scalar-last, and the libraries on your robot disagree:

The second, deeper convention split: Hamilton vs JPL. Beyond component order, there are two algebraic conventions. Hamilton — used by Eigen, ROS, scipy and essentially all robotics — has ij = k. The JPL convention, common in aerospace and in a lot of filter literature, has ij = −k. That looks like a footnote. It is not, and the derivation above tells you exactly why in one line.

Trace it through the four-line derivation. Go back to Step 3. The real part w₁w₂ − v₁·v₂ came only from the four squares i² = j² = k² = −1, which JPL keeps — so the scalar part is unchanged. The terms w₁v₂ + w₂v₁ came from multiplying by a scalar, so they are unchanged too. The only thing that came from ij = k was the cross-product term. Flip ij = k to ij = −k and every one of those six mixed products flips with it:

Compare that with the expression flagged in the callout above — the Hamilton product with its arguments swapped. They are character for character the same thing, because swapping the arguments flips only the cross term. So:

That is the whole thing. It is not "JPL is weird"; it is that the antisymmetric term reversed, and a reversed antisymmetric term is a reversed composition order. Feed JPL bits to a Hamilton matrix builder and your kinematic chain multiplies backwards — which, as Chapter 2 showed, is a large, stable, silent error, not a crash.

How to detect it in a running system. Both conventions produce unit quaternions and both produce valid rotation matrices, so norm and orthogonality checks are blind — as Worked example 4 just showed. The one cheap discriminator: compose two known, non-commuting rotations at startup (a 90° yaw and a 90° roll will do) and check the result against a hard-coded reference. If your library returns the transpose of what you expect, you are on the other convention. Three lines in a unit test, run once, and it fires at build time instead of in the field.

The reference every practitioner cites is Solà, "Quaternion kinematics for the error-state Kalman filter" (2017), whose appendix tabulates both conventions side by side — including the fact that JPL is usually paired with scalar-last storage, so the two convention splits often arrive together and partially mask each other. Know that document by name; it is the thing people actually keep open on a second monitor.

CODE — the conversion nobody gets right first try

Quaternion to matrix is easy — nine quadratic entries, no branching, no failure case. Matrix to quaternion is the one that catches people, because the naive formula w = ½√(1 + tr R) is a subtraction of nearly equal numbers exactly where robots spend real time: near a half turn.

Why the trace shows up at all. Sum the three diagonal entries of R(q): tr R = 3 − 4(x² + y² + z²), and with the unit constraint x² + y² + z² = 1 − w² that is tr R = 4w² − 1, i.e. 1 + tr R = 4w². So ½√(1 + tr R) = |w|, and the trace formula is not a trick — it is that identity solved for w. It also tells you precisely when it dies: at θ = 180°, w = 0, so 1 + tr R = 0, and you are extracting a square root of zero and then dividing by it to get x, y, z.

Shepperd's method is the fix: the same identity exists for each of the four components (1 + R₀₀ − R₁₁ − R₂₂ = 4x², and cyclically for y and z), and at least one of w², x², y², z² is always ≥ ¼ because they sum to 1. Branch on the largest and the square root is never taken of a small number. Here is the whole thing — all four branches, because the two people usually elide are exactly the ones where the index permutation goes wrong:

python — from scratch, numerically safe (Shepperd 1978)
def R_to_quat(R):
    """Shepperd's method: branch on whichever of w, x, y, z is
    largest, so the square root is never taken of a small,
    cancellation-prone number. Six identities do all the work
    (read each one straight off R(q), entry by entry):

        1 + tr R            = 4w^2    R[2,1] - R[1,2] = 4wx
        1 + R00 - R11 - R22 = 4x^2    R[0,2] - R[2,0] = 4wy
        1 + R11 - R00 - R22 = 4y^2    R[1,0] - R[0,1] = 4wz
        1 + R22 - R00 - R11 = 4z^2    R[0,1] + R[1,0] = 4xy
                                      R[0,2] + R[2,0] = 4xz
                                      R[1,2] + R[2,1] = 4yz

    Every line below is one of those over 4*(branch component)."""
    tr = R[0,0] + R[1,1] + R[2,2]
    if tr > 0:                                  # w is largest
        # CAREFUL: here s = 1/(4w) -- a RECIPROCAL -- so this
        # branch MULTIPLIES by s; the other three DIVIDE.
        s = 0.5 / np.sqrt(tr + 1.0)
        w = 0.25 / s
        x = (R[2,1] - R[1,2]) * s           # 4wx * 1/(4w)
        y = (R[0,2] - R[2,0]) * s           # 4wy * 1/(4w)
        z = (R[1,0] - R[0,1]) * s           # 4wz * 1/(4w)
    elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:   # x is largest
        s = 2.0 * np.sqrt(1.0 + R[0,0] - R[1,1] - R[2,2])
        w = (R[2,1] - R[1,2]) / s           # 4wx / 4x  ANTIsym
        x = 0.25 * s                        # s = 4|x|
        y = (R[0,1] + R[1,0]) / s           # 4xy / 4x  sym
        z = (R[0,2] + R[2,0]) / s           # 4xz / 4x  sym
    elif R[1,1] > R[2,2]:                       # y is largest
        # The ANTIsym pair moves to (0,2), and the two sym sums
        # are the ones containing a y. This branch is NOT the
        # x-branch with the letters swapped -- indices permute.
        s = 2.0 * np.sqrt(1.0 + R[1,1] - R[0,0] - R[2,2])
        w = (R[0,2] - R[2,0]) / s           # 4wy / 4y  NOT (2,1)
        x = (R[0,1] + R[1,0]) / s           # 4xy / 4y
        y = 0.25 * s                        # s = 4|y|
        z = (R[1,2] + R[2,1]) / s           # 4yz / 4y
    else:                                       # z is largest
        s = 2.0 * np.sqrt(1.0 + R[2,2] - R[0,0] - R[1,1])
        w = (R[1,0] - R[0,1]) / s           # 4wz / 4z  antisym
        x = (R[0,2] + R[2,0]) / s           # 4xz / 4z
        y = (R[1,2] + R[2,1]) / s           # 4yz / 4z
        z = 0.25 * s                        # s = 4|z|
    q = np.array([w, x, y, z])
    return q / np.linalg.norm(q)            # renormalise on exit

def quat_mul(a, b):
    """Hamilton product. 16 multiplies, 12 adds."""
    w1, v1 = a[0], a[1:]
    w2, v2 = b[0], b[1:]
    return np.concatenate(([w1*w2 - v1 @ v2],
                           w1*v2 + w2*v1 + np.cross(v1, v2)))

def rotate(q, v):
    """q v q^-1 without building the matrix. ~30 flops."""
    u = q[1:]
    t = 2.0 * np.cross(u, v)
    return v + q[0] * t + np.cross(u, t)

The y and z branches are where hand-written versions break, so look at what actually changes. It is tempting to describe them as "the x-branch with the letters rotated," and that is exactly the mistake. Two independent things permute at once. First, the antisymmetric pair — the one that yields w — moves: it is R[2,1]−R[1,2] in the w- and x-branches, R[0,2]−R[2,0] in the y-branch, and R[1,0]−R[0,1] in the z-branch, because each branch needs the difference that contains its own component (4wx, 4wy, 4wz respectively). Second, the two symmetric sums change identity: the x-branch uses the two sums containing an x (4xy and 4xz), the y-branch uses the two containing a y (4xy and 4yz). So R[0,1]+R[1,0] appears in both the x-branch and the y-branch — same numerator, different quotient: divided by 4x it yields y, divided by 4y it yields x. Copy the line without changing which variable it assigns to and you get a silently transposed quaternion that still has norm 1.

python — the production form
from scipy.spatial.transform import Rotation as Rot

r = Rot.from_quat([x, y, z, w])       # scipy default is SCALAR-LAST
r.as_matrix()                          # (3,3)
r.as_rotvec()                          # (3,) -- axis * angle
r.as_euler('zyx', degrees=True)        # yaw, pitch, roll -- lowercase = EXTRINSIC
r.as_euler('ZYX', degrees=True)        # uppercase = INTRINSIC. Different answer!
(r1 * r2).as_quat()                    # composition, r1 applied AFTER r2

DEBUG — the bug whose norm is still exactly 1

Failure mode: quaternion component order swapped across a boundary. A driver publishes the IMU orientation scalar-first into an array; a fusion node reads it scalar-last. Nothing in the system objects, because the norm is unchanged.

Observable symptom: the orientation estimate is wrong by a large, oddly stable angle — usually somewhere between 120° and 180° — and it moves in the wrong axis when the robot rotates. Because the error is large and consistent, people assume a mounting-orientation mistake and start editing the URDF, which produces a second, compensating bug.

The metric that reveals it: log the scalar component of every quaternion crossing a boundary, and compare it against the physically expected angle:

If a transform you know to be a few degrees reports |w| ≈ 0.04 while one of the vector components is 0.999, the order is swapped. This is a one-line assertion and it belongs in your driver.

Second failure mode: the un-normalised quaternion from a naive average. Someone averages N orientation estimates by taking the componentwise mean and normalising. If the estimates are all in the same hemisphere it is a decent approximation (it is the chordal L2 mean). If any of them arrived sign-flipped, the mean is pulled toward zero and the normalisation amplifies noise catastrophically. Symptom: the averaged orientation is wildly wrong, and only sometimes — it depends on which sign the upstream happened to publish. Metric: the norm of the sum before normalisation. For N nearby unit quaternions it should be close to N; if it is close to 0, you have a hemisphere problem. That is exactly the assertion in the Code Lab above.

FRONTIER — rotations that a neural network can actually regress

Something genuinely new happened here recently, and it is a great answer to "what's changing?"

When a network predicts an orientation, the natural instinct is to output a quaternion (4 numbers) or an Euler triple (3 numbers) and train with an L2 loss. Both train badly, and for a long time people blamed the optimiser. The real reason is topological:

• Zhou, Barnes, Lu, Yang & Li, "On the Continuity of Rotation Representations in Neural Networks" (CVPR 2019). The paper showed that any representation of SO(3) in fewer than 5 dimensions is necessarily discontinuous — there exist rotations arbitrarily close together whose representations are far apart, so the network is being asked to learn a function with a jump in it. Quaternions have the double cover; Euler angles have the wrap. Their fix is the 6D representation: output the first two columns of the rotation matrix and Gram–Schmidt them into a valid R. It is continuous, and it measurably improves pose-regression accuracy on the same architecture and data.
• Levinson, Esteves, Chen, Snavely, Kanazawa, Rostamizadeh & Makadia, "An Analysis of SVD for Deep Rotation Estimation" (NeurIPS 2020). Goes one step further: output all 9 numbers and project onto SO(3) with an SVD (the same UV^⊤ trick from Chapter 1's re-orthonormalisation). Better gradients than Gram–Schmidt because the projection is symmetric in all the columns, and it is now the default in a lot of 6-DOF pose work.

PRACTICE — the three drills this chapter is actually preparing you for

Reading this chapter is not the practice. Performing it is. Each drill below has a time box, the answer, and the habit the drill is really training — which is rarely the answer itself.

Drill 1 — "Write the quaternion-to-matrix conversion on the board." (90 seconds.)

Do it in this order, out loud. (a) "The vector part is v = (x, y, z); let me write its skew matrix first, because that is where all the off-diagonal signs live" — then write [v]_× = [[0, −z, y], [z, 0, −x], [−y, x, 0]]. (b) "R is three terms: (w² − ‖v‖²)I + 2vv^⊤ + 2w[v]_× — isotropic, along-axis, and the turn direction." (c) Expand into the nine entries, reading each w-sign off the skew matrix rather than reciting it. (d) Sanity-check one entry by plugging in a 90° yaw.

What the drill trains: writing the skew matrix first instead of guessing signs. Guessing is the tell that you memorised the table; two-thirds of the guesses will even come out right, which is worse, because then you defend the wrong one. The other habit: checking your own answer unprompted at step (d) — the mark of an engineer who tests their code.

Drill 2 — "Implement quaternion multiply, then rotate a vector with it." (5 minutes, no libraries.)

The whole thing is ten lines and you have already seen them: quat_mul is (w1*w2 - v1@v2, w1*v2 + w2*v1 + cross(v1,v2)) and rotate is the sandwich in its cheap algebraic form, t = 2*cross(u,v); return v + w*t + cross(u,t). Narrate three things while typing: (1) "scalar-first ordering, I will assert that at the boundary"; (2) "the cross term is why this does not commute, which is why rotation order matters"; (3) "the sandwich form avoids building the matrix — about 30 flops versus 15 for a matrix rotate, so this is the right call for one vector and the wrong call for a hundred thousand."

What the drill trains: keeping the flop comparison attached to the code. Anyone can write the sandwich. Knowing when not to use it is the harder skill, and it costs you one sentence.

Drill 3 — "Orientation is wrong by a stable 175°. Ten minutes to a hypothesis." (Say the metric, not the guess.)

Large, stable, wrong-axis says convention, not calibration — calibration errors are small and drift. Log |w| on every quaternion crossing a node boundary and compare it to the physically expected angle, since |w| = |cos(θ/2)|. A mount you know to be a few degrees must have |w| > 0.99. If |w| is near zero and a vector component is near one, the component order was swapped at a flatten site. If instead everything looks individually sane but a composed chain is wrong, compose two known non-commuting rotations and check against a hard-coded reference — that separates Hamilton from JPL.

What the drill trains: naming a measurement before a fix. Engineers who lead with "try swapping the order and see" are describing guess-and-check; engineers who lead with a logged quantity and its expected value are describing debugging. The distinction is the whole craft.

Chapter 4: Gimbal Lock, Double Cover, and Choosing a Representation

A pan–tilt camera mount is tracking a drone that is climbing steeply, almost straight up over the operator. At roughly 80° of tilt the picture starts to trail the target. At 87° the head visibly sticks — the drone slides out of the top of the frame and stays out — and then the whole mount whips around and reacquires from the other side. The operator attaches the video to a ticket titled "gimbal goes crazy overhead."

You pull the log. The tilt channel is smooth the whole way through. The pan channel is not: in the two seconds around the stick, commanded pan rate is pinned at the motor limit, and the recorded yaw angle is jumping between values several hundred degrees apart.

Two completely different bugs produce that video, and they produce it on the same hardware. One is a genuine rank deficiency in the map from Euler rates to body rates — a mathematical singularity that no gain, no filter and no faster loop can remove. The other is a sign flip in a quaternion crossing a library boundary, and the entire fix is one dot product. The first costs a week of estimator surgery. The second costs one line. Reaching for the wrong one costs you the week and leaves the bug in the field.

So this chapter does three things. It derives both failures from scratch, so you can rebuild either from a blank page. It hands you the scalars that separate them in a single glance at a plot. And — picking up Chapter 3's claim that every representation of SO(3) pays a price — it turns those two prices into a defensible, per-boundary choice you can argue for out loud.

CONCEPT — gimbal lock is a rank collapse, not a mysterious glitch

Most explanations of gimbal lock show three nested rings and say "look, two of them line up". True, and unsatisfying, because it does not tell you what breaks in your code or how to detect it. Here is the version that does.

Euler angles say the orientation is R = R_z(ψ)R_y(θ)R_x(φ) — yaw, then pitch, then roll, each about the already-rotated body axis. Now differentiate. The body's angular velocity ω is not (φ̇, θ̇, ψ̇), because those three rates are about three different, mutually rotated axes. Collect them:

• φ̇ (roll rate) is about the body's own x-axis, so in body coordinates it is (1, 0, 0).
• θ̇ (pitch rate) is about the y-axis after the yaw but before the roll, so in body coordinates it is (0, cosφ, −sinφ).
• ψ̇ (yaw rate) is about the world z-axis, which in body coordinates is (−sinθ, cosθ sinφ, cosθ cosφ).

Stack those three as columns and you have the Euler-rate Jacobian E, the matrix that turns Euler rates into body rates:

Now take its determinant. Expand along the first column (only the top entry is non-zero):

CONCEPT — the inverse in general, all nine entries

Worked example 1 set φ = 0 to keep the arithmetic readable, and that is the right first move. But it hides where the damage actually lands, because at φ = 0 the second and third rows of E happen to decouple in a way they do not in general. If you are going to claim gimbal lock is a rank collapse, you should be able to write down the inverse for arbitrary roll and point at the singular entries. It is four lines of algebra.

Write out the three scalar equations that ω = E [φ̇; θ̇; ψ̇] actually says, with ω = (p, q, r):

Rows 2 and 3 contain only θ̇ and ψ̇, so that 2×2 block solves on its own. Do not reach for Cramer's rule — the block is a scaled planar rotation by φ, so the clean move is to take the two combinations that annihilate one unknown at a time. Each one collapses through sin² + cos² = 1.

Combination A — multiply row 2 by sinφ, row 3 by cosφ, and add:

The θ̇ coefficient cancels identically — sinφcosφ minus itself — and the ψ̇ coefficient collapses to a bare cosθ. Divide:

Combination B — multiply row 2 by cosφ, row 3 by −sinφ, and add:

This time it is the ψ̇ terms that cancel, and the answer falls out with no θ in it anywhere: θ̇ = cosφ · q − sinφ · r. Hold on to that; it is not a coincidence and we will meet it again from a completely different direction in two sections' time.

Back-substitute into row 1. Rearranged, row 1 says φ̇ = p + sinθ · ψ̇. Substitute the ψ̇ we just found, and sinθ/cosθ becomes tanθ:

Stack the three results as rows and every entry of the inverse is now visible:

Nine entries, and the pitch angle is distributed across them in a very specific pattern. Read the rows:

That middle row is the sentence most people cannot produce under pressure. Pitch rate is never lost. The pitch-rate row of E⁻¹ is a plain unit vector rotated by roll — it has no cosθ in the denominator, so it is perfectly conditioned at every pitch including exactly 90°. Gimbal lock does not destroy your ability to read pitch rate; it destroys your ability to separate roll rate from yaw rate. Saying "you lose a degree of freedom" is vague. Saying "you lose the roll/yaw split, and pitch rate is untouched" is the answer that shows you did the algebra.

The conditioning view, which is the one you should actually monitor. The determinant is a blunt instrument. The honest measure is the smallest singular value of E, because that is what controls how much the inverse amplifies:

That quantity has a name — the operator norm of E⁻¹, the worst stretch it can apply to any input — and it drops out of the SVD in two lines. Write the singular value decomposition E = UΣV^⊤, where U and V are orthogonal and Σ = diag(σ_max, σ_mid, σ_min). Inverting a product reverses it and inverts each factor, and the inverse of an orthogonal matrix is its transpose:

Now read the lengths off. U^⊤ is orthogonal, so it rotates ω without changing ‖ω‖. V is orthogonal, so it rotates the result without changing its length either. Every bit of stretching lives in the diagonal Σ⁻¹ in the middle, and the largest stretch a diagonal matrix can apply is its largest entry. Since σ_min is the smallest singular value, 1/σ_min is the largest entry of Σ⁻¹. Hence the boxed identity, and it is attained, not merely bounded: feed in the particular unit ω that U^⊤ maps onto the third coordinate axis and you get exactly 1/σ_min out.

Put units on it and it stops being abstract. One radian per second of body rate goes in; up to 1/σ_min radians per second of Euler rate is demanded out. At pitch = 85°, where we are about to derive σ_min = √(1 − sin 85°) = √0.0038053 = 0.061687:

That 16.21 is the number the motors have to survive, and it is the number quoted a few paragraphs below when we decide what to log. Keep it; we will exhibit the exact body-rate direction that attains it once the closed form is in hand.

But a number you are told to monitor and cannot compute is a number you will not monitor. So derive the closed form — it is shorter than the determinant was.

Singular values of E are the square roots of the eigenvalues of the Gram matrix G = E^⊤E, and the Gram matrix is just the table of dot products between E's columns. We already know those columns: they are the three rotation axes written in body coordinates. Write s = sinθ, c = cosθ, and take c₁ = (1, 0, 0), c₂ = (0, cosφ, −sinφ), c₃ = (−s, c sinφ, c cosφ). Six dot products and you are done:

• c₁·c₁ = 1. The body x-axis is a unit vector.
• c₂·c₂ = cos²φ + sin²φ = 1. The pitch axis is a rotated unit vector, so still unit.
• c₃·c₃ = s² + c²sin²φ + c²cos²φ = s² + c² = 1. The world z-axis, seen from anywhere, is still a unit vector.
• c₁·c₂ = 0. Roll axis and pitch axis are perpendicular by construction, at every attitude.
• c₂·c₃ = cosφ(c sinφ) + (−sinφ)(c cosφ) = c sinφcosφ − c sinφcosφ = 0. Every roll term cancels. Remember this line — it is doing more work than it looks like.
• c₁·c₃ = −s. The one and only non-trivial entry, and there is no φ in it.

So the Gram matrix, for any roll whatsoever, is

Read the structure off it. The middle basis vector e₂ = (0, 1, 0) has zeros above and below it in both directions, so G e₂ = e₂: it is already an eigenvector, with eigenvalue exactly 1. That accounts for one singular value and it never moves. What is left is the 2×2 block sitting in the (1, 3) corners:

A symmetric matrix of the form aI + b(off-diagonal swap) always has that pair — (1, 1) and (1, −1) are its eigenvectors and a ± b are its eigenvalues, which you can verify in one line by multiplying. Take square roots and you have the whole spectrum:

Reproduce one row of the table by hand. Take pitch θ = 60°, where sin 60° = 0.86603:

Those are the 60° entries of the table below, produced in two square roots and a division. Run the same two lines at 85° (sin = 0.99619): √0.00381 = 0.06169 and √1.99619 / 0.06169 = 22.90. At 89.9° (sin = 0.9999985): √0.0000015 = 0.00123 and cond = 1145.9. Every number in the table is now yours to regenerate from scratch.

One practical note before the table. Because σ_max = √(1 + sinθ) is trapped between 1 and √2, the condition number is never more than √2 times 1/σ_min. The two right-hand columns therefore carry the same information, and you should log σ_min rather than cond(E), because σ_min is the number with a physical reading: 1/σ_min is the worst-case amplification over all directions of body motion, so σ_min = 0.0617 at 85° means "some unit of body rate will demand 16.2 units of Euler rate." That is the bound the motors have to survive. The per-channel figure 1/cosθ = 11.5, which is what the yaw row of E⁻¹ costs, is the typical case — smaller, and the reason a system can look healthy in one axis right up until the unlucky direction shows up.

"Some unit of body rate" is doing a lot of unpaid work in that sentence. A direction you cannot name is a direction you cannot check, so name it.

python — reproduce every number above in five lines
import numpy as np
th = np.deg2rad(85.0)
E  = np.array([[1, 0, -np.sin(th)], [0, 1, 0], [0, 0, np.cos(th)]])
U, S, Vt = np.linalg.svd(E)
w_worst = U[:, -1] * np.sign(U[2, -1])     # LEFT singular vector = a BODY rate
print(S[-1], np.sqrt(1 - np.sin(th)))       # 0.061687 0.061687  -- the closed form
print(w_worst)                              # [0.043619 0. 0.999048]
print(np.linalg.norm(np.linalg.inv(E) @ w_worst), 1 / S[-1])  # 16.2108 16.2108

Note the U[:, -1] in line 5 and not Vt[-1]. That one-character-class difference is the whole of Step 0, and it is exactly the kind of slip you catch by plugging the vector back in — which is what the last line does. Always close the loop: compute the direction, then apply the inverse to it, then confirm the norm matches 1/σ_min. If it does not, you took the wrong singular vector.

What "rank 2" actually means at the singularity. At pitch = +90°, plug three different (yaw, roll) pairs with the same difference into R = R_z(ψ)R_y(90°)R_x(φ) — say (ψ, φ) = (0.3, 0.0), (0.5, 0.2) and (0.9, 0.6). All three produce the identical matrix:

That claim is worth more than your trust in it, so here is the algebra that produces it. It is the single most convincing thirty seconds you can spend on this topic, because it turns "you lose a degree of freedom" from a slogan into a matrix identity.

The rate picture and the angle picture are the same statement. Two sections ago we found that φ̇ and ψ̇ both diverge but φ̇ − ψ̇ walks calmly to p. Here we find that φ and ψ are individually unobservable but ψ − φ is well defined. Those are not two facts. Differentiate the second and you get the first: the one surviving coordinate is ψ − φ, so the one surviving rate is its derivative, and everything orthogonal to it is coordinate noise. If you can state that link out loud, you have understood gimbal lock rather than memorised it.

Only ψ − φ is observable; yaw and roll have collapsed into one degree of freedom. (At pitch = −90° it is ψ + φ instead.) So an estimator that reports Euler angles near vertical is not just noisy — it is reporting two numbers that individually mean nothing, and any controller differencing them is differencing noise.

CONCEPT — the double cover, and the 358° whip

Quaternions have no gimbal lock. Their price is that the map to SO(3) is two-to-one: q and −q are the same rotation. Chapter 3 showed why algebraically (every entry of R is quadratic). The consequence is that the quaternion can be discontinuous while the rotation is perfectly smooth.

This bites hardest in interpolation. Spherical linear interpolation walks the great-circle arc from q₁ to q₂ on the unit 3-sphere:

The arc length Ω in quaternion space is half the physical rotation angle. Two nearby orientations should have Ω near 0. But if the second quaternion arrived sign-flipped — because a driver renormalised, or an optimiser converged to the other branch, or a message crossed a library boundary — then Ω is near 180° and slerp takes the long way round.

if np.dot(q1, q2) < 0.0:
    q2 = -q2        # same rotation, near hemisphere

DESIGN — one representation per boundary, and why

Now the decision all of this machinery exists to serve. Here is a real humanoid stack, boundary by boundary, with the representation and the reason:

The cost of getting the boundary wrong, in one line each. Euler in a message: your consumer has to know the convention and there are 24 of them. Quaternion in a URDF: nobody can review the file. Matrix on the wire: 72 bytes and it arrives off the manifold after a float32 round trip. Euler in an estimator: the covariance is meaningless near vertical. Quaternion out of a network: the loss has a discontinuity in it. Each of those is a real system somebody shipped.

DESIGN — "there are 24 of them" is a claim you should be able to defend

That line about 24 Euler conventions gets repeated a lot and derived almost never. "Why 24?" deserves a real answer — and the count is a thirty-second construction that also tells you exactly which mistakes are possible.

Build the axis sequence first. You pick three axes in order, with one rule: no axis may immediately repeat, because rotating about z and then about z again is just one rotation about z and you have wasted a slot.

• First axis: 3 choices — x, y or z. Nothing constrains it.
• Second axis: 2 choices — anything except the first.
• Third axis: 2 choices — anything except the second. Note it may equal the first.

Those 12 split cleanly in half, and the split has names you should use:

Then double it. Each sequence can be interpreted two ways: intrinsic (each rotation is about the already-rotated body axis) or extrinsic (every rotation is about the fixed world axis). Same three numbers, same three axis letters, different orientation.

Three more doublings that are not in the 24 but bite exactly as hard. Active versus passive (does R rotate the vector, or rotate the frame the vector is expressed in? — they differ by a transpose, which is Chapter 2's whole subject). Degrees versus radians (a 57× error, usually obvious, occasionally not). And quaternion component order, which gets its own section in the DEBUG block below. Counting those, the real space of ways to misread an orientation triple is nearer 200 than 24.

CODE — slerp with the fix, and the Euler-rate inverse with a guard

python — from scratch
def slerp(q1, q2, t):
    """Interpolate two unit quaternions. The first three lines are the ones
    that matter -- everything after is textbook."""
    d = float(np.dot(q1, q2))
    if d < 0.0:                 # THE line. Same rotation, near hemisphere.
        q2, d = -q2, -d
    if d > 0.9995:              # nearly identical: slerp is numerically unstable
        return (q1 + t * (q2 - q1)) / np.linalg.norm(q1 + t * (q2 - q1))
    om = np.arccos(d)
    so = np.sin(om)
    return (np.sin((1 - t) * om) * q1 + np.sin(t * om) * q2) / so

def euler_rates_from_body(omega, roll, pitch, lock_tol=0.05):
    """Inverse of E. Refuses rather than returning nonsense near lock."""
    cp = np.cos(pitch)
    if abs(cp) < lock_tol:      # |pitch| > ~87.1 deg
        raise ValueError(f"gimbal lock: cos(pitch)={cp:.4f}, "
                         "yaw and roll are not separately observable here")
    sr, cr = np.sin(roll), np.cos(roll)
    tp = np.tan(pitch)
    p, q, r = omega
    return np.array([p + tp * (sr * q + cr * r),      # roll rate
                     cr * q - sr * r,                 # pitch rate
                     (sr * q + cr * r) / cp])         # yaw rate

The lock_tol guard is the engineering content. cos(pitch) < 0.05 means |pitch| > 87.1°, where the inverse already amplifies by 20×. Raising there is far better than returning a number that is technically finite and physically meaningless — a controller downstream will act on that number. Fail loudly at the edge of the chart; do not hand your caller a plausible-looking infinity.

python — the production form
from scipy.spatial.transform import Rotation as Rot, Slerp

key_times = [0.0, 1.0]
key_rots = Rot.from_quat([q1_xyzw, q2_xyzw])
interp = Slerp(key_times, key_rots)     # scipy handles the sign internally
q_mid = interp([0.5]).as_quat()

# The controller error, the way you should write it:
R_err = R_des.T @ R_act                  # the rotation still to be undone
e = Rot.from_matrix(R_err).as_rotvec()   # (3,) -- direction AND magnitude
# |e| is the angle in radians; e/|e| is the axis. Feed e straight to the gain.

DEBUG — three failures that look alike and are not

All three arrive as the same ticket — "the gimbal is doing something insane" — and all three are separated by a single scalar you can plot. Ramp, step, or constant: that is the entire triage, and the rest of this section is what each one means.

Failure mode: a camera gimbal that stops tracking overhead. A pan–tilt mount follows a target. As the target passes near vertical, the camera visibly lags, then snaps.

Observable symptom: commanded pan rate saturates at the motor limit for a second or two, then the mount whips around. The video shows the target sliding out of frame and back in.

The metric: log cos(tilt) alongside the commanded pan rate. If the rate command is proportional to 1/cos(tilt) and the saturation events line up with |tilt| > 85°, it is gimbal lock, not a tuning problem — and no amount of gain scheduling will fix it, because the required rate is genuinely unbounded. The fix is representational: compute the pointing error as a rotation vector between the desired and actual optical axes and drive that, which has no singularity, then convert to motor commands only at the last step (and accept that mechanically the mount still cannot point straight up while yawing — that part is hardware, and saying so is the honest answer).

Failure mode: the 360° whip. Same mount, different fault: once every few minutes, the camera spins almost a full turn and comes back.

Observable symptom: an isolated, large, fast excursion with no build-up, at a tilt angle nowhere near vertical.

The metric: dot(q_prev, q_now) between consecutive samples. Gimbal lock gives a smooth 1/cos ramp in the commanded rate; a sign flip gives an instantaneous negative dot product and a single huge step. Ramp versus step is the discriminator. Both look like "the gimbal went crazy" in the video; they are different bugs with different fixes, and naming which one from the trace is the whole debugging answer.

Failure mode: the third one, which neither of the first two monitors can see. Same mount again. This time the camera's reported orientation is simply wrong — wildly, constantly wrong — and it was wrong from the first frame after somebody swapped a driver. The tilt does not matter. The rate does not matter. Nothing in the log ramps or steps, because nothing ever changes: the error was there at boot.

The root cause: quaternion component order. Four numbers, and the industry never agreed which one is the scalar. Both orders are in production, in libraries you will use in the same process:

The metric that does catch it: park the robot and assert identity. Put the mount in its mechanical home, where the true orientation is the identity rotation. The correct quaternion is (x, y, z, w) = (0, 0, 0, 1). Misread through Direction A, w becomes 0 and the axis becomes (0, 0, 1):

That is the whole test, and it is worth wiring into the startup sequence permanently. A stationary body at a known home pose must report an orientation angle of 0.000°. If it reports exactly 180.000°, you have a component-order swap, not a calibration error — calibration errors are small and untidy, whereas this one is enormous and suspiciously round. Two supporting tells for when you cannot park the hardware: (1) for a mount that only yaws, x ≡ 0, so the misread scalar is pinned at zero and the reported angle is stuck at exactly 180.000° while the mount demonstrably turns — a channel that will not move is as diagnostic as one that moves too much; (2) the reported rotation axis will be a unit vector that includes the true w-component, so it drifts as the body rotates, whereas a genuine mechanical misalignment has a fixed axis.

The whole triage, on one page. Say this table out loud at a broken robot and you have answered the question before you have touched a keyboard:

Notice that the three fixes live at three different layers — the control law, one line inside an interpolation routine, and a type conversion at a process boundary. That is the real reason naming the failure matters more than knowing all three cures: the cures do not overlap at all, so guessing wrong does not get you partway there. It gets you nowhere, one layer away from the bug.

FRONTIER — the error state grew up into invariant filtering

The error-state trick in the DESIGN table — nominal on the manifold, small error in the tangent space — was for a long time a practical hack with good empirical behaviour. It now has theory, and the theory changed what people build:

• Barrau & Bonnabel, "The Invariant Extended Kalman Filter as a Stable Observer" (IEEE Transactions on Automatic Control, 2017). If you define the error using the group operation — η = X̂⁻¹X rather than X̂ − X — then for a large class of systems the error dynamics become independent of the estimate itself. That means the linearisation is exact in a sense a standard EKF's never is, and the filter has provable convergence properties instead of "it works if you initialise it well". Practically: the InEKF does not develop the spurious yaw confidence that plagues standard EKF-SLAM, because the observability structure of the true system is preserved by construction.
• Hartley, Ghaffari, Eustice & Grizzle, "Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation" (IJRR 2020, from the RSS 2018 paper). The legged-robot application, and the one to reach for on a humanoid. It puts base pose, velocity and contact points into a single matrix Lie group and runs an InEKF over it. The reported result is markedly better convergence from bad initial yaw — exactly the case where a conventional EKF becomes overconfident and never recovers.

Chapter 5: Frame Conventions — world, odom, base_link, and the Optical Trap

It is day three of the bug. A pick-and-place cell keeps missing, and every subsystem insists it is innocent. The detector is clearly fine: in RViz the bounding box sits on the box on the conveyor, frame after frame, confidence 0.94, no flicker. The IK is fine too — the solver returns a valid joint configuration in under a millisecond, no limit hit, no singularity warning, no exception. The arm moves smoothly and lands exactly on the pose it was asked for. And then the gripper closes on air, roughly a metre above the box, every single time.

The number that crossed the boundary between the perception node and the motion node was (0.3, 0.2, 1.0). Three floats. All small, all plausible, none of them NaN, none of them out of range. There is no log line to grep for, because nothing failed: each side is internally consistent and each side is doing exactly what it was written to do. What is missing is an agreement about what those three floats mean — and the agreement was never written anywhere a compiler, a linter or a unit test could check it.

That is the shape of this chapter. The previous four were mathematics; this one is agreements, and agreements are where teams lose weeks precisely because there is nothing to derive. Two documents cover most of them: REP-103 fixes units and axis directions, and REP-105 fixes what the frames on a mobile robot mean and how they nest. Knowing them cold is table stakes for robotics software work. Knowing why they are that way — and being able to rebuild the matrices from scratch instead of pasting them — is the differentiator, and it shows within ninety seconds of touching a real tree.

By the end you will have derived the transform that would have caught the grasp bug on day zero, pushed a covariance through it (the step almost everyone forgets), taken the determinant apart at the game-engine boundary where it comes out negative, and written the twenty lines of code that produce the launch-file numbers rather than asserting them.

CONCEPT — REP-103, the axes and units everyone agrees on

CONCEPT — the conventions your neighbours use

REP-103 governs your robot. It does not govern the simulator, the game engine the visualisation team uses, the CAD package the mechanical team exports from, or the flight-controller firmware. Every one of those boundaries is a rotation you must apply, and every one of them has been the subject of a multi-day bug somewhere.

CONCEPT — REP-105, and the one idea that makes it work

REP-105 defines the chain on any mobile robot:

Each frame has a job, and the jobs are chosen so that two incompatible requirements — be globally accurate and be smooth — can both be satisfied at once, by different frames.

earth — the frame you register in and never compute in

Most REP-105 explanations dismiss earth in four words and move on, which is a mistake, because why it exists and why you must immediately leave it is a complete design argument worth five minutes of your attention.

What ECEF actually is. Earth-Centred, Earth-Fixed is a right-handed Cartesian frame whose origin is the Earth's centre of mass. +z points along the conventional rotation axis, out through the reference pole. +x pierces the surface where the equator meets the IERS reference meridian (about 102 m east of the old Greenwich line, which is itself a fine trivia answer). +y completes the right-handed set, coming out at the equator at 90° east. "Fixed" means it rotates with the planet, so a surveyed benchmark has constant coordinates in it — that is the whole point, and it is what makes ECEF the natural common frame for two robots that have never met.

And here is why you leave it immediately: floating point. The Earth's radius is about 6.37×10⁶ m, so every ECEF coordinate on the surface has a magnitude near 6.4 million. A float32 carries a 24-bit significand, and for a value in the binade [2²², 2²³) = [4,194,304 , 8,388,608) — which is exactly where 6.4×10⁶ lands — the spacing between adjacent representable numbers is 2²²⁻²³ = 2⁻¹:

Those two numbers are the same data type, 8,192× apart, and the only difference is where you put the origin. A sensor_msgs/PointCloud2 stores XYZ as float32. So does every GPU vertex buffer, every OpenGL model-view matrix, most mesh formats and a good fraction of the mapping libraries. Express a LiDAR sweep in ECEF and it quantises onto a half-metre lattice before any algorithm touches it — the scan simply stops containing the wall. Symptom: point clouds that look "blocky" or "terraced" at range, ICP that will not converge below half a metre of residual, and a mesh with visible stair-stepping. Metric: take the set of unique coordinate values in one axis and difference the sorted list; if the smallest non-zero gap is a power of two rather than a sensor-noise-like continuum, you are looking at ULP quantisation, not sensor noise.

float64 survives the magnitude — its ULP at 6.4×10⁶ m is 2²²⁻⁵² ≈ 10⁻⁹ m, a nanometre — but it does not survive the arithmetic. Every geometric quantity you care about is a difference of two nearby huge numbers: subtract two 6.4×10⁶ m coordinates to get a 1 cm feature and you have burned about nine of your sixteen significant digits to catastrophic cancellation before the first cosine. Doing it in float32 burns all of them.

So REP-105 introduces a local tangent plane, and the decision it forces is where to put the origin. You pick a datum point (latitude, longitude, altitude), convert it to ECEF once, and build a local ENU frame tangent to the ellipsoid there. That is map. Everything downstream is now metres from a nearby origin, and float32 is fine again. Three ways to choose the origin, with real costs:

Size the tangent plane, because "a few kilometres" should be a number. A plane tangent to a sphere of radius R departs from the surface by about d²/(2R) at horizontal distance d. With R = 6.371×10⁶ m:

So a single local origin is excellent out to about a kilometre, marginal at five, and indefensible at ten — and because the error goes as d², it is invisible during testing near the origin and then grows fast at the edge of the site. That quadratic is the answer to "when do you need more than one map frame", and it is a much better answer than "when it feels too big".

When earth earns its place. Three cases, and they are the ones worth naming out loud: a fleet spanning several buildings, each with its own surveyed map origin, needing one frame in which to express "go to site B"; two robots that booted independently and must relate their maps without ever having seen each other; and any system fusing RTK GNSS, where the measurement genuinely arrives in a global datum and has to be brought into map by a transform somebody chose deliberately. In all three, earth → map is a latched static transform computed once from the survey — it does not stream, it does not drift, and no fast loop ever reads it. The one-sentence version: earth exists so that maps can be registered to each other; the local tangent plane exists so that nobody has to do geometry in numbers with six digits in front of the decimal point.

And the formula the localizer uses is a direct application of Chapter 2:

Read it with the subscript rule: map←base composed with base←odom gives map←odom, and base←odom is the inverse of what odometry publishes. Four symbols, and it is the entire body of a localizer's publish function.

DESIGN — who publishes what, and the one-parent rule

A tf tree is a tree, and that word is load-bearing. Every frame has exactly one parent, and exactly one node may publish a given parent→child edge. Both halves are enforced by convention, not by the library, which is why both get violated.

The frame-choice cheat sheet for consumers, because "which frame does this node live in?" comes up on every new node:

Sizing the correction latency. A localizer at 10 Hz with 60 ms of internal processing means the map→odom edge can be up to 160 ms old. If a 200 Hz controller read map, that is 32 control cycles operating on a stale global pose — and the moment it updates, all 32 cycles' worth of correction arrives at once. In odom, the same controller reads a transform that is at most 5 ms old and never steps. That is the latency argument for REP-105, and it is a better answer than "because the REP says so".

CODE — a localizer's publish function, and the optical bridge

Three functions, and the rule that binds them: nothing in this section contains a pasted matrix, and nothing calls a helper you have not seen the body of. A matrix you cannot regenerate is a matrix you cannot review, and a matrix you cannot regenerate is a matrix you cannot defend.

python — build the matrix from the axis map, never paste it
import numpy as np

def rotation_from_axis_map(mapping):
    """Turn an axis correspondence into a rotation matrix.

    IN    mapping : dict 'x','y','z' -> length-3 sequence.
                    The value is WHERE THAT SOURCE BASIS VECTOR LANDS
                    in the target frame. That is the entire input.
    OUT   (3,3) float64, guaranteed orthonormal with det = +1
    RAISE ValueError naming the fault: not orthonormal, or left-handed

    The columns of a rotation matrix ARE the images of the basis
    vectors, so the whole construction is one column_stack. This is
    the bold rule from Worked example 1, executed instead of asserted."""
    R = np.column_stack([np.asarray(mapping[k], dtype=float)
                         for k in ('x', 'y', 'z')])
    err = float(np.abs(R.T @ R - np.eye(3)).max())
    if err > 1e-12:                   # orthogonality half of Chapter 1's is_valid
        raise ValueError(
            f"axis images are not orthonormal: max|R^T R - I| = {err:.2e}. "
            "Two source axes probably land on the same target axis.")
    d = float(np.linalg.det(R))
    if d < 0:                             # the handedness half -- and the Unity case
        raise ValueError(
            f"det = {d:+.1f}: this is a REFLECTION, not a rotation. The source "
            "frame is LEFT-HANDED. Factor a single-axis mirror out in the "
            "importer and pass the proper part here; tf2 stores rotations as "
            "unit quaternions and cannot represent a mirror at all.")
    return R

def matrix_to_rpy(R, eps=1e-9):
    """Extract (roll, pitch, yaw) for the ZYX convention: R = Rz(y) Ry(p) Rx(r).
    That is what URDF <origin rpy="..."> and static_transform_publisher
    --roll/--pitch/--yaw both mean.

    Read the closed form of that product and four entries do all the work:
        R[2,0] = -sin(pitch)
        R[2,1] =  cos(pitch)*sin(roll)   R[2,2] = cos(pitch)*cos(roll)
        R[1,0] =  cos(pitch)*sin(yaw)    R[0,0] = cos(pitch)*cos(yaw)
    so roll and yaw are one atan2 each -- UNLESS cos(pitch) = 0, when all
    four vanish together, atan2(0,0) is meaningless, and only the SUM or
    DIFFERENCE of roll and yaw is observable. That is gimbal lock, and the
    honest response is to pin one angle rather than return noise."""
    s_pitch = -float(R[2, 0])                # = sin(pitch)
    if s_pitch > 1.0 - eps:                  # pitch = +90 deg: LOCKED
        # here R[0,1] = sin(roll - yaw), R[0,2] = cos(roll - yaw)
        return (float(np.arctan2(R[0, 1], R[0, 2])), np.pi / 2, 0.0)
    if s_pitch < -1.0 + eps:                 # pitch = -90 deg: LOCKED
        # here R[0,1] = -sin(roll + yaw), R[0,2] = -cos(roll + yaw)
        return (float(np.arctan2(-R[0, 1], -R[0, 2])), -np.pi / 2, 0.0)
    roll  = np.arctan2(R[2, 1], R[2, 2])
    pitch = np.arctan2(s_pitch, np.hypot(R[0, 0], R[1, 0]))
    yaw   = np.arctan2(R[1, 0], R[0, 0])   # atan2, never asin: all four quadrants
    return (float(roll), float(pitch), float(yaw))

python — run it, and get the launch-file numbers out
R_body_optical = rotation_from_axis_map({
    'x': (0, -1, 0),      # optical x (image right) lands on body -y
    'y': (0, 0, -1),      # optical y (image down)  lands on body -z
    'z': (1, 0, 0)})     # optical z (forward)     lands on body +x
print(R_body_optical)
# [[ 0.  0.  1.]      <- Worked example 1's matrix, ASSEMBLED, not typed in
#  [-1.  0.  0.]
#  [ 0. -1.  0.]]

roll, pitch, yaw = matrix_to_rpy(R_body_optical)
# the "+ 0.0" turns numpy's signed -0.0 into 0.0 so the printout matches
# the URDF a human will type; -0.0 in a diff wastes a reviewer's minute
print(f"rpy = {roll + 0.0:.4f} {pitch + 0.0:.4f} {yaw + 0.0:.4f}")
# rpy = -1.5708 0.0000 -1.5708
#       ^^^^^^^          ^^^^^^^  these two numbers are the -1.5708 pasted
#       into the launch file and the URDF below. Produced, not asserted.

# And the Unity boundary from Worked example 3, through the SAME function:
rotation_from_axis_map({'x': (0, -1, 0), 'y': (0, 0, 1), 'z': (1, 0, 0)})
# ValueError: det = -1.0: this is a REFLECTION, not a rotation. The source
# frame is LEFT-HANDED. Factor a single-axis mirror out in the importer ...

That failure is the point of the function. The Unity axis map is a reasonable-looking dictionary — three unit vectors, all orthogonal, nothing obviously wrong — and it is rejected with a message that names the actual fault and the actual fix. Compare that to what a pasted matrix does: nothing. It just quietly produces the wrong quaternion, and you find out three days later when a gripper closes on air.

python — the four lines that ARE REP-105, with nothing left undefined
def to_matrix(ts):
    """geometry_msgs/TransformStamped -> (4,4) float64 homogeneous matrix.

    ts.transform.translation : (x, y, z) in METRES        (REP-103, always SI)
    ts.transform.rotation    : (x, y, z, w), SCALAR LAST  (ROS order; Eigen
                               and MuJoCo put w first -- this swap is its own
                               multi-day bug, and it looks like a 180 deg error)"""
    q = ts.transform.rotation
    x, y, z, w = q.x, q.y, q.z, q.w
    n = float(np.sqrt(x*x + y*y + z*z + w*w))
    if abs(n - 1.0) > 1e-6:        # hand-edited YAML and float32 hops both do this
        x, y, z, w = x/n, y/n, z/n, w/n   # an unnormalised q SCALES R by n^2
    T = np.eye(4)
    T[:3, :3] = np.array([
        [1 - 2*(y*y + z*z), 2*(x*y - z*w),     2*(x*z + y*w)],
        [2*(x*y + z*w),     1 - 2*(x*x + z*z), 2*(y*z - x*w)],
        [2*(x*z - y*w),     2*(y*z + x*w),     1 - 2*(x*x + y*y)]])
    t = ts.transform.translation
    T[:3, 3] = [t.x, t.y, t.z]
    return T
# to_msg is the exact inverse: same translation copy, plus matrix -> quaternion,
# which is Chapter 3's Shepperd branch (pick the largest of the four candidate
# denominators). se3_inv is Chapter 1's: R.T and -R.T @ t, never np.linalg.inv.

def publish_correction(T_map_base_measured, buf, stamp, br):
    """Called every time the localizer produces a global pose fix.

    IN   T_map_base_measured : (4,4) float64 ndarray, metres, map <- base_link
         buf                 : tf2_ros.Buffer
         stamp               : builtin_interfaces/Time -- the SENSOR CAPTURE
                               time, never now(). Using now() here dates the
                               correction to the wrong pose.
         br                  : tf2_ros.TransformBroadcaster
    OUT  one geometry_msgs/TransformStamped on /tf: parent 'map', child 'odom'

    We never publish map -> base_link. We publish map -> odom, chosen so
    that the existing odom -> base_link composes to the corrected pose."""
    try:
        ts = buf.lookup_transform('odom', 'base_link', stamp)  # TransformStamped
    except tf2_ros.ExtrapolationException:
        return          # stamp is newer than the newest odom sample. Grabbing
                        # the latest sample instead injects an error equal to the
                        # distance travelled in the gap: 1.5 m/s x 50 ms = 7.5 cm,
                        # which then looks exactly like a bad landmark match.
    T_odom_base = to_matrix(ts)                              # (4,4) float64
    T_map_odom  = T_map_base_measured @ se3_inv(T_odom_base)   # map←base · base←odom
    br.sendTransform(to_msg(T_map_odom, parent='map', child='odom', stamp=stamp))

# The correction magnitude is a free health metric -- publish it.
# A growing ||t|| of map->odom means odometry is drifting faster than
# it used to: a tyre is worn, a wheel is slipping, or VIO is degrading.

That last comment hides a genuinely deep insight. The map→odom transform is a free, always-available measurement of accumulated odometry drift. Publish its translation norm as a diagnostic, alarm when it grows faster than usual, and you have a wheel-slip detector and a VIO health monitor that cost four lines and no new sensors.

the optical bridge, three ways
# 1. In a launch file (ROS 2), as a static transform:
#   ros2 run tf2_ros static_transform_publisher \
#     --x 0 --y 0 --z 0 --roll -1.5708 --pitch 0 --yaw -1.5708 \
#     --frame-id head_camera --child-frame-id head_camera_optical

# 2. In the URDF, which is where it belongs (reviewable, versioned):
#   <joint name="cam_optical_joint" type="fixed">
#     <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
#     <parent link="head_camera"/><child link="head_camera_optical"/>
#   </joint>

# 3. In code, when you must -- built from the axis map, never pasted. Same
#    function as above, so the orthonormality and det checks come for free:
R_BODY_FROM_OPTICAL = rotation_from_axis_map({'x': (0, -1, 0),
                                             'y': (0, 0, -1),
                                             'z': (1, 0, 0)})
# A reviewer can read that dict out loud -- "optical z lands on body +x" --
# and agree or disagree. Nobody can review [[0,0,1],[-1,0,0],[0,-1,0]].
# The rpy above came from matrix_to_rpy(R_BODY_FROM_OPTICAL), so forms 1, 2
# and 3 are provably the same transform and cannot drift apart in review.

The reason there are three forms and not one is that they fail differently, and which one to ship is a real decision. The launch-file form is the worst: the numbers live in a string, nothing validates them, and a typo in one digit of -1.5708 is a silent few-degree error that survives code review. The URDF form is the right default — it sits with the rest of the geometry, it is diffable, and robot_state_publisher latches it once on /tf_static so it costs nothing at runtime. The code form is for the boundary a URDF cannot express: an external SDK, a game engine, a device driver that hands you raw arrays. The rule that ties them together is that all three should be generated from one axis map, so they cannot disagree. A URDF that says rpy="-1.5708 0 -1.5708" and a Python constant that says something else is a bug that no test catches, because each is self-consistent.

DEBUG — the jerk that arrives on good news

Failure mode: a controller subscribed to the wrong frame. A warehouse AMR drives smoothly, then lurches. Not randomly — a few times a minute, and much more often near the ends of aisles.

Observable symptom: a brief violent acceleration command with no corresponding obstacle, no corresponding operator input, and no corresponding sensor event. Operators describe it as "it flinches". It gets worse when localisation is working well, because a well-localised robot corrects more often.

The metric that reveals it in one plot: overlay the timestamps of map→odom updates on the commanded acceleration trace. If every acceleration spike coincides with a correction, you have found it. Quantitatively, differentiate the pose the controller is consuming: a genuine motion gives a bounded velocity; a frame jump gives an impulse whose height is (jump size)/(control period), which for a 0.35 m jump at 200 Hz is 0.35/0.005 = 70 m/s — a number no physical robot can produce, and therefore an unambiguous fingerprint.

The variant that hides from that plot. If the consumer is interpolating rather than holding — which is what tf2 does by default — the fingerprint is not a 70 m/s spike but a 3.5 m/s plateau lasting one localizer period, because the 0.35 m jump is spread over the 100 ms between two 10 Hz samples. Nothing in the trace is physically impossible any more, so the "no robot can do that" argument evaporates and the acceleration looks like an aggressive but legal command. The tell that survives: the plateau still starts on a map→odom timestamp and still lasts exactly one localizer period, and its height still equals the correction magnitude divided by that period. Plot the correction size against the plateau height across a hundred events; if the points fall on a straight line through the origin whose slope is the localizer rate, the pose is jumping, not the robot moving. An impulse is easy to spot and a ramp is not, so always check the ramp before concluding the frames are fine.

Second failure mode: the missing /tf_static in a replayed bag. An engineer records a bag to debug a field failure, replays it, and every transform lookup fails with "frame does not exist". Because /tf_static is latched and published once at startup, a bag started afterwards contains none of it. Symptom: the tree in view_frames is a set of disconnected islands — sensors floating with no path to base_link. Metric: ros2 bag info and check the /tf_static message count; if it is zero, the bag is not replayable. Fix: record /tf_static explicitly in every recording profile, and add a CI check on the recording config. This is a five-minute fix that saves a field trip.

FRONTIER — when there is more than one map frame

REP-105 assumes one robot and one map. The frontier is what happens when neither is true.

• Tian, Chang, Hughes, Carlone et al., "Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems" (IEEE T-RO, 2022). Each robot boots with its own odom origin and its own private map frame. The system has to estimate the inter-robot transforms online from shared observations, decide which candidate alignments to trust, and reject the bad ones — a single false inter-robot loop closure corrupts every robot's map at once. The frame problem stops being bookkeeping and becomes an estimation problem with an outlier-rejection layer (they use graduated non-convexity on the pose graph).
• Lajoie & Beltrame, "Swarm-SLAM: Sparse Decentralized Collaborative SLAM Framework for Multi-Robot Systems" (IEEE T-RO, 2024). The bandwidth-aware version: robots exchange sparse descriptors rather than maps, and align frames only when the expected information gain justifies the radio budget. The design question it raises is the one that matters in the field — which frame does a shared goal live in, and who is authoritative for the transform between two robots' maps?

PRACTICE — four drills to do on paper

None of these are lookups. Each is the kind of question that arrives mid-conversation at a robot and deserves an answer within a minute. Cover the answer, do the arithmetic, then check.

Chapter 6: Extrinsics vs Intrinsics — the Distinction That Locates the Error

A manipulator that was grasping at 96% is now at 78%. Nothing shipped. No software changed. The failures are all on the far bin — the near picks still work. You have five minutes and a tape measure before the line restarts.

Which of the camera's two calibrations is broken?

That question — not "define intrinsics" — is the one production will actually ask you, and the whole chapter is the machinery for answering it. Notice what the scenario already handed you: near works, far fails. That is not a complaint, it is a measurement. Errors that grow with distance and errors that do not come from different families, and by the end of this chapter you will be able to name the family from that one sentence, then propose the five-minute experiment that confirms it.

So here are the two families, introduced as the answer to the scenario rather than as a preamble to it.

Intrinsics answer: given a ray arriving in the sensor's own frame, which measurement does the sensor report? For a camera that is focal length, principal point and lens distortion. It is a property of the device, and it does not change when you move the device. Ship the camera to another continent, bolt it to another robot: same intrinsics.

Extrinsics answer: where is that sensor frame, relative to the rest of the robot? A rigid transform — the SE(3) from Chapter 1, six numbers. It changes the moment someone bumps the mount, and it says nothing whatever about the sensor's internals.

Nobody at a broken robot asks for definitions. They describe the symptom and need to know which one is broken from the shape of the error. That is what this chapter trains, and it hinges on one idea: the two families produce completely different error signatures, and the signature is measurable in five minutes.

CONCEPT — the pipeline, and where each parameter sits

Read the pipeline and the discriminator falls out of it. The extrinsics act before the perspective divide, so their effect is modulated by depth. The intrinsics act after, so their effect depends only on where you are in the image. That is the whole chapter.

CONCEPT — one equation, five perturbations, five different shapes

Everything in this chapter comes out of one line. Ignore distortion for a moment and write the horizontal half of the projection:

Nothing else is needed. Every calibration fault is a perturbation of one symbol in that line, and the shape of the resulting residual Δu is just the derivative of u with respect to that symbol. Do the five derivatives once, by hand, and you never have to memorise a signature table again — you can regenerate it at a whiteboard whenever a robot is broken, which is the point.

Perturbation 1 — the focal length is wrong by δf. Differentiate u with respect to f_x:

Linear in x_n, exactly zero at x_n = 0, and Z does not appear anywhere. The depth cancelled in the perspective divide before f_x ever touched the number, which is the formal version of "intrinsics act after the divide".

Perturbation 2 — the principal point is wrong by δc_x. Even easier:

A constant. The same number at every pixel, at every depth. It is the only fault in the chapter whose image residual is genuinely constant — remember that, because the next one is often mis-stated as constant and is not.

Perturbation 3 — the mount is yawed by θ. This one people get wrong, so do it slowly. A rotation acts on the ray, not on the pixel. Write the ray in the camera frame as the direction d = (x_n, y_n, 1) — any point along it projects to the same pixel, so the direction is all that matters. A small rotation of θ about the optical y-axis (the yaw axis for a forward-looking camera) is, to first order, d → d + θ (ŷ × d). Compute the cross product with ŷ = (0, 1, 0):

So the rotated ray is d′ = (x_n + θ, y_n, 1 − θx_n). Now re-do the perspective divide, because the third component moved too — this is the step everyone skips:

dropping the θ² term. Multiply by f_x:

The same result falls out of the tangent form: a yaw sends x_n → tan(arctan x_n + θ), and d/dθ of that at θ = 0 is sec²(arctan x_n) = 1 + x_n². Two derivations, one answer — a good sign you have it right.

Perturbation 4 — the mount is translated by δt laterally. Here the rays stay parallel and the origin moves. Start from the projection of a point at depth Z, perturb X_c by δt at fixed Z, and subtract:

Three lines, and now the 1/depth falloff is obvious rather than memorised: the numerator is a fixed number of metres and the denominator is how far away the thing is. Same physical shift, fewer pixels, the further you look. This is the only perturbation in which Z survives to the end, and it survives because a translation happens before the divide and does not cancel with it.

Perturbation 5 needs its model built first, because it is the only equation in this chapter that has not been earned yet. The other four came out of u = f_xx_n + c_x by differentiation. Distortion is a new term, and anyone who writes the polynomial down from memory and cannot say where it comes from is one "why even powers?" away from trouble. It takes forty seconds to derive, so do it slowly the way we did the yaw.

Why the correction can only point along the radius. A lens element is ground on a spherical surface, so the whole assembly is rotationally symmetric about the optical axis: spin the barrel about that axis by any angle and it is physically the same lens. Whatever displacement the glass induces at an image point must therefore be equivariant under that spin — rotate the incoming ray by φ and the displacement has to rotate by the same φ, because the lens cannot tell you did it. The only vector field on the plane with that property is one that points along the radius with a magnitude depending on the radius alone. Any term that singles out a direction — a bare y_n², an x_ny_n cross term, anything carrying the polar angle explicitly — would require the lens to have a preferred azimuth, and a ground sphere does not have one. So before any physics enters, symmetry alone has forced the shape:

Why g contains only even powers of r. Two independent arguments, and they agree, which is the same comfort the two yaw derivations gave. (1) Parity. A 180° spin is a special case of the symmetry above, and it sends (x_n, y_n) → (−x_n, −y_n) while leaving r unchanged. For x_d = x_ng(r) to negate along with its input — which it must, since the displaced point has to follow the ray — g may not change when x_n flips sign, so g must be an even function of r. Every odd power is forbidden. (2) Smoothness. The wavefront leaving a polished lens has no kink on the optical axis, so g must be differentiable through r = 0. But r = √(x_n² + y_n²) has a corner at the origin exactly as |x| does, while r² is a plain polynomial in the coordinates and is perfectly smooth. A smooth g can therefore only be a power series in r². Same conclusion, different route.

One constant is still free, and pinning it is worth a sentence because it is the chapter's degeneracy theme showing up again: we define g(0) = 1. A g(0) of anything else would be a uniform magnification applied to every ray equally, and a uniform magnification is indistinguishable from a change in f_x — the calibration would simply absorb it into the focal length and you would have two parameters fighting over one number. Normalising g(0) to 1 is what makes k₁ and f_x separately identifiable. What is left is the lowest-order series that can exist:

And now the physics, which decides the sign of k₁ rather than the form. A pinhole is exact only in the paraxial limit, where sin u is replaced by u. Refraction at a spherical surface obeys Snell's law with the true sine, and the first term the paraxial model threw away is −u³/6 — cubic in the field angle. That single fact is why the displacement f·k₁·r²·x_n scales as r³ and not as r² or r⁴: the leading distortion term is the leading term of the sine expansion, the third-order Seidel aberration. Physically, a spherical surface bends marginal rays — the ones arriving far off-axis — more strongly than the paraxial rule predicts, so those rays land closer to the axis than an ideal pinhole says. The corners are pulled inward, straight lines bow outward like the staves of a barrel, and that is k₁ < 0, barrel distortion, the normal case for a short focal length. Move the aperture stop so magnification instead rises with field angle and the corners are pushed outward: k₁ > 0, pincushion, which is what long lenses and some machine-vision optics do.

Perturbation 5 — the radial distortion coefficient k₁ is wrong by δk₁. With the model now derived rather than asserted — x_d = x_n(1 + k₁r² + …), r² = x_n² + y_n² — differentiate with respect to k₁ and multiply by f_x:

Cubic in the radius. At r = 1/3 of the way out, r³ = 1/27 of the corner value — which is precisely why distortion is invisible in the middle third of the frame and enormous at the corners, and why calibrating on targets that never leave the centre gives you distortion coefficients that are essentially noise.

Now flip the pipeline around, because the robot cares about metres, not pixels. The manipulator does not consume u; it consumes a 3D point. Back-project: given a measured pixel u and a depth Z (from stereo, a depth sensor, or a known target size), the reconstructed lateral coordinate is

with hats marking the stored calibration. Every 3D error in this chapter is the difference between that and the truth, and because Z multiplies the whole expression, any error in the assumed ray direction becomes a metric error proportional to range, while an error that is already metric stays put. That is the second sentence of the answer, and the two together are the entire diagnostic:

CONCEPT — the discriminator table, and how to earn it in five minutes

The stereo case is worth its own subsection, because it is the one where every internal metric stays green. And since the formula gets quoted more often than it gets derived, derive it — it is two lines of similar triangles.

Two identical pinhole cameras, optical axes parallel, separated by a baseline B along x. A world point at depth Z and lateral offset X from the left camera projects to x_L = f·X/Z. Seen from the right camera the same point sits at lateral offset X − B, so x_R = f·(X − B)/Z. The disparity is the difference of those two image coordinates, and X cancels:

X vanishing is the whole reason stereo works: disparity depends on depth and on nothing else about where the point is. Now perturb B, which is the failure mode. Suppose the true baseline is B but the stored value is B̂ = B(1 + ε). The cameras still see the same photons, so d does not change — it is a measurement, not a parameter. Only the conversion changes:

Every depth is scaled by exactly (1 + ε), and the disparity — the only thing reprojection error is computed from — is untouched. That is the formal reason the metric stays green: reprojection error asks "do the pixels agree?", and they do, perfectly. A uniformly scaled scene viewed by a uniformly scaled rig lands on identical pixels. Concretely, at ε = 8% a target truly at 2.000 m is reported at 2.160 m, at 4 m it is reported at 4.32 m, and the reprojection RMS reads whatever it read yesterday.

Note the shape, because it is the sixth signature and it is different from the other five: the error is 160 mm at 2 m and 320 mm at 4 m — proportional to range, so it looks like a rotation on the range plot — but expressed as a percentage it is flat, and unlike a rotation it does not depend on the target's angular position at all. Healthy reprojection error plus a constant percentage error in every distance means suspect a metric input — baseline, calibration-target square size, wheel radius. Those three are the only places a number in metres enters the pipeline, so that is the whole search space. Multi-view geometry works through the full version.

CONCEPT — every sensor has both, not just cameras

Engineers who have only done vision think "intrinsics" means K. The split is general, and knowing what each sensor's intrinsics are is what lets you debug any of them:

DESIGN — calibration is data, and data has a lifecycle

The single most useful framing for this topic: calibration is not code, it is per-unit data, and it needs the same lifecycle discipline as any other data.

Count what defines the geometry of a six-camera humanoid: per camera, K is 4 numbers (f_x, f_y, c_x, c_y), distortion is 5, and the extrinsic to base_link is 6. That is 15 numbers per camera, 90 numbers for the rig, plus one time offset each. Ninety floating-point values, unique to that physical robot, on which every millimetre of manipulation accuracy depends. Lose the file and you have a robot that cannot see.

The runtime data flow, with shapes. Every frame: an image (720, 1280, 3) plus K (3,3) and dist (5,) go into undistortion — done once into a cached remap table, roughly 2–4 ms on CPU for 720p, or free on the GPU. The undistorted rays plus depth give points in the optical frame (N, 3) float32. Then one 4×4 extrinsic lifts them to base_link: for a 300,000-point cloud that is 300k × 15 flops = 4.5 Mflop, well under a millisecond vectorised. The extrinsic is applied once per cloud; the intrinsics are baked into a lookup table once at startup. Knowing which one is per-frame work and which is one-time is a real design distinction.

CODE — the projection, and a residual classifier

One term in the code below has not appeared in the prose yet, so name it before you read past it. The dist vector unpacks as k1, k2, p1, p2, k3 — three radial coefficients and two others. Those two are tangential distortion, and they exist because the symmetry argument that forced the radial form has a loophole. Radial distortion came from the lens being rotationally symmetric about the optical axis. p₁ and p₂ model what happens when it is not: an element whose optical axis is tilted or laterally offset relative to the sensor, which is decentering — an assembly tolerance, not a grinding property. Somebody's pick-and-place machine placed the barrel a few tens of microns off, or the glue cured with the element a fraction of a degree cocked. Because that breaks the rotational symmetry, the displacement is no longer forced to point along the radius, and the two components are free to be asymmetric. That asymmetry is exactly what you see in the code: each component mixes an x_ny_n cross term with an r² + 2·(its own coordinate)² term, the pair being the lowest-order polynomial that a single tilt direction can produce. This is Brown's 1966 decentering model, the "Conrady" half of Brown–Conrady.

python — from scratch
import numpy as np

def project(X_w, K, dist, R_cw, t_cw):
    """World point -> pixel. EXTRINSICS first, INTRINSICS last."""
    Xc = R_cw @ X_w + t_cw               # extrinsics: 6 numbers
    if Xc[2] <= 0:
        return None                     # behind the camera; a real check
    xn = Xc[:2] / Xc[2]                  # perspective divide: no parameters
    k1, k2, p1, p2, k3 = dist
    r2 = float(xn @ xn)
    radial = 1 + k1*r2 + k2*r2**2 + k3*r2**3
    tang = np.array([2*p1*xn[0]*xn[1] + p2*(r2 + 2*xn[0]**2),
                     p1*(r2 + 2*xn[1]**2) + 2*p2*xn[0]*xn[1]])
    xd = radial * xn + tang              # intrinsics, part 1
    return np.array([K[0,0]*xd[0] + K[0,2],   # intrinsics, part 2
                     K[1,1]*xd[1] + K[1,2]])

python — the diagnostic worth more than the projection
import numpy as np

def classify_fault(ranges, errors_3d, radii_px, residuals_px,
                   centred=False, noise_px=0.2):
    """Two fits, and BOTH of them fire.

    Fit 1 (3D error vs range)  splits ray-ORIGIN faults from ray-DIRECTION ones.
    Fit 2 (residual vs radius) splits the ray-direction faults from each other,
                               so it is consulted on every path that reaches a
                               ray-direction verdict - never as a fall-through.

    ranges       (N,) metres  - target distance for each 3D measurement
    errors_3d    (N,) mm      - measured 3D position error at that distance
    radii_px     (M,) px      - radius from the principal point, one image
    residuals_px (M,) px      - reprojection residual at that radius
    centred      bool         - True if the 3D sweep used a CENTRED target,
                                which is what kills the focal term
    noise_px     float        - per-feature residual noise; growth across the
                                frame is only believable above it
    """
    ranges, errors_3d = np.asarray(ranges, float), np.asarray(errors_3d, float)
    radii_px, residuals_px = np.asarray(radii_px, float), np.asarray(residuals_px, float)

    # GUARD FIRST. A single-range sweep makes A rank-deficient, so this has to
    # sit ABOVE the solves, not after them.
    span = float(ranges.max() - ranges.min())
    if span <= 0:
        return "inconclusive - every sample is at the same range"

    # 1. 3D error vs range: a ramp through the origin vs a flat offset
    A = np.stack([ranges, np.ones_like(ranges)], axis=1)
    slope, offset = np.linalg.lstsq(A, errors_3d, rcond=None)[0]

    # 2. image residual vs radius. r_offset = the residual extrapolated to the
    #    PRINCIPAL POINT; growth = how much it gains out to the frame edge.
    B = np.stack([radii_px, np.ones_like(radii_px)], axis=1)
    r_slope, r_offset = np.linalg.lstsq(B, residuals_px, rcond=None)[0]
    growth = abs(r_slope) * float(np.ptp(radii_px))

    # UNITS. slope is mm per metre; offset is mm. Comparing them directly is a
    # dimensional error: the same physical data logged in cm changes slope by
    # 100x and leaves offset alone, flipping the verdict. Multiply the slope by
    # the range span so both sides are mm and the test is unit-free.
    ramp = abs(slope) * span          # mm of growth across the sweep

    if ramp > 3 * abs(offset):
        # RAY-DIRECTION fault. Fit 2 now NAMES which one, here on the main path.
        if not centred and growth > 3 * abs(r_offset):
            return "INTRINSIC focal / distortion (residual -> 0 at the principal point)"
        if growth > 3 * noise_px:
            return "extrinsic ROTATION (non-zero at centre AND grows as 1+xn^2)"
        if abs(r_offset) > 3 * noise_px:
            return "PRINCIPAL POINT (non-zero at centre and flat across the frame)"
        return "ray-direction fault - residuals under noise; widen the radius spread"
    if abs(offset) > 3 * ramp:
        return "extrinsic TRANSLATION (3D error is constant with range)"
    return "mixed / inconclusive - collect a wider range spread"

# On THIS chapter's DEBUG data - ranges 0.5/1.0/2.0 m, errors 1.8/3.5/7.0 mm,
# centred=True, and the five image radii 0/160/320/480/640 px whose residuals
# a 0.20 deg yaw predicts as 2.79/2.90/3.24/3.80/4.58 px:
#   fit 1 -> ramp  = 3.471 mm/m * 1.5 m = 5.21 mm  vs offset  0.05 mm -> direction
#   fit 2 -> growth = 0.0027925 * 640   = 1.79 px  vs r_offset 2.57 px
#   1.79 px of growth is 9x the 0.2 px noise  -> "extrinsic ROTATION"
# Feed it a flat 2.79 px residual instead (growth 0, r_offset 2.79) and the
# SAME 3D numbers now return "PRINCIPAL POINT". That is the whole Q3 branch,
# decided by fit 2, on data fit 1 cannot tell apart.

Fifty-odd lines that turn "the robot is inaccurate" into a subsystem name. Writing a diagnostic like this is worth far more than reciting the projection equations, which everyone already knows. A diagnostic that reports "I cannot distinguish these two from this data" is worth more than one that guesses — but a diagnostic that computes a second fit and then ignores it is worse than either, and the earlier version of this function did exactly that. Making the second fit load-bearing is the difference between code that looks like a decision procedure and code that is one.

the production tools
# Intrinsics + stereo extrinsics, checkerboard or ChArUco:
cv2.calibrateCamera(...)        # returns K, dist, per-view rvecs/tvecs
cv2.stereoCalibrate(...)        # returns R, T between the two cameras

# Camera-IMU extrinsics AND the time offset, jointly (Kalibr):
#   kalibr_calibrate_imu_camera --bag rig.bag --cam cams.yaml --imu imu.yaml

# LiDAR-camera and multi-sensor rigs: OpenCalib, direct_visual_lidar_calibration
# Online, inside the estimator: OpenVINS estimates K, extrinsics and the
# time offset as part of the filter state.

DEBUG — the scenario we opened with, resolved

Back to the top of the chapter. The grasp success rate fell from 96% to 78% over three weeks, nothing shipped, and the failures are all on the far bin. Now you have the machinery, so run it.

Say the symptom first, because the symptom is already a measurement. The failures are concentrated on far objects. Picks at the near edge of the workspace still succeed; picks at the far edge miss. Nobody noticed at first because the far bin is used less. From the two-sentence classifier: an error that grows with range is a ray-direction fault, not a ray-origin one, which rules out extrinsic translation before you have touched a tool.

The metric that reveals the rest: place a calibration target at 0.5 m, 1.0 m and 2.0 m, centred in the frame each time — that detail is doing real work, and worked example 5 is why — measure the 3D position error at each, and fit a line.

0.20° is about a fifth of a degree — you cannot see it, you cannot feel it by hand, and it is exactly what two loose M3 screws and three weeks of vibration produce. Note also how small the cause is relative to the effect: 0.20° at the far bin is 7 mm of miss, which for a 20 mm gripper aperture on a 15 mm part is precisely the margin. The fix is Loctite and a torque spec, not a recalibration, because a recalibration will hold for another three weeks and then you will be back. Saying that out loud is what separates fixing the symptom from fixing the system.

FRONTIER — calibration that stops being an event

The direction of travel is clear: from calibration as a one-off procedure with a target, toward calibration as a continuously estimated part of the state.

• Furgale, Rehder & Siegwart, "Unified Temporal and Spatial Calibration for Multi-Sensor Systems" (IROS 2013) — the Kalibr paper. The key move was treating the time offset between sensors as just another parameter in the same continuous-time batch optimisation as the extrinsics, using B-spline trajectories. Before this, teams calibrated space and time separately and the two errors traded off against each other invisibly.
• Geneva, Eckenhoff, Lee, Yang & Huang, "OpenVINS: A Research Platform for Visual-Inertial Estimation" (ICRA 2020) — puts camera intrinsics, camera–IMU extrinsics and the time offset into the filter state and estimates them online. Practically: the robot converges to its own calibration during normal operation, and a mount that shifts gets tracked instead of silently degrading. The tradeoff to be able to state: online calibration needs excitation. Parameters are only observable under motion that makes them observable — a robot driving in a straight line at constant velocity cannot observe its camera–IMU translation at all, and a filter that tries anyway will drift the estimate into nonsense.
• Yan et al., "OpenCalib: A Multi-sensor Calibration Toolbox for Autonomous Driving" (2022) — the targetless direction: aligning LiDAR to camera using scene edges and road-plane geometry instead of a checkerboard, so an extrinsic check can be run on any recorded drive rather than requiring the vehicle to come back to a calibration bay.

PRACTICE — the sixty-second answer, and three to classify cold

Everything above is knowledge. This section is the performance, which is a separate skill. Rehearse it out loud — the failure mode at a broken robot is never "I did not know the (1 + x_n²) factor", it is "I knew it and produced it in the wrong order, after ninety seconds of silence".

The one sentence to carry out of this chapter. If you remember nothing else about calibration: "Extrinsics and intrinsics enter the measurement on opposite sides of the perspective divide, so extrinsic errors are modulated by depth and intrinsic errors are modulated by image position — which means a target at two ranges and two image positions partitions the entire fault space in five minutes." That sentence contains the mechanism, the consequence and the experiment, in that order, which is the shape every good technical answer has.

Chapter 7: Showcase — A Live tf-Tree Inspector You Can Break

02:40, Tuesday night shift, cell 4. The humanoid has been lifting totes off a conveyor for six weeks without a complaint. Tonight it is missing the bin by roughly 15 cm — but only on left-hand picks. Right-hand picks land clean. The arm was recalibrated on Friday and its report is green. The gripper closes on empty air at exactly the moment it should, so the timing is not the problem. The detector's boxes sit perfectly on the totes in the debug image. The line supervisor gives you twenty minutes before cell 4 goes to manual and the shift's numbers are gone.

Nothing in that paragraph tells you which subsystem is wrong, and that is the whole difficulty: a frame bug is invisible to every component that does not span the broken edge. The arm's calibration only ever measures the arm against itself. The detector's confidence only ever scores pixels. Each subsystem certifies itself and the robot still misses. The only thing that can see the fault is the end-to-end geometry, and the only tool that localises it is the one this chapter builds: make the error a function of something you can turn, then find out what it is a function of.

The data flow the widget stands for, with the numbers. The localizer publishes odom → base_link as a nav_msgs/Odometry at 50 Hz, mirrored into tf. The motor controller publishes sensor_msgs/JointState on /joint_states at 200 Hz — parallel name[], position[], velocity[] arrays — and robot_state_publisher turns each revolute joint into a tf edge at that same 200 Hz. torso → shoulder and torso → head_camera are static: latched once on /tf_static at boot and never republished. The detector publishes a geometry_msgs/PoseStamped in camera_color_optical_frame at 30 Hz. The IK consumes exactly one thing: a single 4×4 in odom. Four frames, five edges, three message types, and one number at the end that is either right or 15 cm wrong.

Where the bytes and the milliseconds go. A TransformStamped is 7 doubles of payload — 56 bytes, roughly 100 with the header and bookkeeping — so two dynamic edges at 200 Hz is 400 messages/s, about 40 kB/s. Bandwidth is not the constraint here and saying so is part of the answer. Latency is. The detection is stamped at time t, and if the consumer looks up "the latest tf" instead of tf at t, it uses joint angles that are up to a full detector period stale. At 30 Hz that is 33 ms; with the torso slewing at 60 °/s that is 2.0° of waist rotation, and 2.0° = 0.035 rad across the 0.55 m lever arm from the waist pivot to the gripper is 1.9 cm of pure timing error with every transform in the tree perfectly correct. Hold that number: it is the noise floor against which every fault below has to be distinguished, and it is why Lesson 2 exists.

Below is that humanoid on the pick cell, viewed from above. Drag the base anywhere in the scene. Turn the torso. Turn the gripper. The teal chain is the true kinematic chain, drawn frame by frame from odom down to gripper. The orange ghost is what the software believes, once you inject a bug.

The readout below the scene gives you the true gripper position, the commanded position, the error in centimetres, and the exact algebraic reason for the gap. The point of this widget is not the picture — it is the relationship between the control you move and the number that changes. Each bug has a distinct signature, and by the end of this chapter you should be able to predict which control makes which error grow, and to how many centimetres, before you touch it. Every number quoted in this chapter is one the widget will print back at you; if any of them disagrees, one of us is wrong and it is worth ten minutes to find out which.

CONCEPT — the five constants this robot is actually made of

A showcase widget you cannot open is a black box with a nice colour scheme, and a black box teaches nothing. So here is the entire robot. It is five planar rigid transforms, each written P2(θ, x, y) — rotate by θ about z, then translate by (x, y) in the parent's axes. Every number the readout prints falls out of these five and nothing else.

Compose them in order and the gripper's position in odom is one nested expression:

Worked example — the true gripper at the default sliders, by hand. Load the chapter and touch nothing: base yaw β = 15°, torso yaw ψ = −35°, shoulder σ = 25°, base at (0.55, 0.55) m. Work outward from the arm.

Now read the widget's first line. It says true gripper = (0.930, 0.134) m. That agreement is the licence to trust every other number in this chapter — you have verified the instrument against your own arithmetic once, so from here on a disagreement means a real fault rather than a broken toy.

CONCEPT — deriving all five error laws before you touch a slider

The experiments below are worth nothing if you run them and then rationalise what you saw. Derive first, predict a number, then look. Four of the five laws need one small identity, so derive that once.

The (I − R) identity. For any planar rotation R(θ) and any vector v:

A rotation preserves length, so the last term is ‖v‖². And v·R(θ)v = ‖v‖² cos θ, because rotating v by θ puts it at angle θ from itself. So the whole thing is 2‖v‖²(1 − cos θ), and the half-angle identity 1 − cos θ = 2 sin²(θ/2) collapses it:

Linear in the offset, and in the angle it starts at exactly zero, rises almost linearly for small θ, and reaches 2‖v‖ at a half turn. "Zero at identity, growing like 2 sin(half the angle)" is the fingerprint of a missing rotation, and you now own it for the rest of your career.

Law 1 — unrotated offset. The code builds base_link ← shoulder by composing the rotations correctly but merely adding the translations: t = t_t + t_s instead of t_t + R(ψ)t_s. Because the rotation part is untouched, everything downstream (T_a) contributes identically to both chains and cancels. What survives is the translation gap, carried into odom by the base rotation:

with ‖t_s‖ = √(0.06² + 0.18²) = √0.036 = 0.18974 m. R(β) is a rotation, so it cannot change the magnitude — which is the entire content of experiment 2.

Law 2 — reversed compose. The chain multiplies T_sT_t where it should multiply T_tT_s. Both products have the same total rotation, ψ − 0.25, so again T_a cancels and only the translations differ:

Law 3 — wrong inverse. A round trip base → odom → base through an inverse that negated t instead of applying −R^⊤t leaves T_b·T_b^bad −1, whose rotation cancels to identity and whose translation is the residual:

Law 4 — flipped quaternion sign. The torso executes ψ + 180°. Since R(ψ + π) = −R(ψ), everything hanging below the waist pivot is reflected through it, and the gap is twice that sub-chain's offset. The sub-chain's offset is exactly the u you already computed:

Law 5 — optical frame skipped. Here the kinematics are untouched and the measurement is corrupted: (right, down, forward) read straight into (x, y, z), so (0.35, 0.10) is consumed as (−0.10, 0.00). The fault is the target's displacement, a fixed vector in the camera frame rotated into odom:

Now the payoff table. Every one of these is computable before you click anything, and all six are readable off the widget at the default sliders — press the six buttons in turn and change nothing else.