Rigid body motion

A rigid body motion (rigid transformation) preserves distances between all points. In 3D it consists of a rotation and a translation, and the mathematical structures that represent it — $SO(3)$ and $SE(3)$ — are central to SLAM: a camera pose is an element of $SE(3)$ .

Rotation Representations

Euler angles. A rotation can be parameterized by three angles $(\phi, \theta, \psi)$ representing successive rotations about fixed or body axes; the 12 possible conventions (ZYX, XYZ, …) each give different angle sequences. The gimbal lock problem occurs when two rotation axes align, reducing the effective degrees of freedom to 2. Euler angles are intuitive for display but numerically problematic — avoid them for iterative optimization.

Rotation matrices. A rotation matrix satisfies $R^TR = I$ and $\det(R) = +1$ . The set of all such matrices forms the Special Orthogonal Group:

$SO(3) = \{R \in \mathbb{R}^{3\times3} \mid R^TR = I,\ \det(R) = +1\}$

Rotation matrices are the preferred representation for computation: matrix multiplication is the composition law, and the inverse is simply the transpose.

Quaternions. A unit quaternion $q = w + xi + yj + zk$ with $w^2+x^2+y^2+z^2 = 1$ represents a rotation by angle $\theta = 2\arccos(w)$ around axis $[x,y,z]^T/\sin(\theta/2)$ . They are compact (4 numbers vs. 9), have no gimbal lock, and compose via the Hamilton product. Note the double cover: $q$ and $-q$ represent the same rotation.

Conversion from quaternion to rotation matrix:

R = \begin{bmatrix} 1 - 2(y^2+z^2) & 2(xy - wz) & 2(xz + wy) \\ 2(xy + wz) & 1 - 2(x^2+z^2) & 2(yz - wx) \\ 2(xz - wy) & 2(yz + wx) & 1 - 2(x^2+y^2) \end{bmatrix}

A quick NumPy sanity check that a matrix is a valid rotation:

import numpy as np
# R is a rotation iff R^T R = I and det(R) = +1
print(np.allclose(R.T @ R, np.eye(3)), np.isclose(np.linalg.det(R), 1.0))

Homogeneous Transformation: $T \in SE(3)$

A rigid body motion combines rotation $R$ and translation $\mathbf{t}$ into one $4\times4$ matrix acting on homogeneous coordinates:

T = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in SE(3), \qquad T\begin{bmatrix}\mathbf{X}\\1\end{bmatrix} = \begin{bmatrix} R\mathbf{X} + \mathbf{t} \\ 1 \end{bmatrix}

Composition is matrix multiplication, with rotation and translation interleaving:

$T_1 T_2 = \begin{bmatrix} R_1R_2 & R_1\mathbf{t}_2 + \mathbf{t}_1 \\ \mathbf{0}^T & 1 \end{bmatrix}$

— note that $\mathbf{t}_2$ gets rotated by $R_1$ , which is why translations of chained transforms do not simply add. The inverse has the closed form:

$T^{-1} = \begin{bmatrix} R^T & -R^T\mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}$

Homogeneous coordinates come from projective space: appending a 1 (points) or 0 (directions) lets one matrix express rotation and translation together. In projective geometry, parallel 3D lines meet at a vanishing point in the image — a useful cue for rotation estimation in man-made environments and a reminder that image formation is projective, not Euclidean.

Keeping frames straight

The most effective habit is a subscript convention that makes composition self-checking: let $T_{AB}$ map coordinates from frame $B$ to frame $A$ . Then

$T_{AC} = T_{AB}\,T_{BC}, \qquad \mathbf{X}_A = T_{AB}\,\mathbf{X}_B$

and adjacent subscripts must “cancel” like units. If they don’t, the expression is wrong — this catches a remarkable fraction of pose-arithmetic bugs at a glance. Be explicit about whether a stored “camera pose” means $T_{world,cam}$ (camera-to-world, the camera’s position in the world) or its inverse $T_{cam,world}$ (the extrinsics used in projection); both conventions are common across codebases.

Common pitfalls

Hamilton vs. JPL quaternion conventions: two incompatible definitions of quaternion multiplication coexist in the literature (Eigen and ROS use Hamilton); mixing them silently conjugates rotations.
Quaternion storage order: (w, x, y, z) vs. (x, y, z, w) differs between libraries (e.g., Eigen’s constructor vs. its internal layout, ROS messages); always test with a known rotation.
Drifting off the manifold: repeatedly multiplying rotation matrices or integrating quaternions accumulates numerical error; re-orthonormalize matrices (e.g., via SVD) or re-normalize quaternions periodically.
Sign flips under the double cover: interpolating between $q$ and a nearby $-q'$ takes the long way around; flip the sign first if the dot product is negative.

Why it matters for SLAM

Every SLAM system is, at bottom, estimating a trajectory of $SE(3)$ elements. Chaining relative motions ( $T_{world,cam} = T_{world,kf}\,T_{kf,cam}$ ), inverting transforms, and converting between quaternions (compact storage, ROS messages) and rotation matrices (computation) are daily operations. Getting frame conventions right — which frame a transform maps from and to — is the most common source of bugs for newcomers, so nail this before moving on.