A pose is not a vector

Each step uses the one before it. If you are short of time, do not skip Step 5. It puts every idea from the lecture on a circle, where you can see all of it at once, and the 3D steps after it repeat that picture with bigger matrices.

Each step ends with a few questions. Answer them before you open them. Where an outside tutorial explains the same point well, a Read elsewhere box names the exact figure, section or video. The slide titles each step backs up are listed under its heading.

The figures use colour the same way every time:

Look at the update step of every estimator you have used:

Each one adds vectors and scales them. A robot's orientation looks like it should work the same way, since it has three degrees of freedom. It does not work, and there are three separate reasons.

Reason 1: the order of rotations matters

Take a book lying flat. Turn it 90° about the world $x$ axis, then 90° about the world $y$ axis. Start again, and do the same two turns in the other order. The book ends up in two different positions:

Reason 2: the sum of two rotations is not a rotation

In 2D, add the rotation by 0° to the rotation by 90°:

The result rotates by 45°, but it also stretches everything by $\sqrt2$, so it is not a rotation. The lecture did the 3D version: the average of the 0° and 180° turns about $z$ has determinant 0. It flattens space onto a line.

Reason 3: every three-number description breaks somewhere

Roll, pitch and yaw lose a degree of freedom at pitch $\pm90^\circ$, where roll and yaw turn about the same axis. This is gimbal lock. Choosing different angles does not fix it. Step 4 shows why with a globe: any single flat coordinate system laid over a shape that closes on itself has a seam somewhere.

So we need three things, and the rest of this tutorial supplies them:

1. Rules for what you may do with rotations. That is the group.
2. A place where adding and scaling are legal again. That is the tangent space.
3. A way to move between the two. That is $\exp$ and $\mathrm{Log}$.

A group is a set together with one way of combining two of its members, obeying four rules. The rules are not arbitrary. Each one is something you rely on every time you chain transforms in code:

Two things are not on the list. Commutativity: $ab$ need not equal $ba$, and for 3D rotations it usually does not. Adding and scaling: a group has exactly one operation, and for rotations that operation is composition. “Half of $\mathbf{R}$” or “$\mathbf{R}_1+\mathbf{R}_2$” are not group operations.

Example: the four rotations of a square

Take a square with one corner marked and rotate it counter-clockwise by 0°, 90°, 180° or 270°. Each rotation puts the square back on its own outline, so these four are its symmetries. Combine two of them by doing one after the other.

The table lists every combination. Read the row, then the column: the entry is the row rotation followed by the column rotation.

You can check three of the rules by reading the table. Every entry is one of the four rotations, so the set is closed. The r0 row repeats the header, so r0 is the identity. The identity appears exactly once in every row, highlighted, so every rotation has an inverse: 90° is undone by 270°. Associativity comes for free, because doing one thing after another is always associative.

Example: all rotations of the plane

Let the angle be anything, not just multiples of 90°. Combining two rotations adds their angles, wrapping at 360°. The identity is 0° and the inverse of $\theta$ is $-\theta$. This group is called $\mathrm{SO}(2)$. Its members also form a circle, and that circle is what leads to manifolds in Step 4.

Two sets that are not groups

• Rotation matrices under addition: closure fails, as Step 1 showed.
• Integers under multiplication: the identity is 1, but 2 has no integer inverse.

Read $\mathbf{R}^\top\mathbf{R}=\mathbf{I}$ one column at a time. Entry $(i,j)$ of $\mathbf{R}^\top\mathbf{R}$ is the dot product of column $i$ with column $j$. So the equation says each column has length 1, and any two columns are perpendicular.

The columns also have a physical meaning: column $j$ is where the $j$-th axis ends up. Here is the rotation by 90° about $z$:

Counting degrees of freedom

The matrix has nine entries. Written out, $\mathbf{R}^\top\mathbf{R}=\mathbf{I}$ is six equations, not nine, because the $(i,j)$ and $(j,i)$ entries give the same equation:

