Homogeneous transforms: one box for the turn and the shift

Where you are. You can build a rotation in 3D and store it as a quaternion without walking into gimbal lock. This lesson adds the other half of a pose, where a thing sits rather than which way it faces, and packs both halves into one object that composes and inverts by ordinary matrix arithmetic.

Put your phone on the desk at an angle

Lay your phone flat on the desk, turned maybe thirty degrees off square. Drop a coin on the desk a hand’s width from it.

Now say where the coin is, twice.

First along the desk: so far to the right of the desk’s front-left corner, so far back. Then along the phone: so far up the phone’s long edge, so far across its short edge. Two pairs of numbers, one coin, and nothing moved.

Turning the phone’s pair into the desk’s pair is a two-step move. Turn the pair by thirty degrees so it lines up with the desk’s directions, then add on where the phone’s corner sits. Rotate, then translate. That part feels obvious.

Now go the other way. Desk numbers in, phone numbers out. The move your hand reaches for is to run the same two steps backwards: undo the turn, then subtract the corner offset. Both halves are correct operations. That order is not. And the answer it gives is wrong by the same fixed amount for every coin on the desk, which means it never looks like a bug. It looks like somebody mismeasured the desk.

This lesson builds the object that gets both directions right without asking you to remember which step comes first.

The idea in one paragraph

A pose is a rotation together with a translation: which way something faces and where it sits. Carried as two loose variables, poses compose by a formula you have to remember, and re-derive, every time you use it. Carried as one 4 by 4 matrix, with the rotation in the top-left 3 by 3 block, the translation down the last column and the row 0 0 0 1 along the bottom, the same operations collapse into matrix multiplication. Applying a pose to a point is one multiply. Chaining two poses is one multiply. Undoing a pose is a matrix inverse with a two-line closed form. That object is the homogeneous transform, and every robotics library, every simulator, every robot description file and every GPU already speaks it.

The formula nobody remembers correctly

Start with the two-variable form, because you need to feel the problem before the fix means anything.

Frame $\{B\}$ sits somewhere inside frame $\{A\}$. Its axes are turned by a rotation $R$, and its origin sits at offset $\mathbf{t}$. A point whose coordinates in $\{B\}$ are $\mathbf{p}_B$ has coordinates in $\{A\}$:

In words: turn the point’s numbers until they line up with A’s directions, then shift by where B’s origin sits. Exactly what you did with the coin.

Now chain two of them. A point known in $\{C\}$, expressed in $\{B\}$, expressed in $\{A\}$:

So the composed pair is $\left(R_{AB}\,R_{BC},\; R_{AB}\,\mathbf{t}_{BC} + \mathbf{t}_{AB}\right)$. Rotations multiply, which nobody gets wrong. Translations, however, do not simply add.

Pad the point with a 1

Here is the fix, and it is one of those small ideas that quietly runs an entire field.

Stack $R$ and $\mathbf{t}$ into a 4 by 4, and stick a 1 onto the end of the point.

Multiply them and read the rows. Each of the first three rows dots one row of $R$ with $\mathbf{p}_B$, then adds one entry of $\mathbf{t}$ multiplied by that appended 1. Together they are exactly $R\,\mathbf{p}_B + \mathbf{t}$. The fourth row computes $0x + 0y + 0z + 1\cdot 1$, which is 1, so the answer comes out padded the same way the input went in.

The appended 1 is the whole mechanism. A matrix multiply can only add a quantity it is multiplying by something, so a translation has nowhere to live inside a 3 by 3. Give every point a fourth coordinate that is always 1, and the translation column rides into the sum for free.

Why the bottom row is 0 0 0 1

That row looks like padding. It is doing four jobs.

It keeps the shape. Multiply two of these matrices and the product’s bottom row comes out 0 0 0 1 again, with a valid rotation still in the top-left block. Checked on a random pair: bottom row exactly [0 0 0 1], and the composed rotation block satisfied $R^{\top}R = I$. That closure is what lets you chain as many poses as you like and still hold a pose at the end.

It keeps the point legal. The fourth row computes the output’s fourth coordinate. With 0 0 0 1 it copies the incoming 1 straight through, so the result is again a padded point you can feed into the next multiply. Any other bottom row would put something else there, and the next multiply would silently scale your point by it.

It is a switch. The fourth coordinate decides whether the translation applies at all. Pad with 1 and you have a place, which gets turned and then shifted. Pad with 0 and you have a direction, which gets turned and nothing else, because every entry of $\mathbf{t}$ is multiplied by that 0.

That distinction matters more than it looks. Run the 90-degree-about-z transform with offset (1, 0, 0) on the x direction and on the point at the same coordinates:

