Spatial maths

Overview

Every robot computation asks where one thing is relative to another. The gripper relative to the cup, the lidar relative to the base, the robot relative to the map. Spatial maths is the small set of tools that answer those questions without ambiguity.

The maths is old and settled. The bugs come from conventions: which way an axis points, which order a quaternion is stored in, whether a transform maps points from frame A to frame B or the other way. This hub is for anyone who has watched a robot turn left when it should have turned right. It covers the representations, the ROS frame conventions and enough Lie group vocabulary to read estimation and optimisation papers.

Key ideas

Rotations and how to store them

A rotation matrix is a 3 by 3 orthonormal matrix with determinant +1. It has nine numbers but only 3 DoF. It composes by multiplication and inverts by transposition, which makes it the safest form to compute with.

Euler angles store three angles about successive axes. They are compact and readable, but REP 103 discourages them because there are many valid sequences and different fields use different defaults. Every Euler sequence also has a configuration where two axes line up and one angle becomes undefined (gimbal lock).

A unit quaternion stores a rotation in four numbers with no singularity. It is the first choice in REP 103. Two facts cause most quaternion bugs. First, q and -q represent the same rotation, so comparing quaternions element by element can fail on identical rotations. Second, libraries disagree on component order.

The block below shows the same 90° yaw in two orders and composes two transforms.

import numpy as np
from scipy.spatial.transform import Rotation
from spatialmath import SE3, UnitQuaternion

# A 90 degree yaw (rotation about z), written two ways.
r = Rotation.from_euler("z", np.pi / 2)
print("SciPy as_quat (x, y, z, w):", np.round(r.as_quat(), 4))
print("SciPy scalar_first (w, x, y, z):", np.round(r.as_quat(scalar_first=True), 4))
print("spatialmath UnitQuaternion (w, x, y, z):", np.round(UnitQuaternion.Rz(np.pi / 2).vec, 4))

# Compose transforms: T_map_base = T_map_odom @ T_odom_base
T_map_odom = SE3(2.0, 0.0, 0.0) * SE3.Rz(np.pi / 2)
T_odom_base = SE3(1.0, 0.0, 0.0)
T_map_base = T_map_odom * T_odom_base
print("base_link origin in map:", np.round(T_map_base.t, 3))
# The inverse gives the map origin seen from base_link
print("map origin in base_link:", np.round(T_map_base.inv().t, 3))

It prints:

SciPy as_quat (x, y, z, w): [0.     0.     0.7071 0.7071]
SciPy scalar_first (w, x, y, z): [0.7071 0.     0.     0.7071]
spatialmath UnitQuaternion (w, x, y, z): [0.7071 0.     0.     0.7071]
base_link origin in map: [2. 1. 0.]
map origin in base_link: [-1.  2.  0.]

Homogeneous transforms

A homogeneous transformation packs a rotation R and a translation p into one 4 by 4 matrix, so a chain of frames becomes a chain of matrix products. Name each transform by the two frames it relates. In this hub T_a_b is the pose of frame b expressed in frame a, and it maps a point written in b into a. With that naming, inner indices cancel: T_map_odom @ T_odom_base gives T_map_base. The inverse has a closed form: the rotation becomes R transposed and the translation becomes minus R transposed times p. Use it instead of a general matrix inverse.

Frame conventions: REP 103 and REP 105

ROS fixes the conventions in two short documents worth reading in full. REP 103 sets SI units and right-handed frames. Body frames point x forward, y left, z up. Local world frames use east-north-up (ENU). Camera frames with an _optical suffix use z forward, x right, y down, and an _ned suffix marks a north-east-down frame. Yaw increases anticlockwise, and for geographic poses it is zero facing east, which is not a compass bearing.

REP 105 names the frames of a mobile platform. base_link is fixed to the robot. odom is a world-fixed frame in which the robot’s pose is continuous but drifts without bound. map is a world-fixed frame in which the pose should not drift but may jump when localisation corrects it. The tree is map to odom to base_link, because each frame can have only one parent. The localisation component computes the robot’s pose in map but publishes the map to odom transform, leaving odom to base_link to the odometry source. The tf2 library keeps this tree and answers lookups between any two frames at a given time.

Lie groups at a working level

The set of rotations, SO(3), and the set of rigid transforms, SE(3), are Lie groups: smooth spaces where you can compose and invert, but cannot add. Adding two rotation matrices does not give a rotation matrix. Each group has a flat tangent space at the identity, its Lie algebra, where ordinary vector arithmetic works. For SO(3) the tangent vector is a 3-vector, an axis scaled by an angle. For SE(3) it is a 6-vector twist with a linear and an angular part.

Two maps connect them. The exponential map turns a tangent vector into a group element, and the logarithm goes back. That is enough to do most practical work: interpolate between poses, average rotations, write a small pose error as a 6-vector, put a covariance on it, and take gradient steps on a pose without leaving the group. Estimation libraries and the product-of-exponentials kinematics in /topics/kinematics/ both rest on this.

Where to start

Work through /learn/beginner/02-frames-and-rotations/, then keep /reference/rotation-representations/ open while you code. /learn/beginner/10-hello-ros2/ shows tf2 in practice, and the project /projects/ros2-talker-listener-tf2/ builds a small transform tree.

Tools and platforms

Common mistakes

• Passing a w-first quaternion to a w-last API. The result is a valid but wrong rotation, so nothing crashes.
• Reading a transform the wrong way round. Write the frame names into every variable.
• Using Euler angles for interpolation or averaging. Convert to quaternions or work in the tangent space.
• Forgetting the optical frame. An image’s z axis points out of the lens, not up.
• Normalising quaternions only at the end. Small errors compound, so renormalise after each composition.

Going deeper

• REP 103: Standard Units of Measure and Coordinate Conventions and REP 105: Coordinate Frames for Mobile Platforms, ROS Enhancement Proposals.
• Joan Solà, Jeremie Deray and Dinesh Atchuthan, A micro Lie theory for state estimation in robotics, arXiv, 2018. The most readable working introduction.
• Kevin M. Lynch and Frank C. Park, Modern Robotics, Cambridge University Press, 2017. Chapter 3 covers rigid-body motions, twists and exponential coordinates.
• Peter Corke, Robotics, Vision and Control: Fundamental Algorithms in Python, 3rd edition, Springer, 2023. The early chapters cover pose representation in code.