The condition $\det\mathbf{R}=+1$ removes no dimensions. It throws out mirror images, such as $\mathrm{diag}(1,1,-1)$, which passes the column test but turns a right hand into a left hand.

Every representation stores the same three degrees of freedom

The group is the same in every row. Only the bookkeeping changes, and every choice pays for its convenience somewhere.

A manifold is a set that looks like ordinary flat $\mathbb{R}^n$ if you zoom in close enough around any of its points. The number $n$ is its dimension. The standard picture is the Earth. A street map of your town is flat and works perfectly. A map of the whole planet cannot be flat without stretching or tearing somewhere.

Three things carry over from the globe to rotations.

1. Dimension counts the directions you can move in, not the numbers you store. A point on the sphere is stored as $(x,y,z)$, but you can only move north–south and east–west, so the sphere is 2-dimensional. A rotation is stored as nine numbers and has three directions to move in.
2. The flat map at a point is the tangent space. Distances, directions, averages and derivatives all work on the flat map, and you can trust them near the point where it touches. Calculus on a manifold is calculus on these maps.
3. Every single global coordinate system has a bad spot. At the North Pole every longitude names the same point, and one small step can change longitude by 180°. Euler angles fail the same way at pitch $\pm90^\circ$. No choice of three angles avoids it, because SO(3), like the sphere, closes back on itself, and a flat grid laid over it must have a seam.

From the circle up to poses

A manifold that is also a group, where the operation is smooth, is a Lie group. SO(3) and SE(3) are Lie groups. So “SO(3) is a group” and “SO(3) is a manifold” are both true, and they describe different things.

“Drifting off the manifold”

Think of the constraints as carving out a surface. $x^2+y^2=1$ picks a circle out of the plane, and $\mathbf{R}^\top\mathbf{R}=\mathbf{I}$ picks a 3-dimensional surface out of the space of $3\times3$ matrices. A matrix that no longer satisfies the constraint is a point off that surface. Multiplying rotations in floating point creeps off it very slowly. Adding a correction jumps off it immediately:

Each added step stretches the matrix by a factor of $\sqrt{1+\mathrm{d}t^2}$. That is tiny per step, but it never averages out. Step 5 lets you watch it happen.

The group, and the manifold

A 2D rotation and its first column:

So every rotation is a point on the circle, and the circle is the manifold. Here are the group rules on it:

• Compose: $\mathbf{R}(a)\mathbf{R}(b)=\mathbf{R}(a+b)$. Angles add, and the result wraps around the circle.
• Identity: $\mathbf{R}(0)=\mathbf{I}$, the point $(1,0)$.
• Inverse: $\mathbf{R}(\theta)^{-1}=\mathbf{R}(-\theta)=\mathbf{R}(\theta)^\top$, the mirror-image point below the axis.

The tangent space, and the hat map

Move along the circle through the identity and ask for your velocity. Differentiate $\mathbf{R}(\theta)$ at $\theta=0$:

Every velocity at the identity is a multiple of $\mathbf{J}$. These are the skew-symmetric $2\times2$ matrices, and each one is described by a single number $\omega$. Geometrically they form the vertical line touching the circle at $(1,0)$. The hat map is $\omega^\wedge = \omega\mathbf{J}$, turning the number into the matrix. On this line, adding and scaling are fine.

exp wraps the line around the circle

Put $\theta\mathbf{J}$ into the power series for the exponential. Since $\mathbf{J}^2=-\mathbf{I}$, the powers cycle through $\mathbf{I},\mathbf{J},-\mathbf{I},-\mathbf{J}$. Group the terms:

So $\exp$ takes a distance $\theta$ along the tangent line and wraps it onto the circle as an arc of the same length. $\mathrm{Log}$ unwraps it: $\mathrm{Log}(\mathbf{R}) = \mathrm{atan2}(R_{21}, R_{11})$, which always returns an angle in $(-\pi,\pi]$.