Velocities, joint axes, surface normals and camera view directions are all directions. Translate one of them by accident and the direction picks up the transform’s offset, so a unit axis comes back with a length that depends on where the frames happen to be. The smaller the offsets in your test scene, the longer it hides.

A different bottom row is a camera. Nothing forces the bottom row to be 0 0 0 1. Put depth there instead and dividing by the fourth coordinate shrinks distant things: that is perspective projection, and it is what a camera matrix does in Module 4. The 4x4 you are learning is the rigid, non-projective corner of a larger family. The family is what “homogeneous” names, and it is why the word attaches to the coordinates rather than to the transform.

Reading $T_{AB}$

The subscripts are a contract, and it is worth fixing now because the next lesson leans on it entirely.

$T_{AB}$ means the pose of frame B, expressed in frame A. Equivalently, and more usefully, it is the function that takes B-coordinates and returns A-coordinates. Outer frame first, inner frame second. In code, put it in the name:

cup_C = np.array([0.05, -0.02, 0.55])   # cup, in CAMERA coordinates
cup_B = apply_T(T_BC, cup_C)            # cup, in BASE coordinates

cup with no suffix is a bug incubator. The suffix is not decoration, it is the only type system you get.

Undoing it

$T_{AB}$ maps B-coordinates to A-coordinates. You constantly need the other direction, and you do not need a general 4x4 inverse to get it. Solve the forward equation for $\mathbf{p}_B$:

Read the last line as a transform and you can see its two parts: the rotation is $R^{\top}$ and the translation is $-R^{\top}\mathbf{t}$.

The $R^{\top}$ sitting in front of $\mathbf{t}$ is not an algebraic flourish. It is the answer to a physical question: where is A’s origin, measured along B’s axes?

What it costs

Sixteen numbers to store six numbers of information. That redundancy has to be paid for somewhere, and it is worth knowing the size of the bill.

The closed form above is not more accurate than a general inverse, it is cheaper and it is guaranteed to stay rigid. Over 10,000 random poses, $\left[R^{\top},\, -R^{\top}\mathbf{t}\right]$ and numpy.linalg.inv agreed to within 2.7e-15, so the reason to write the closed form is not precision. It is that a general solver knows nothing about rotations and can hand you back a matrix that is no longer one, whereas transposing a rotation cannot.

Drift is real and slower than the folklore suggests. Composing a million small transforms in sequence, the rotation block ended up off orthonormal by 2.2e-10, with a determinant of 0.999999999867. Renormalize in a long-running estimator; do not lose sleep over a 6-joint arm.

Review

Check yourself

Do this

Open code/transforms_starter.py. This file becomes your transform library and every later lesson in the module imports it, so it is worth doing properly.

Fill in the three functions this lesson earned: make_T(Rm, t), apply_T(T, p) and inv_T(T). Leave compose and the quaternion helpers alone for now, they land in the next lesson. Write inv_T from the closed form and resist the urge to call np.linalg.inv.

Then run this from the code/ directory. It is the worked example from the gotcha, and the comments are the answers:

import numpy as np
from scipy.spatial.transform import Rotation
from transforms_starter import make_T, apply_T, inv_T

T = make_T(Rotation.from_euler("z", 90, degrees=True).as_matrix(), [1, 0, 0])
p_B = np.array([0.2, 0.3, 0.0])
p_A = apply_T(T, p_B)

print("forward      ", p_A)                        # [0.7 0.2 0. ]
print("back again   ", apply_T(inv_T(T), p_A))     # [0.2 0.3 0. ]

wrong = make_T(T[:3, :3].T, -T[:3, 3])             # the -t version
print("wrong inverse", apply_T(wrong, p_A))        # [-0.8 -0.7  0. ]

for q in np.random.default_rng(0).uniform(-3, 3, (4, 3)):
    print("error", apply_T(wrong, q) - apply_T(inv_T(T), q))   # [-1. -1.  0.] four times

Two things to notice before moving on. The last loop prints the same error four times, because the wrong inverse shifts the whole world by a constant. And if you swap the transform for one with no rotation, or with its translation along z, the wrong inverse stops being wrong. That is the shape of a bug that reaches production.

What you can now do

You can pack a rotation and a translation into a single 4x4, apply it to a point by padding the point with a 1, and say precisely what the bottom row is for: closure under multiplication, keeping the output a legal point, and switching translation on for places and off for directions. You can write the closed-form inverse from memory, explain why its translation is $-R^{\top}\mathbf{t}$ rather than $-\mathbf{t}$ in terms of which frame the offset is measured in, and describe the two test setups in which that bug hides.