Lie Groups SO(3) and SE(3) for Robotics

Rotations and poses do not live on a flat grid. A small heading error far from the start swings the position along an arc. Lie groups give a principled way to describe uncertainty on this curved space. This page shows it in the plane with SE(2), the 2D cousin of SE(3).

Heading noise per step Forward noise Distance driven Gaussian fit in x and y (gray) Gaussian fit in the Lie algebra (red)

The exponential and log maps

A pose in SE(2) is a rotation plus a translation. Its Lie algebra is the flat space of twists $\boldsymbol{\xi} = (v_x, v_y, \omega)$. The exponential map turns a twist into a pose, and the log map goes back:

$$\exp(\boldsymbol{\xi}) = \begin{bmatrix} R(\omega) & V(\omega)\,\mathbf{v} \\ 0 & 1 \end{bmatrix}, \qquad V(\omega) = \frac{1}{\omega}\begin{bmatrix} \sin\omega & -(1-\cos\omega) \\ 1-\cos\omega & \sin\omega \end{bmatrix}$$

The trick is to take the log of every pose, measured from the start, fit the Gaussian there in the flat Lie algebra, then map samples back to poses:

$$\boldsymbol{\xi}_i = \log\left(T_i\right), \qquad \boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{\mu}, \Sigma), \qquad T = \exp(\boldsymbol{\xi})$$

What to notice

The black dots are where the robot might really be. They bend into a banana, because heading errors early in the drive swing the robot sideways later. The gray fit treats x and y as flat and gives an ellipse that puts much of its probability in empty space, inside the curve where the robot could never be. The red fit, made in the Lie algebra, bends to match the real cloud. The readout shows why: the robot always ends about the same distance from the start, but the center of the gray fit sits closer in, at a place the robot never ends up. The center of the red fit stays much closer to the arc.

This is why modern estimators such as factor graph SLAM and on-manifold Kalman filters work on SO(3) and SE(3) directly. In 3D the ideas are the same with rotation matrices or quaternions.