Now try it. Pick an estimate $\bar{\mathbf{R}}$ and a correction $\tau$. The figure applies the correction in two ways: by adding $\tau$ times the tangent direction to the matrix, and by composing with $\mathrm{Exp}(\tau)$.

Plus and minus

Every estimator needs two things: apply a correction and measure a difference. On a group they become:

Headings make the difference easy to see. Take 10° minus 350°. As plain numbers it is $-340^\circ$. On the circle it is $\mathrm{Log}\big(\mathbf{R}(-350^\circ)\mathbf{R}(10^\circ)\big) = \mathrm{Log}\big(\mathbf{R}(-340^\circ)\big) = +20^\circ$, because $\mathrm{Log}$ returns the short way round. A compass heading filter that forgets this turns the robot the long way.

Averaging, the right way

Three heading measurements: 350°, 10° and 20°. Averaged as numbers they give $(350+10+20)/3 = 126.7^\circ$, which points almost backwards. The correct method uses the tangent-space idea:

1. Pick a starting guess, say $\bar{\mathbf{R}} = 350^\circ$.
2. Unwrap each measurement onto the tangent line there: $\mathbf{R}_i \ominus \bar{\mathbf{R}} = 0^\circ,\ 20^\circ,\ 30^\circ$.
3. Average on the line, where adding is legal: $16.7^\circ$.
4. Wrap back onto the circle: $\bar{\mathbf{R}} \leftarrow \bar{\mathbf{R}} \oplus 16.7^\circ = 6.7^\circ$.
5. Repeat. The unwrapped values are now $-16.7^\circ, 3.3^\circ, 13.3^\circ$, which average to 0, so it has converged.

Keep this loop in mind. Step 11 shows that pose-graph optimisation, PnP and ICP all have the same structure: unwrap onto the tangent space, solve there, wrap back.

Repeat Step 5's derivative in 3D. Let $\mathbf{R}(t)$ be any smooth path of rotations through the identity, so $\mathbf{R}(0)=\mathbf{I}$. The constraint holds at every instant, so its derivative is zero:

So the velocities at the identity are exactly the skew-symmetric matrices. A skew-symmetric $3\times3$ matrix has zeros on the diagonal, and the lower triangle is minus the upper, which leaves three free numbers. The hat map packs them in and the vee map takes them out:

Check the last identity on the first row: $-\omega_3x_2+\omega_2x_3$ is the first component of $\boldsymbol\omega\times\mathbf{x}$. So $\boldsymbol\omega$ is an ordinary angular velocity, and $\boldsymbol\omega^\wedge$ is the matrix that gives the velocity of every point of a body spinning at that rate:

Tangent spaces at other points

At a rotation $\bar{\mathbf{R}}$ that is not the identity, the velocities are $\bar{\mathbf{R}}\boldsymbol\omega^\wedge$. Differentiating $\bar{\mathbf{R}}\exp(t\boldsymbol\omega^\wedge)$ at $t=0$ gives that. It is the same three-number space carried over to $\bar{\mathbf{R}}$ by the group, which is why only the one at the identity gets a name. In this course's convention, corrections multiply on the right, $\mathbf{R} = \bar{\mathbf{R}}\,\mathrm{Exp}(\boldsymbol\omega)$, so $\boldsymbol\omega$ is expressed in the body's own frame.

Spin at a constant angular velocity $\boldsymbol\omega$, measured in the body frame. The orientation changes at a rate proportional to itself, which is a linear ODE with constant coefficients. Its solution is an exponential, as it would be for a scalar:

