Phase VI — Lie Groups & Manifold Optimization | Week 11 | 2.5 hours Four numbers, zero gimbal lock, double the coverage.
ROS tf2 represents all orientations as quaternions [x, y, z, w]. The OKS IMU publishes orientation in quaternion form. Every geometry_msgs/Quaternion in the navigation stack is a unit quaternion. Understanding the double-cover property explains why q and -q represent the same rotation — a frequent source of sign-flip bugs in sensor fusion.
A quaternion is $q = w + xi + yj + zk$ where $i, j, k$ satisfy:
$$i^2 = j^2 = k^2 = ijk = -1$$
From which: $ij = k$, $jk = i$, $ki = j$, and $ji = -k$, $kj = -i$, $ik = -j$.
We write $q = (w, \mathbf{v})$ with scalar part $w$ and vector part $\mathbf{v} = (x, y, z)$.
Quaternion multiplication:
$$(w_1, \mathbf{v}_1)(w_2, \mathbf{v}_2) = (w_1 w_2 - \mathbf{v}_1 \cdot \mathbf{v}_2,\; w_1\mathbf{v}_2 + w_2\mathbf{v}_1 + \mathbf{v}_1 \times \mathbf{v}_2)$$
Note: multiplication is non-commutative (due to the cross product term).
A unit quaternion has $\|q\| = \sqrt{w^2 + x^2 + y^2 + z^2} = 1$.
For axis $\hat{n}$ and angle $\theta$:
$$q = \left(\cos\frac{\theta}{2},\; \sin\frac{\theta}{2}\,\hat{n}\right)$$
The rotation of vector $\mathbf{p}$ is: $\mathbf{p}' = q \mathbf{p} q^*$ (sandwich product), where $q^* = (w, -\mathbf{v})$ is the conjugate.
Both $q$ and $-q$ represent the same rotation:
$$q \mathbf{p} q^* = (-q) \mathbf{p} (-q)^*$$
This means unit quaternions form a double cover of SO(3): the map $\phi: S^3 \to \text{SO}(3)$ is 2-to-1. Topologically, SO(3) $\cong$ $S^3 / \{q \sim -q\} \cong \mathbb{RP}^3$.
Quaternion → matrix:
$$R = \begin{pmatrix} 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{pmatrix}$$
Matrix → quaternion (Shepperd's method): compute from the trace and diagonal entries, choosing the numerically stable branch.
| Property | Euler Angles | Quaternions |
|---|---|---|
| Parameters | 3 | 4 (with 1 constraint) |
| Singularities | Gimbal lock at $\pm 90°$ pitch | None |
| Composition | Trig functions | Simple multiply |
| Interpolation | Non-trivial | SLERP (geodesic) |
| Normalization | N/A | Normalize to unit length |
import numpy as np
from code.lie_groups.lie_groups import (
angle_axis_to_quaternion, quaternion_to_rotation,
so3_exp, so3_log
)
def quaternion_multiply(q1, q2):
"""Multiply two quaternions [w, x, y, z]."""
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
return np.array([
w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2
])
def quaternion_conjugate(q):
"""Conjugate: (w, -v)."""
return np.array([q[0], -q[1], -q[2], -q[3]])
def quaternion_rotate_vector(q, v):
"""Rotate vector v by unit quaternion q: q v q*."""
p = np.array([0.0, v[0], v[1], v[2]])
result = quaternion_multiply(quaternion_multiply(q, p), quaternion_conjugate(q))
return result[1:]
def rotation_matrix_to_quaternion(R):
"""Shepperd's method: SO(3) → unit quaternion [w,x,y,z]."""
tr = np.trace(R)
if tr > 0:
s = 2.0 * np.sqrt(tr + 1.0)
w = 0.25 * s
x = (R[2,1] - R[1,2]) / s
y = (R[0,2] - R[2,0]) / s
z = (R[1,0] - R[0,1]) / s
elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
s = 2.0 * np.sqrt(1.0 + R[0,0] - R[1,1] - R[2,2])
w = (R[2,1] - R[1,2]) / s
x = 0.25 * s
y = (R[0,1] + R[1,0]) / s
z = (R[0,2] + R[2,0]) / s
elif R[1,1] > R[2,2]:
s = 2.0 * np.sqrt(1.0 + R[1,1] - R[0,0] - R[2,2])
w = (R[0,2] - R[2,0]) / s
x = (R[0,1] + R[1,0]) / s
y = 0.25 * s
z = (R[1,2] + R[2,1]) / s
else:
s = 2.0 * np.sqrt(1.0 + R[2,2] - R[0,0] - R[1,1])
w = (R[1,0] - R[0,1]) / s
x = (R[0,2] + R[2,0]) / s
y = (R[1,2] + R[2,1]) / s
z = 0.25 * s
q = np.array([w, x, y, z])
return q / np.linalg.norm(q)
# --- Basic quaternion operations ---
omega_z = np.array([0, 0, np.pi / 2]) # 90° around z
q = angle_axis_to_quaternion(omega_z)
print("=== Axis-angle → Quaternion ===")
print(f"ω = [0, 0, π/2] → q = [{q[0]:.4f}, {q[1]:.4f}, {q[2]:.4f}, {q[3]:.4f}]")
print(f"Expected: [cos(π/4), 0, 0, sin(π/4)] = [{np.cos(np.pi/4):.4f}, 0, 0, {np.sin(np.pi/4):.4f}]")
# --- Quaternion rotation of a vector ---
v = np.array([1.0, 0.0, 0.0])
v_rot = quaternion_rotate_vector(q, v)
print(f"\n=== Rotating [1,0,0] by 90° around z ===")
print(f"Result: {np.round(v_rot, 4)} (expected: [0, 1, 0])")
# --- Double cover demonstration ---
q_neg = -q
R_pos = quaternion_to_rotation(q)
R_neg = quaternion_to_rotation(q_neg)
print(f"\n=== Double Cover ===")
print(f"q and -q give same R? {np.allclose(R_pos, R_neg)}")
# --- Composition via quaternion multiply ---
q1 = angle_axis_to_quaternion([np.pi/4, 0, 0]) # 45° around x
q2 = angle_axis_to_quaternion([0, np.pi/4, 0]) # 45° around y
q12 = quaternion_multiply(q1, q2)
R12 = quaternion_to_rotation(q12)
R_direct = so3_exp([np.pi/4, 0, 0]) @ so3_exp([0, np.pi/4, 0])
print(f"\n=== Composition ===")
print(f"Quaternion multiply matches matrix multiply? {np.allclose(R12, R_direct)}")
# --- Roundtrip: R → q → R ---
R_test = so3_exp([0.3, -0.7, 1.2])
q_test = rotation_matrix_to_quaternion(R_test)
R_back = quaternion_to_rotation(q_test)
print(f"\n=== Roundtrip R → q → R ===")
print(f"Max error: {np.max(np.abs(R_test - R_back)):.2e}")
Problem 1: Convert a $180°$ rotation around the axis $(1, 0, 0)$ to a quaternion. What is $q$? What is $-q$? Verify both give the same rotation matrix.
Problem 2: Compute the quaternion product of $q_1 = (\frac{\sqrt2}{2}, \frac{\sqrt2}{2}, 0, 0)$ and $q_2 = (\frac{\sqrt2}{2}, 0, \frac{\sqrt2}{2}, 0)$ by hand.
Problem 3: Why does ROS use the convention [x, y, z, w] while most math textbooks use [w, x, y, z]? Does it affect any computation?
Challenge 1: Prove that the set of unit quaternions $S^3$ with quaternion multiplication forms a group.
Challenge 2: Derive the quaternion-to-rotation-matrix formula from the sandwich product $q \mathbf{p} q^*$.
Challenge 3: The quaternion group and SO(3) have different topologies. Explain why a continuous path from $q$ to $-q$ on $S^3$ corresponds to a loop in SO(3) (the "plate trick" or "belt trick").
tf2::Quaternion in the OKS stack is a unit quaternion; IMU drivers publish orientations as quaternionsq and -q encode the same rotation