Read $\mathrm{Exp}(\boldsymbol\omega)$ as: spin about the axis $\boldsymbol\omega/\|\boldsymbol\omega\|$ at $\|\boldsymbol\omega\|$ rad/s for one second, then stop. So the vector $\boldsymbol\omega$ is the axis times the angle. Following Solà, capital $\mathrm{Exp}$ and $\mathrm{Log}$ act on the vector directly, and lowercase $\exp$ acts on the matrix, so $\mathrm{Exp}(\boldsymbol\omega) = \exp(\boldsymbol\omega^\wedge)$. Summing the series the way Step 5 did gives Rodrigues' formula, with $\theta=\|\boldsymbol\omega\|$ and $\mathbf{K}=(\boldsymbol\omega/\theta)^\wedge$ for the unit axis:

Worked example: Exp

Take $\boldsymbol\omega = (0,0,\pi/2)$, a quarter turn about $z$. Then $\theta=\pi/2$, $\sin\theta=1$ and $1-\cos\theta=1$:

Worked example: Log

Go backwards. The angle comes from the trace and the axis from the antisymmetric part:

Two facts to carry away

• For small angles, $\mathrm{Exp}(\boldsymbol\omega) \approx \mathbf{I} + \boldsymbol\omega^\wedge$. That is “adding a correction”. It is correct to first order, and Step 5 showed what the second-order error does over many steps. Point-to-plane ICP linearises this way to build its Jacobian, then applies the step it solves for by composing.
• The angle between two rotations is $\|\mathrm{Log}(\mathbf{R}_1^\top\mathbf{R}_2)\| = \|\mathbf{R}_2\ominus\mathbf{R}_1\|$. That is what a rotation error is. From the trace it is $\arccos\big((\operatorname{tr}(\mathbf{R}_1^\top\mathbf{R}_2)-1)/2\big)$; clip the argument to $[-1,1]$ first, because round-off can push it just outside.

SE(3) is a group under matrix multiplication and a 6-dimensional manifold. Course convention: $\mathbf{T}_{wc}$ maps points from $c$ into $w$, so the subscripts cancel when adjacent: $\mathbf{T}_{wb}\mathbf{T}_{bl}=\mathbf{T}_{wl}$.

Worked example: a lidar on a robot

The robot base is at $(2,1,0)$ in the world, turned 90° left, so it faces world $+y$. The lidar is mounted 0.5 m ahead of the base centre and 0.3 m up, not rotated. It sees a point 10 m straight ahead. Where is that point in the world?

The point is ten metres along world $+y$, the way the robot faces. Two checks:

• Order: the wrong product, $\mathbf{T}_{bl}\mathbf{T}_{wb}$, has translation $(2.5,1,0.3)$. It moves the lidar 0.5 m along world $x$ instead of along the robot's heading.
• Inverse: $\mathbf{T}_{wl}^{-1}$ has translation $-\mathbf{R}_z(-90^\circ)(2,1.5,0.3) = (-1.5,\,2,\,-0.3)$. Applying it to $(2,11.5,0.3)$ gives $(11.5,-2,0.3)+(-1.5,2,-0.3)=(10,0,0)$, back where we started.

The tangent space of SE(3): twists

A velocity on SE(3) is a twist, $\boldsymbol\xi = (\mathbf{v},\boldsymbol\omega)\in\mathbb{R}^6$: a linear velocity and an angular velocity, with translation first in this course. The hat map gives a $4\times4$ matrix, and $\exp$ of it is where you are after one second of moving with that twist:

The translation is $\mathbf{V}\mathbf{v}$ rather than $\mathbf{v}$ because the body keeps turning while it moves, so $\mathbf{v}$, fixed in the body frame, keeps pointing somewhere new. A car makes this concrete.

Worked example: one second of a turning car

Drive forward at 1 m/s while turning left at 90°/s: $\mathbf{v}=(1,0,0)$, $\boldsymbol\omega=(0,0,\pi/2)$. The car follows a circle of radius $\|\mathbf{v}\|/\|\boldsymbol\omega\| = 2/\pi = 0.637$ m, and after one second it has gone a quarter of the way round.

You can check it with the formula: $\theta=\pi/2$, $\frac{1-\cos\theta}{\theta}=0.637$ and $\frac{\theta-\sin\theta}{\theta}=0.363$. With $\mathbf{K}\mathbf{v}=(0,1,0)$ and $\mathbf{K}^2\mathbf{v}=(-1,0,0)$, $\mathbf{V}\mathbf{v} = (1,0,0) + 0.637\,(0,1,0) - 0.363\,(1,0,0) = (0.637,\,0.637,\,0)$. The circle and the matrix agree.

Log on SE(3)

$\mathrm{Log}$ runs the same steps backwards. Take $\boldsymbol\omega$ from the rotation block with the SO(3) Log from Step 7, then undo $\mathbf{V}$ on the translation, $\mathbf{v}=\mathbf{V}^{-1}\mathbf{t}$, with

$\mathbf{V}$ and $\mathbf{V}^{-1}$ both turn into $0/0$ as $\theta\to0$. Below a small threshold, switch to their Taylor series: $\mathbf{V}\approx\mathbf{I}+\tfrac12\boldsymbol\omega^\wedge+\tfrac16(\boldsymbol\omega^\wedge)^2$ and $\mathbf{V}^{-1}\approx\mathbf{I}-\tfrac12\boldsymbol\omega^\wedge+\tfrac1{12}(\boldsymbol\omega^\wedge)^2$. Above it, write $1-\cos\theta$ as $2\sin^2(\theta/2)$. The two expressions are equal, but the first cancels in floating point: at $\theta=10^{-7}$ it loses two percent, enough to put a visible error in the translation. The reference code does both.

An estimator should report “my best pose is $\bar{\mathbf{T}}$, and here is how unsure I am”. The course writes that as:

The mean is a pose on the manifold. The noise is an ordinary Gaussian on six ordinary numbers, in the tangent space at the mean. Every sample $\bar{\mathbf{T}}\,\mathrm{Exp}(\boldsymbol\xi)$ is a valid pose. The two alternatives show why the course does it this way.

• A Gaussian on the matrix entries: a sample from it has entries nudged independently, so it is not a rotation at all: it fails the column test from Step 3. Most directions in 12-number space point off the manifold.
• A Gaussian on $(x,y,\theta)$: this is closer, since every sample is at least a pose. It still puts probability where the robot cannot be, as the next figure shows.

The figure simulates 400 robots, each driving 1 m along $x$ in 40 small steps. Each starts with a slightly wrong heading and picks up a little more heading error at every step. Fig. 1 of the “banana distribution” paper shows the same effect for a differential-drive robot. Fit each model to the simulated robots, then draw new samples from the model and see where they land.

In 3D the covariance is $6\times6$. In the course's $(\mathbf{v},\boldsymbol\omega)$ ordering, the top-left $3\times3$ block is position uncertainty in metres squared, expressed in the frame of $\bar{\mathbf{T}}$. The bottom-right block is orientation uncertainty in radians squared, and the off-diagonal blocks say how the two are correlated. The bend in the banana is that correlation, between sideways position and heading.

A tangent vector is always expressed in some frame. The same small motion looks different from somewhere else. Turn your wrist slightly and your fingertips move sideways. The adjoint converts a twist from one frame into another:

The top-right block $\mathbf{t}^\wedge\mathbf{R}$ is the lever arm. It turns rotation in one frame into translation in the other, and it grows with the offset $\mathbf{t}$ between them.

Worked example: base yaw error, seen at the sensor

A sensor sits 2 m ahead of the base, not rotated: $\mathbf{T}_{bs} = [\,\mathbf{I}\mid(2,0,0)\,]$. The base pose has a small yaw error, $\mathbf{T}_{wb} = \bar{\mathbf{T}}_{wb}\,\mathrm{Exp}(\boldsymbol\delta)$ with $\boldsymbol\delta = (0,0,0,\ 0,0,\delta\psi)$. What does that do to the sensor pose?

The sensor turns by the same $\delta\psi$ and also slides sideways by $2\,\delta\psi$. For covariances the rule is $\boldsymbol\Sigma_s = \mathrm{Ad}_{\mathbf{T}_{bs}}^{-1}\,\boldsymbol\Sigma_b\,\mathrm{Ad}_{\mathbf{T}_{bs}}^{-\top}$, so $\sigma_{y,s} = 2\,\sigma_\psi$. With $\sigma_\psi = 0.5^\circ = 8.73\times10^{-3}$ rad, the sensor's sideways uncertainty is 1.7 cm.

Chains: why odometry drifts even with a perfect sensor

Odometry chains many relative poses: each step is measured from the previous one, and each step's uncertainty is carried forward through the adjoint of everything after it. A heading error early on acts as a lever arm for every metre that follows. Suppose each metre of driving adds $0.1^\circ$ of independent heading noise. After $N$ metres the heading has wandered by $0.1^\circ\sqrt{N}$, and the sideways position error is $\sigma_\psi\sqrt{\sum_{j=1}^{N} j^2} \approx \sigma_\psi\sqrt{N^3/3}$:

Doubling the distance multiplies position error by almost three ($2^{1.5}$), even though nothing about the sensor changed. The growth comes from integrating the steps, so a better sensor only slows it. Only an observation of something fixed, such as a loop closure, removes it.

In code the adjoint is four lines. The reference implementation has it, along with a numerical check of the identity that defines it.

Every pose optimiser in this course, including PnP, ICP, bundle adjustment and the pose graph, runs the same three-line loop. You have already run it once, on headings, in Step 5:

1. Unwrap. Write the cost as a function of a small twist around the current estimate, $f(\bar{\mathbf{T}}\oplus\boldsymbol\xi)$, and linearise it in $\boldsymbol\xi$: $\mathbf{r}(\bar{\mathbf{T}}\oplus\boldsymbol\xi)\approx\mathbf{r}(\bar{\mathbf{T}})+\mathbf{J}\boldsymbol\xi$.
2. Solve for the best $\boldsymbol\xi$ in the tangent space, where it is six unconstrained numbers: $\mathbf{J}^\top\mathbf{J}\,\boldsymbol\xi = -\mathbf{J}^\top\mathbf{r}$.
3. Wrap back by composing: $\bar{\mathbf{T}}\leftarrow\bar{\mathbf{T}}\,\mathrm{Exp}(\boldsymbol\xi)$. Repeat until $\boldsymbol\xi$ is tiny.

Averaging rotations is the smallest non-trivial case. The residual of measurement $i$ is $\mathbf{R}_i\ominus\bar{\mathbf{R}}$, its Jacobian is approximately the identity, and the Gauss–Newton step is just the mean of the unwrapped residuals:

Three properties follow from doing it this way:

• The unknowns are unconstrained. The solver works with six free numbers, not twelve matrix entries tied together by six equations.
• Every iterate is a valid pose. Composing never leaves the group, so there is no re-orthogonalising, and the estimate never drifts off the manifold.
• No singularities. The tangent space is always taken at the current estimate, so the problem is always near its origin, far from any seam. No Euler-angle singularity can appear.

The code is written to be read. Each extra branch covers a place where the textbook formula goes wrong in floating point: small angles, where $1-\cos\theta$ cancels, and angles near $\pi$, where $\mathbf{R}-\mathbf{R}^\top$ vanishes. It was checked against a brute-force matrix exponential at angles from 0 to $\pi$, including $10^{-12}$ and $\pi-10^{-7}$. Every round trip agrees to better than $10^{-12}$. The last lines reproduce the car from Step 8, the 180° rotation from Step 7, and the identity that defines the adjoint in Step 10.

To see the lever arm from Step 10 in numbers, put a sensor 2 m ahead of the base and push a small base yaw through the inverse adjoint: