8  3D Rotations and SO(3)

8.1 Rotation Matrices: SO(3)

A rotation matrix is an orthogonal matrix with determinant \(+1\). The set of all such \(3 \times 3\) matrices is the Special Orthogonal group:

\[\text{SO}(3) = \{ R \in \mathbb{R}^{3 \times 3} \mid R^T R = I,\; \det R = +1 \}\]

The two conditions carry precise geometric meaning: - \(R^T R = I\): columns are orthonormal — each column is a unit vector orthogonal to all others. Equivalently, \(R\) preserves lengths (\(\|R\mathbf{x}\| = \|\mathbf{x}\|\)) and dot products (\(R\mathbf{x} \cdot R\mathbf{y} = \mathbf{x} \cdot \mathbf{y}\)). - \(\det R = +1\): right-handed orientation is preserved (no reflection).

A matrix with \(R^T R = I\) but \(\det R = -1\) is an improper rotation — it reflects the coordinate frame. SO(3) excludes reflections.

---

8.1.1 Why SO(3) Has Only 3 Degrees of Freedom

A general \(3 \times 3\) matrix has 9 free parameters. The constraint \(R^TR = I\) provides 6 independent equations (one per entry of the symmetric matrix \(R^TR\) on and above the diagonal). So:

\[9 \text{ parameters} - 6 \text{ constraints} = 3 \text{ degrees of freedom}\]

Those 3 DoF correspond to the 3 angles that describe any rigid rotation in space (e.g., roll, pitch, yaw). The matrix representation uses 9 numbers to encode what is fundamentally a 3-dimensional object — this over-parameterisation is the source of numerical drift in long simulation chains.

import numpy as np

def Rx(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])   # shape (3, 3)

def Ry(b):
    c, s = np.cos(b), np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])   # shape (3, 3)

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])   # shape (3, 3)

# Compose three elemental rotations
R = Rz(0.4) @ Ry(0.2) @ Rx(0.3)   # shape (3, 3)

print("Rotation matrix R:")
print(R.round(6))
print(f"\nR^T R = I?   {np.allclose(R.T @ R, np.eye(3))}")
print(f"det(R) = {np.linalg.det(R):.8f}  (should be exactly +1)")

Rotation matrix R:
[[ 0.902701 -0.317949  0.289895]
 [ 0.381656  0.902786 -0.198282]
 [-0.198669  0.289629  0.936293]]

R^T R = I?   True
det(R) = 1.00000000  (should be exactly +1)

---

8.1.2 The Group Structure: Why Composition Stays in SO(3)

SO(3) is a group: it is closed under multiplication and inversion.

• Closure: if \(R_1, R_2 \in \text{SO}(3)\), then \(R_1 R_2 \in \text{SO}(3)\).
• Identity: \(I \in \text{SO}(3)\).
• Inverse: \(R^{-1} = R^T \in \text{SO}(3)\).

import numpy as np

def Ry(b):
    c, s = np.cos(b), np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

R1 = Ry(np.pi / 5)   # shape (3, 3)
R2 = Rz(np.pi / 7)   # shape (3, 3)

R12 = R1 @ R2   # shape (3, 3)

print("Closure — R1 @ R2 still in SO(3):")
print(f"  R^T R = I?   {np.allclose(R12.T @ R12, np.eye(3))}")
print(f"  det  = {np.linalg.det(R12):.8f}")

# Inverse = transpose (free — no matrix inversion required)
R_inv = R1.T   # shape (3, 3)
print(f"\nInverse via transpose: R1 @ R1^T = I?  {np.allclose(R1 @ R_inv, np.eye(3))}")
print(f"np.linalg.inv matches R1^T?  {np.allclose(np.linalg.inv(R1), R_inv)}")

Closure — R1 @ R2 still in SO(3):
  R^T R = I?   True
  det  = 1.00000000

Inverse via transpose: R1 @ R1^T = I?  True
np.linalg.inv matches R1^T?  True

---

8.1.3 Numerical Drift and Re-Orthogonalisation

After many multiplications (e.g., integrating angular velocity in a simulation), floating-point errors accumulate and \(R\) drifts off SO(3): \(R^T R \neq I\) exactly.

The fix is to project back onto SO(3) via SVD:

\[R_\text{fixed} = UV^T \quad \text{where} \quad R_\text{drifted} = U\Sigma V^T\]

import numpy as np

rng = np.random.default_rng(0)

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

# Simulate drift: multiply 5000 small rotations
R = np.eye(3)   # shape (3, 3)
for _ in range(5000):
    angle = rng.uniform(-0.001, 0.001)
    R = R @ Rz(angle)

# Add a small artificial noise to exaggerate the effect
R += rng.standard_normal((3, 3)) * 1e-6

orth_err_before = np.linalg.norm(R.T @ R - np.eye(3))
det_before      = np.linalg.det(R)

# Project back onto SO(3) via SVD
U, _, Vt = np.linalg.svd(R)       # U: (3,3), Vt: (3,3)
R_fixed = U @ Vt                   # shape (3, 3)

# Ensure det = +1 (not -1 from a reflection)
if np.linalg.det(R_fixed) < 0:
    U[:, -1] *= -1
    R_fixed = U @ Vt

orth_err_after = np.linalg.norm(R_fixed.T @ R_fixed - np.eye(3))
det_after      = np.linalg.det(R_fixed)

print(f"Before re-orthogonalisation: ||R^T R - I|| = {orth_err_before:.2e},  det = {det_before:.8f}")
print(f"After  re-orthogonalisation: ||R^T R - I|| = {orth_err_after:.2e},  det = {det_after:.8f}")

Before re-orthogonalisation: ||R^T R - I|| = 5.46e-06,  det = 0.99999903
After  re-orthogonalisation: ||R^T R - I|| = 1.26e-15,  det = 1.00000000

---

8.1.4 What Rotation Matrices Do Geometrically

The three columns of \(R\) are the images of the three standard basis vectors: column \(j\) of \(R\) is where \(\mathbf{e}_j\) goes after the rotation. They form a new orthonormal frame — the body frame expressed in the world frame.

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D   # noqa: F401

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def Ry(b):
    c, s = np.cos(b), np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

R = Rz(np.pi/4) @ Ry(np.pi/6)   # shape (3, 3)

fig = plt.figure(figsize=(7, 6))
ax = fig.add_subplot(111, projection='3d')

origin = np.zeros(3)   # shape (3,)
colors = ['#4a90d9', '#2ecc71', 'tomato']
labels_world = ['$e_1$', '$e_2$', '$e_3$']
labels_body  = ["$Re_1$", "$Re_2$", "$Re_3$"]

for i in range(3):
    ei = np.eye(3)[i]   # shape (3,)
    Rei = R[:, i]        # shape (3,)
    ax.quiver(*origin, *ei,  color=colors[i], lw=1.2, alpha=0.4,
              arrow_length_ratio=0.2)
    ax.quiver(*origin, *Rei, color=colors[i], lw=2.5,
              arrow_length_ratio=0.2, label=f'{labels_world[i]} → {labels_body[i]}')
    ax.text(*(ei * 1.12),  labels_world[i], fontsize=9, color='#777777')
    ax.text(*(Rei * 1.12), labels_body[i],  fontsize=9, color=colors[i])

ax.set_xlim(-1, 1); ax.set_ylim(-1, 1); ax.set_zlim(-1, 1)
ax.set_xlabel('x'); ax.set_ylabel('y'); ax.set_zlabel('z')
ax.set_title('Columns of R = body frame axes in world frame', fontsize=10)
fig.tight_layout()
plt.show()

---

8.1.5 SO(3) in Robotics and Graphics

Every coordinate frame relationship in a robotic system is stored as an element of SO(3) (or SE(3) — Chapter 9). In ROS2, the geometry_msgs/Quaternion message encodes a unit quaternion, which is converted to \(R \in \text{SO}(3)\) internally. In game engines (Unity, Unreal), every Transform.rotation is a quaternion wrapping the same underlying SO(3) element.

The reason rotations are represented this way — rather than storing three Euler angles — is precisely the properties derived above: closed under composition, cheap inversion, stable numerics when re-orthogonalised via SVD.

8.2 Euler Angles and Gimbal Lock

Euler angles encode a rotation as three successive rotations about coordinate axes. The most common convention in robotics and aerospace is ZYX (yaw-pitch-roll):

\[R = R_z(\psi)\; R_y(\phi)\; R_x(\theta)\]

where \(\psi\) is yaw (heading), \(\phi\) is pitch (nose up/down), and \(\theta\) is roll (bank). The intuition: first turn the vehicle’s nose left or right, then tilt it up or down, then roll about the body’s long axis.

---

8.2.1 Why Euler Angles Are Tempting — and Dangerous

Appeal: three numbers, directly interpretable by a pilot or operator. Problem: the three-angle parameterisation has a singularity called gimbal lock, where two of the three rotation axes become aligned and the system loses a degree of freedom.

For ZYX convention this happens when pitch \(\phi = \pm 90°\): at that point yaw and roll both rotate about the same axis, making the combined rotation non-unique.

import numpy as np

def Rx(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])   # shape (3, 3)

def Ry(b):
    c, s = np.cos(b), np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])   # shape (3, 3)

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])   # shape (3, 3)

def R_zyx(psi, phi, theta):
    """ZYX Euler angles: yaw(psi), pitch(phi), roll(theta)."""
    return Rz(psi) @ Ry(phi) @ Rx(theta)   # shape (3, 3)

# Normal pose: yaw=30°, pitch=20°, roll=10°
R_normal = R_zyx(np.deg2rad(30), np.deg2rad(20), np.deg2rad(10))

# Gimbal-locked pose: pitch = 90°
# Different yaw/roll combos that look the same in this state
R_lock_a = R_zyx(np.deg2rad(45), np.deg2rad(90), np.deg2rad(0))
R_lock_b = R_zyx(np.deg2rad(0),  np.deg2rad(90), np.deg2rad(-45))

print("Normal pose R:")
print(R_normal.round(4))
print("\nGimbal-locked pose A (yaw=45°, pitch=90°, roll=0°):")
print(R_lock_a.round(4))
print("\nGimbal-locked pose B (yaw=0°, pitch=90°, roll=-45°):")
print(R_lock_b.round(4))
print("\nAre these two locked poses the same rotation?",
      np.allclose(R_lock_a, R_lock_b))

Normal pose R:
[[ 0.8138 -0.441   0.3785]
 [ 0.4698  0.8826  0.018 ]
 [-0.342   0.1632  0.9254]]

Gimbal-locked pose A (yaw=45°, pitch=90°, roll=0°):
[[ 0.     -0.7071  0.7071]
 [ 0.      0.7071  0.7071]
 [-1.      0.      0.    ]]

Gimbal-locked pose B (yaw=0°, pitch=90°, roll=-45°):
[[ 0.     -0.7071  0.7071]
 [ 0.      0.7071  0.7071]
 [-1.     -0.      0.    ]]

Are these two locked poses the same rotation? True

The two seemingly different angle triples produce the same rotation matrix — gimbal lock destroyed one degree of freedom.

---

8.2.2 Visualising Gimbal Lock

import numpy as np
import matplotlib.pyplot as plt

def Rx(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])

def Ry(b):
    c, s = np.cos(b), np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def R_zyx(psi, phi, theta):
    return Rz(psi) @ Ry(phi) @ Rx(theta)

# Sweep yaw from 0 to 360° at two pitch values: 20° (no lock) and 90° (locked)
psi_vals = np.linspace(0, 2*np.pi, 360)

# Track where [1, 0, 0] (nose direction) ends up as yaw sweeps
nose = np.array([1., 0., 0.])   # shape (3,)

nose_normal = np.array([(R_zyx(psi, np.deg2rad(20), 0.) @ nose)[:2] for psi in psi_vals])   # shape (360, 2)
nose_locked = np.array([(R_zyx(psi, np.deg2rad(90), 0.) @ nose)[:2] for psi in psi_vals])   # shape (360, 2)

fig, axes = plt.subplots(1, 2, figsize=(10, 4))

axes[0].plot(nose_normal[:, 0], nose_normal[:, 1], color='#4a90d9', lw=2)
axes[0].set_title('Yaw sweep, pitch = 20°\n(nose traces a full circle)', fontsize=10)
axes[0].set_aspect('equal'); axes[0].grid(True, alpha=0.2)
axes[0].axhline(0, color='#333333', lw=0.4, alpha=0.5)
axes[0].axvline(0, color='#333333', lw=0.4, alpha=0.5)
axes[0].set_xlabel('x-component'); axes[0].set_ylabel('y-component')

axes[1].plot(nose_locked[:, 0], nose_locked[:, 1], color='tomato', lw=2)
axes[1].set_title('Yaw sweep, pitch = 90°\n(nose is stuck — gimbal lock)', fontsize=10)
axes[1].set_aspect('equal'); axes[1].grid(True, alpha=0.2)
axes[1].axhline(0, color='#333333', lw=0.4, alpha=0.5)
axes[1].axvline(0, color='#333333', lw=0.4, alpha=0.5)
axes[1].set_xlabel('x-component'); axes[1].set_ylabel('y-component')
axes[1].set_xlim(-1.5, 1.5); axes[1].set_ylim(-1.5, 1.5)

fig.suptitle('Gimbal lock: at pitch = 90° the nose direction freezes', fontsize=11)
plt.tight_layout()
plt.show()

---

8.2.3 Extracting Euler Angles from a Rotation Matrix

Given \(R\), the ZYX Euler angles can be extracted analytically:

\[\phi = \arcsin(-r_{31}), \quad \theta = \text{atan2}(r_{32}, r_{33}), \quad \psi = \text{atan2}(r_{21}, r_{11})\]

These formulas break down (return NaN or a non-unique result) when \(r_{31} = \pm 1\), i.e., at gimbal lock.

import numpy as np

def Rx(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])

def Ry(b):
    c, s = np.cos(b), np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def R_zyx(psi, phi, theta):
    return Rz(psi) @ Ry(phi) @ Rx(theta)

def mat_to_euler_zyx(R):
    """Extract ZYX Euler angles from a rotation matrix.
    Returns (psi, phi, theta) = (yaw, pitch, roll) in radians."""
    phi   = np.arcsin(-R[2, 0])          # pitch
    if np.abs(np.cos(phi)) > 1e-6:       # not at gimbal lock
        theta = np.arctan2(R[2, 1], R[2, 2])   # roll
        psi   = np.arctan2(R[1, 0], R[0, 0])   # yaw
    else:                                # gimbal lock: one DoF lost
        theta = 0.
        psi   = np.arctan2(-R[0, 1], R[1, 1])
    return psi, phi, theta

# Round-trip test (not near gimbal lock)
yaw0, pitch0, roll0 = 0.6, 0.3, -0.2
R = R_zyx(yaw0, pitch0, roll0)   # shape (3, 3)
yaw1, pitch1, roll1 = mat_to_euler_zyx(R)
print("Original (yaw, pitch, roll):", np.array([yaw0, pitch0, roll0]).round(6))
print("Recovered:                   ", np.array([yaw1, pitch1, roll1]).round(6))
print(f"Max error: {max(abs(yaw0-yaw1), abs(pitch0-pitch1), abs(roll0-roll1)):.2e}")

Original (yaw, pitch, roll): [ 0.6  0.3 -0.2]
Recovered:                    [ 0.6  0.3 -0.2]
Max error: 0.00e+00

---

8.2.4 Other Euler Conventions

There are 12 valid Euler angle conventions (6 proper Euler sequences like ZYZ plus 6 Tait-Bryan sequences like ZYX). Common choices:

Convention	Field	Note
ZYX (yaw-pitch-roll)	Aerospace, robotics	Most common in ROS
ZXZ	Classical mechanics	Euler’s original
XYZ (roll-pitch-yaw)	Some robotics frameworks	Opposite order
ZYZ	Camera orientation	Useful for spherical motion

All conventions suffer from a gimbal-lock singularity at some configuration. This motivates the singularity-free representations in §8.3 (axis-angle) and §8.4 (quaternions).

8.3 Axis-Angle: Rodrigues’ Formula

Every rotation in 3D can be described as a rotation by some angle \(\theta\) about some unit axis \(\hat{\mathbf{k}}\). This axis-angle representation is both geometrically intuitive and singularity-free.

The rotation matrix corresponding to angle \(\theta\) about unit axis \(\hat{\mathbf{k}} = [k_x, k_y, k_z]^T\) is given by Rodrigues’ formula:

\[R = I\cos\theta + (1-\cos\theta)\hat{\mathbf{k}}\hat{\mathbf{k}}^T + \sin\theta\,[\hat{\mathbf{k}}]_\times\]

where \([\hat{\mathbf{k}}]_\times\) is the skew-symmetric cross-product matrix:

\[[\hat{\mathbf{k}}]_\times = \begin{bmatrix}0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0\end{bmatrix}\]

---

8.3.1 The Three Terms: A Geometric Decomposition

Rodrigues’ formula decomposes the rotated vector into three orthogonal parts:

1. \((\mathbf{v} \cdot \hat{\mathbf{k}})\hat{\mathbf{k}}\): the component along the axis (unchanged by rotation)
2. \(\cos\theta\,(\mathbf{v} - (\mathbf{v} \cdot \hat{\mathbf{k}})\hat{\mathbf{k}})\): the perpendicular component scaled down
3. \(\sin\theta\,(\hat{\mathbf{k}} \times \mathbf{v})\): the perpendicular component rotated 90°

import numpy as np

def skew(k):
    """Skew-symmetric matrix for cross product: k × v = skew(k) @ v."""
    kx, ky, kz = k
    return np.array([[ 0., -kz,  ky],
                     [ kz,  0., -kx],
                     [-ky,  kx,  0.]])   # shape (3, 3)

def rodrigues(k_hat, theta):
    """Rotation matrix via Rodrigues' formula.
    k_hat: unit axis (shape (3,)), theta: angle in radians."""
    K = skew(k_hat)        # shape (3, 3)
    return (np.eye(3) * np.cos(theta)
            + (1 - np.cos(theta)) * np.outer(k_hat, k_hat)   # shape (3, 3)
            + np.sin(theta) * K)                               # shape (3, 3)

# Example: 120° about the (1,1,1)/√3 axis (cycles xyz components)
k_hat = np.array([1., 1., 1.]) / np.sqrt(3.)   # shape (3,)
theta = 2 * np.pi / 3                           # 120°

R = rodrigues(k_hat, theta)   # shape (3, 3)
print("R (120° about [1,1,1]/√3):")
print(R.round(6))

# Verify: rotating e_x should give e_y, e_y → e_z, e_z → e_x
print("\nR @ e_x =", (R @ np.array([1.,0.,0.])).round(6), " (should be e_y)")
print("R @ e_y =", (R @ np.array([0.,1.,0.])).round(6), " (should be e_z)")
print("R @ e_z =", (R @ np.array([0.,0.,1.])).round(6), " (should be e_x)")

# Sanity checks
print(f"\nR^T R = I?   {np.allclose(R.T @ R, np.eye(3))}")
print(f"det(R) = {np.linalg.det(R):.8f}")

R (120° about [1,1,1]/√3):
[[0. 0. 1.]
 [1. 0. 0.]
 [0. 1. 0.]]

R @ e_x = [0. 1. 0.]  (should be e_y)
R @ e_y = [0. 0. 1.]  (should be e_z)
R @ e_z = [1. 0. 0.]  (should be e_x)

R^T R = I?   True
det(R) = 1.00000000

---

8.3.2 Recovering Axis and Angle from a Rotation Matrix

Given \(R \in \text{SO}(3)\), the axis-angle parameters are:

\[\theta = \arccos\!\left(\frac{\text{tr}(R) - 1}{2}\right)\]

\[\hat{\mathbf{k}} = \frac{1}{2\sin\theta}\begin{bmatrix}R_{32}-R_{23}\\R_{13}-R_{31}\\R_{21}-R_{12}\end{bmatrix}\]

This formula is valid for \(\theta \in (0°, 180°)\). At \(\theta = 0\) the axis is undefined (no rotation). At \(\theta = 180°\), \(\sin\theta = 0\) and the formula is numerically unstable — a special case is needed.

import numpy as np

def skew(k):
    kx, ky, kz = k
    return np.array([[ 0., -kz,  ky],
                     [ kz,  0., -kx],
                     [-ky,  kx,  0.]])

def rodrigues(k_hat, theta):
    K = skew(k_hat)
    return (np.eye(3) * np.cos(theta)
            + (1 - np.cos(theta)) * np.outer(k_hat, k_hat)
            + np.sin(theta) * K)

def mat_to_axis_angle(R):
    """Extract axis and angle from a rotation matrix R."""
    trace = np.clip((np.trace(R) - 1.) / 2., -1., 1.)   # clip for numerical safety
    theta = np.arccos(trace)
    if np.abs(np.sin(theta)) < 1e-8:   # near 0° or 180°
        if theta < 0.01:
            return np.array([0., 0., 1.]), 0.   # arbitrary axis
        else:
            # 180°: use symmetric part of R
            A = (R + R.T) / 2. - np.eye(3) * (np.cos(theta))  # proportional to k k^T
            k_hat = np.sqrt(np.maximum(np.diag(A) / (1. - np.cos(theta)), 0.))
            # Fix signs: k_hat[0] positive by convention
            if R[2, 1] - R[1, 2] < 0: k_hat[0] *= -1
            return k_hat / np.linalg.norm(k_hat), theta
    vee = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]])   # shape (3,)
    k_hat = vee / (2. * np.sin(theta))                                # shape (3,)
    return k_hat, theta

# Round-trip test
k_original = np.array([1., 2., -1.]) / np.linalg.norm([1., 2., -1.])   # shape (3,)
theta_original = np.deg2rad(55.)

R = rodrigues(k_original, theta_original)   # shape (3, 3)
k_rec, theta_rec = mat_to_axis_angle(R)

print("Original axis:  ", k_original.round(6))
print("Recovered axis: ", k_rec.round(6))
print(f"Original angle: {np.rad2deg(theta_original):.4f}°")
print(f"Recovered angle:{np.rad2deg(theta_rec):.4f}°")
print(f"Axis match: {np.allclose(np.abs(k_rec), np.abs(k_original))}")

Original axis:   [ 0.408248  0.816497 -0.408248]
Recovered axis:  [ 0.408248  0.816497 -0.408248]
Original angle: 55.0000°
Recovered angle:55.0000°
Axis match: True

---

8.3.3 Visualising a Rotation: Sweeping the Angle

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D   # noqa: F401

def skew(k):
    kx, ky, kz = k
    return np.array([[ 0., -kz,  ky],[kz, 0.,-kx],[-ky, kx, 0.]])

def rodrigues(k_hat, theta):
    K = skew(k_hat)
    return np.eye(3)*np.cos(theta) + (1-np.cos(theta))*np.outer(k_hat, k_hat) + np.sin(theta)*K

# Rotate the point (1, 0, 0) about axis z=(0,0,1) by angles 0..360°
k_hat = np.array([0., 0., 1.])   # shape (3,)
v0    = np.array([1., 0., 0.])   # shape (3,)
thetas = np.linspace(0, 2*np.pi, 200)

trajectory = np.array([rodrigues(k_hat, t) @ v0 for t in thetas])   # shape (200, 3)

# Also rotate a tilted vector about a tilted axis
k2  = np.array([1., 1., 1.]) / np.sqrt(3.)   # shape (3,)
v2  = np.array([0., 1., 0.])                  # shape (3,)
traj2 = np.array([rodrigues(k2, t) @ v2 for t in thetas])           # shape (200, 3)

fig = plt.figure(figsize=(8, 5))
ax  = fig.add_subplot(111, projection='3d')

ax.plot(trajectory[:,0], trajectory[:,1], trajectory[:,2],
        color='#4a90d9', lw=2, label='Axis = z, initial = e_x')
ax.plot(traj2[:,0], traj2[:,1], traj2[:,2],
        color='tomato', lw=2, label='Axis = (1,1,1)/√3, initial = e_y')

# Draw axes
for col, lbl in zip(['#4a90d9', 'tomato'], [k_hat, k2]):
    ax.quiver(0,0,0, *lbl, color=col, lw=1.5, alpha=0.5, arrow_length_ratio=0.2)

ax.set_xlabel('x'); ax.set_ylabel('y'); ax.set_zlabel('z')
ax.set_title('Rodrigues rotation: a vector sweeps a circle on the unit sphere', fontsize=10)
ax.legend(fontsize=8, loc='upper left')
fig.tight_layout()
plt.show()

---

8.3.4 The Compact Form: Rotation Vector

The rotation vector \(\mathbf{r} = \theta\hat{\mathbf{k}}\) encodes both axis and angle in a single 3-vector. Its magnitude is the rotation angle; its direction is the rotation axis.

• \(\|\mathbf{r}\| = 0\): identity (no rotation)
• \(\mathbf{r} = [0, 0, \pi/4]^T\): rotate 45° about \(z\)

This compact form is the basis of the Lie algebra \(\mathfrak{so}(3)\) (Chapter 24) and is what ROS2 stores as angular_velocity messages.

import numpy as np

def skew(k):
    kx, ky, kz = k
    return np.array([[ 0., -kz,  ky],[kz, 0.,-kx],[-ky, kx, 0.]])

def rodrigues_vec(r):
    """Rotation matrix from rotation vector r = theta * k_hat."""
    theta = np.linalg.norm(r)   # scalar
    if theta < 1e-10:
        return np.eye(3)
    k_hat = r / theta            # shape (3,)
    K = skew(k_hat)              # shape (3, 3)
    return np.eye(3)*np.cos(theta) + (1-np.cos(theta))*np.outer(k_hat,k_hat) + np.sin(theta)*K

# Encode "rotate 45° about z" as a rotation vector
r = np.array([0., 0., np.pi/4])   # shape (3,)
R = rodrigues_vec(r)               # shape (3, 3)
print("r =", r.round(4), "  ||r|| =", np.linalg.norm(r).round(4), "rad =", np.rad2deg(np.linalg.norm(r)).round(2), "°")
print("R ="); print(R.round(4))

r = [0.     0.     0.7854]   ||r|| = 0.7854 rad = 45.0 °
R =
[[ 0.7071 -0.7071  0.    ]
 [ 0.7071  0.7071  0.    ]
 [ 0.      0.      1.    ]]

8.4 Quaternions: Algebra and Rotation

A quaternion is a hypercomplex number with one real part and three imaginary parts:

\[q = w + xi + yj + zk \qquad w, x, y, z \in \mathbb{R}\]

The imaginary units satisfy Hamilton’s rules:

\[i^2 = j^2 = k^2 = ijk = -1\] \[ij = k, \quad jk = i, \quad ki = j, \quad ji = -k, \quad kj = -i, \quad ik = -j\]

A unit quaternion (\(\|q\| = 1\)) encodes a 3D rotation and avoids all the problems of Euler angles: no singularities, smooth interpolation, and efficient composition.

---

8.4.1 Quaternion as Axis-Angle

A rotation by angle \(\theta\) about unit axis \(\hat{\mathbf{k}}\) is encoded as:

\[q = \cos\!\frac{\theta}{2} + \sin\!\frac{\theta}{2}\,(k_x i + k_y j + k_z k)\]

In vector notation: \(q = [w, \mathbf{v}]\) where \(w = \cos(\theta/2)\) and \(\mathbf{v} = \sin(\theta/2)\hat{\mathbf{k}}\).

import numpy as np

def axis_angle_to_quat(k_hat, theta):
    """Unit quaternion from axis-angle.  Returns [w, x, y, z] shape (4,)."""
    w = np.cos(theta / 2.)
    v = np.sin(theta / 2.) * k_hat   # shape (3,)
    return np.array([w, v[0], v[1], v[2]])   # shape (4,)

# 90° about z-axis
k = np.array([0., 0., 1.])   # shape (3,)
q = axis_angle_to_quat(k, np.pi / 2)
print(f"q (90° about z) = {q.round(6)}")
print(f"||q|| = {np.linalg.norm(q):.8f}  (should be 1)")

# Identity rotation: theta = 0
q_id = axis_angle_to_quat(k, 0.)
print(f"\nIdentity quaternion = {q_id}  (w=1, xyz=0)")

q (90° about z) = [0.707107 0.       0.       0.707107]
||q|| = 1.00000000  (should be 1)

Identity quaternion = [1. 0. 0. 0.]  (w=1, xyz=0)

---

8.4.2 The Hamilton Product

Composing two rotations corresponds to the Hamilton product of their quaternions. For \(p = [p_w, \mathbf{p}_v]\) and \(q = [q_w, \mathbf{q}_v]\):

\[p \otimes q = \begin{bmatrix}p_w q_w - \mathbf{p}_v \cdot \mathbf{q}_v \\ p_w \mathbf{q}_v + q_w \mathbf{p}_v + \mathbf{p}_v \times \mathbf{q}_v\end{bmatrix}\]

import numpy as np

def quat_mul(p, q):
    """Hamilton product of two quaternions [w,x,y,z].  Returns shape (4,)."""
    pw, px, py, pz = p
    qw, qx, qy, qz = q
    return np.array([
        pw*qw - px*qx - py*qy - pz*qz,
        pw*qx + px*qw + py*qz - pz*qy,
        pw*qy - px*qz + py*qw + pz*qx,
        pw*qz + px*qy - py*qx + pz*qw
    ])   # shape (4,)

def axis_angle_to_quat(k_hat, theta):
    w = np.cos(theta / 2.)
    v = np.sin(theta / 2.) * k_hat
    return np.array([w, v[0], v[1], v[2]])

# Compose: 90° about x, then 90° about z
q1 = axis_angle_to_quat(np.array([1.,0.,0.]), np.pi/2)   # shape (4,)
q2 = axis_angle_to_quat(np.array([0.,0.,1.]), np.pi/2)   # shape (4,)

q_composed = quat_mul(q2, q1)   # shape (4,) — q2 applied after q1 (right-to-left)
print("q1 (90° about x):", q1.round(6))
print("q2 (90° about z):", q2.round(6))
print("q2 ⊗ q1 (composed):", q_composed.round(6))
print(f"||q_composed|| = {np.linalg.norm(q_composed):.8f}")

# Non-commutativity: q1 ⊗ q2 ≠ q2 ⊗ q1
q_rev = quat_mul(q1, q2)   # shape (4,)
print(f"\nq1 ⊗ q2 = {q_rev.round(6)}")
print(f"q2 ⊗ q1 = {q_composed.round(6)}")
print(f"Equal? {np.allclose(q_composed, q_rev)}")

q1 (90° about x): [0.707107 0.707107 0.       0.      ]
q2 (90° about z): [0.707107 0.       0.       0.707107]
q2 ⊗ q1 (composed): [0.5 0.5 0.5 0.5]
||q_composed|| = 1.00000000

q1 ⊗ q2 = [ 0.5  0.5 -0.5  0.5]
q2 ⊗ q1 = [0.5 0.5 0.5 0.5]
Equal? False

---

8.4.3 Rotating a Vector: \(q\mathbf{v}q^{-1}\)

To rotate vector \(\mathbf{v}\) by unit quaternion \(q\):

1. Embed: \(\tilde{v} = [0, \mathbf{v}]\) (pure quaternion)
2. Conjugate: \(\mathbf{v}' = q \otimes \tilde{v} \otimes q^*\) where \(q^* = [w, -\mathbf{v}]\)

For a unit quaternion, \(q^{-1} = q^*\).

import numpy as np

def quat_mul(p, q):
    pw, px, py, pz = p
    qw, qx, qy, qz = q
    return np.array([
        pw*qw - px*qx - py*qy - pz*qz,
        pw*qx + px*qw + py*qz - pz*qy,
        pw*qy - px*qz + py*qw + pz*qx,
        pw*qz + px*qy - py*qx + pz*qw
    ])

def quat_conj(q):
    """Conjugate q* = [w, -x, -y, -z]."""
    return np.array([q[0], -q[1], -q[2], -q[3]])   # shape (4,)

def quat_rotate(q, v):
    """Rotate 3D vector v by unit quaternion q.  Returns shape (3,)."""
    v_quat = np.array([0., v[0], v[1], v[2]])   # shape (4,)
    v_rot = quat_mul(quat_mul(q, v_quat), quat_conj(q))   # shape (4,)
    return v_rot[1:]   # shape (3,) — drop the scalar part

def axis_angle_to_quat(k_hat, theta):
    w = np.cos(theta / 2.)
    v = np.sin(theta / 2.) * k_hat
    return np.array([w, v[0], v[1], v[2]])

# Verify against Rodrigues' formula
def rodrigues(k_hat, theta):
    from numpy import cos, sin, eye, outer
    K = np.array([[0,-k_hat[2],k_hat[1]],[k_hat[2],0,-k_hat[0]],[-k_hat[1],k_hat[0],0]])
    return eye(3)*cos(theta) + (1-cos(theta))*outer(k_hat,k_hat) + sin(theta)*K

k_hat = np.array([1., 1., 0.]) / np.sqrt(2.)   # shape (3,)
theta = np.deg2rad(70.)
v     = np.array([1., 2., 3.])                  # shape (3,)

q = axis_angle_to_quat(k_hat, theta)   # shape (4,)
v_quat = quat_rotate(q, v)             # shape (3,)
v_rod  = rodrigues(k_hat, theta) @ v   # shape (3,)

print("Rotation via quaternion:       ", v_quat.round(6))
print("Rotation via Rodrigues matrix: ", v_rod.round(6))
print(f"Match: {np.allclose(v_quat, v_rod)}")

Rotation via quaternion:        [ 3.322379 -0.322379  1.690523]
Rotation via Rodrigues matrix:  [ 3.322379 -0.322379  1.690523]
Match: True

---

8.4.4 Quaternion to Rotation Matrix

Converting a unit quaternion \(q = [w, x, y, z]\) to a \(3 \times 3\) 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}\]

import numpy as np

def quat_to_mat(q):
    """Convert unit quaternion [w,x,y,z] to 3x3 rotation matrix."""
    w, x, y, z = q / np.linalg.norm(q)   # normalise for safety
    return np.array([
        [1-2*(y*y+z*z),   2*(x*y-w*z),   2*(x*z+w*y)],
        [  2*(x*y+w*z), 1-2*(x*x+z*z),   2*(y*z-w*x)],
        [  2*(x*z-w*y),   2*(y*z+w*x), 1-2*(x*x+y*y)]
    ])   # shape (3, 3)

def axis_angle_to_quat(k_hat, theta):
    w = np.cos(theta / 2.)
    v = np.sin(theta / 2.) * k_hat
    return np.array([w, v[0], v[1], v[2]])

k_hat = np.array([0., 1., 0.])   # shape (3,)
theta = np.deg2rad(45.)

q = axis_angle_to_quat(k_hat, theta)   # shape (4,)
R = quat_to_mat(q)                      # shape (3, 3)
print("R from quaternion:")
print(R.round(6))
print(f"\nR^T R = I?  {np.allclose(R.T @ R, np.eye(3))}")
print(f"det(R) = {np.linalg.det(R):.8f}")

R from quaternion:
[[ 0.707107  0.        0.707107]
 [ 0.        1.        0.      ]
 [-0.707107  0.        0.707107]]

R^T R = I?  True
det(R) = 1.00000000

---

8.4.5 The Double Cover of SO(3)

A critical subtlety: \(q\) and \(-q\) represent the same rotation.

\[R(q) = R(-q)\]

This is because flipping all four components of a unit quaternion negates the pure-quaternion part in \(q\tilde{v}q^*\) twice, leaving the result unchanged. The sphere \(S^3\) of unit quaternions is a double cover of SO(3): every rotation corresponds to exactly two antipodal quaternions.

This matters for interpolation (§8.5): to take the short path on \(S^3\), always check that \(p \cdot q > 0\) and negate \(q\) if not.

import numpy as np

def quat_to_mat(q):
    w, x, y, z = q / np.linalg.norm(q)
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y-w*z), 2*(x*z+w*y)],
        [2*(x*y+w*z), 1-2*(x*x+z*z), 2*(y*z-w*x)],
        [2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x*x+y*y)]
    ])

q = np.array([0.707, 0.0, 0.707, 0.0])   # shape (4,) — some unit quaternion
q_neg = -q                                 # shape (4,) — antipodal

R1 = quat_to_mat(q)       # shape (3, 3)
R2 = quat_to_mat(q_neg)   # shape (3, 3)
print("q and -q produce the same rotation matrix:", np.allclose(R1, R2))
print(f"\nq     = {q}")
print(f"-q    = {q_neg}")
print(f"R(q)  =\n{R1.round(6)}")
print(f"R(-q) =\n{R2.round(6)}")

q and -q produce the same rotation matrix: True

q     = [0.707 0.    0.707 0.   ]
-q    = [-0.707 -0.    -0.707 -0.   ]
R(q)  =
[[-0.  0.  1.]
 [ 0.  1.  0.]
 [-1.  0. -0.]]
R(-q) =
[[-0.  0.  1.]
 [ 0.  1.  0.]
 [-1.  0. -0.]]

8.5 SLERP: Interpolating Rotations

Interpolating between two rotations is not as simple as linearly interpolating between two matrices or two Euler-angle triples. The correct tool is Spherical Linear Interpolation (SLERP) — a geodesic walk along the great circle of the unit quaternion sphere \(S^3\).

---

8.5.1 Why Linear Matrix Interpolation Fails

A matrix \((1-t)R_0 + tR_1\) does not stay in SO(3) for \(t \in (0,1)\): the interpolated matrix is not orthogonal and can even have determinant \(< 1\).

import numpy as np

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])   # shape (3, 3)

R0 = Rz(0.)           # identity
R1 = Rz(np.pi / 2.)   # 90° rotation

ts = np.linspace(0, 1, 9)
print(f"{'t':>5}  {'det(lerp)':>12}  {'||R^T R - I||':>16}")
for t in ts:
    R_lerp = (1 - t) * R0 + t * R1   # shape (3, 3) — linear interpolation
    det_val = np.linalg.det(R_lerp)
    orth_err = np.linalg.norm(R_lerp.T @ R_lerp - np.eye(3))
    print(f"{t:5.2f}  {det_val:12.6f}  {orth_err:16.6f}")

    t     det(lerp)     ||R^T R - I||
 0.00      1.000000          0.000000
 0.12      0.781250          0.309359
 0.25      0.625000          0.530330
 0.38      0.531250          0.662913
 0.50      0.500000          0.707107
 0.62      0.531250          0.662913
 0.75      0.625000          0.530330
 0.88      0.781250          0.309359
 1.00      1.000000          0.000000

The determinant drops well below 1 — the interpolated matrices are not rotations.

---

8.5.2 SLERP: Geodesic on the Quaternion Sphere

For unit quaternions \(q_0\) and \(q_1\), SLERP at parameter \(t \in [0,1]\) is:

\[\text{slerp}(q_0, q_1, t) = q_0 \left(q_0^{-1} q_1\right)^t = \frac{\sin((1-t)\Omega)}{\sin\Omega} q_0 + \frac{\sin(t\Omega)}{\sin\Omega} q_1\]

where \(\Omega = \arccos(q_0 \cdot q_1)\) is the half-angle between the quaternions on \(S^3\). When \(\Omega \approx 0\) (nearly identical rotations), fall back to linear interpolation (LERP) to avoid division by zero.

import numpy as np

def slerp(q0, q1, t):
    """Spherical linear interpolation between unit quaternions q0 and q1."""
    q0 = q0 / np.linalg.norm(q0)   # shape (4,)
    q1 = q1 / np.linalg.norm(q1)   # shape (4,)

    dot = np.clip(np.dot(q0, q1), -1., 1.)   # scalar

    # Take the short path: if dot < 0, negate q1
    if dot < 0.:
        q1  = -q1
        dot = -dot

    Omega = np.arccos(dot)   # scalar

    if Omega < 1e-8:         # nearly identical — use LERP
        q = (1. - t) * q0 + t * q1
        return q / np.linalg.norm(q)

    return (np.sin((1. - t) * Omega) / np.sin(Omega)) * q0 \
         + (np.sin(t * Omega)         / np.sin(Omega)) * q1   # shape (4,)

def axis_angle_to_quat(k_hat, theta):
    w = np.cos(theta / 2.)
    v = np.sin(theta / 2.) * k_hat
    return np.array([w, v[0], v[1], v[2]])   # shape (4,)

# Interpolate between 0° and 90° about z
q0 = axis_angle_to_quat(np.array([0.,0.,1.]), 0.)
q1 = axis_angle_to_quat(np.array([0.,0.,1.]), np.pi/2)

ts = np.linspace(0., 1., 6)
print(f"{'t':>5}  {'q (slerp)':>40}  {'||q||':>8}")
for t in ts:
    q = slerp(q0, q1, t)   # shape (4,)
    print(f"{t:5.2f}  {str(q.round(4)):>40}  {np.linalg.norm(q):.6f}")

    t                                 q (slerp)     ||q||
 0.00                             [1. 0. 0. 0.]  1.000000
 0.20             [0.9877 0.     0.     0.1564]  1.000000
 0.40             [0.9511 0.     0.     0.309 ]  1.000000
 0.60                 [0.891 0.    0.    0.454]  1.000000
 0.80             [0.809  0.     0.     0.5878]  1.000000
 1.00             [0.7071 0.     0.     0.7071]  1.000000

SLERP keeps \(\|q\| = 1\) at every step: the interpolated quaternions are always valid unit quaternions, hence always valid rotations.

---

8.5.3 Euler-Angle Interpolation: The Artefact

Linear interpolation of Euler angles near a singularity produces a jarring “jump” or unexpected trajectory. Here we compare SLERP to naive Euler lerp:

import numpy as np
import matplotlib.pyplot as plt

def Rx(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])

def Ry(b):
    c, s = np.cos(b), np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

def Rz(g):
    c, s = np.cos(g), np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def R_zyx(psi, phi, theta):
    return Rz(psi) @ Ry(phi) @ Rx(theta)

def axis_angle_to_quat(k_hat, theta):
    w = np.cos(theta / 2.)
    v = np.sin(theta / 2.) * k_hat
    return np.array([w, v[0], v[1], v[2]])

def quat_mul(p, q):
    pw, px, py, pz = p; qw, qx, qy, qz = q
    return np.array([pw*qw-px*qx-py*qy-pz*qz, pw*qx+px*qw+py*qz-pz*qy,
                     pw*qy-px*qz+py*qw+pz*qx, pw*qz+px*qy-py*qx+pz*qw])

def quat_conj(q):
    return np.array([q[0], -q[1], -q[2], -q[3]])

def quat_rotate(q, v):
    vq = np.array([0., *v])
    return quat_mul(quat_mul(q, vq), quat_conj(q))[1:]

def slerp(q0, q1, t):
    q0 = q0 / np.linalg.norm(q0); q1 = q1 / np.linalg.norm(q1)
    dot = np.clip(np.dot(q0, q1), -1., 1.)
    if dot < 0.: q1 = -q1; dot = -dot
    Om = np.arccos(dot)
    if Om < 1e-8:
        q = (1-t)*q0 + t*q1; return q/np.linalg.norm(q)
    return np.sin((1-t)*Om)/np.sin(Om)*q0 + np.sin(t*Om)/np.sin(Om)*q1

def mat_to_quat(R):
    """Shepperd method: rotation matrix to unit quaternion."""
    trace = np.trace(R)   # scalar
    if trace > 0:
        s = 0.5 / np.sqrt(trace + 1.)
        return np.array([0.25/s, (R[2,1]-R[1,2])*s, (R[0,2]-R[2,0])*s, (R[1,0]-R[0,1])*s])
    elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
        s = 2.*np.sqrt(1.+R[0,0]-R[1,1]-R[2,2])
        return np.array([(R[2,1]-R[1,2])/s, 0.25*s, (R[0,1]+R[1,0])/s, (R[0,2]+R[2,0])/s])
    elif R[1,1] > R[2,2]:
        s = 2.*np.sqrt(1.+R[1,1]-R[0,0]-R[2,2])
        return np.array([(R[0,2]-R[2,0])/s, (R[0,1]+R[1,0])/s, 0.25*s, (R[1,2]+R[2,1])/s])
    else:
        s = 2.*np.sqrt(1.+R[2,2]-R[0,0]-R[1,1])
        return np.array([(R[1,0]-R[0,1])/s, (R[0,2]+R[2,0])/s, (R[1,2]+R[2,1])/s, 0.25*s])

# Pose A: yaw=0°, pitch=80°, roll=0°
# Pose B: yaw=90°, pitch=80°, roll=0°
# Near gimbal lock (pitch close to 90°) — Euler lerp will wobble
e_A = np.array([0.,      np.deg2rad(80.), 0.])   # shape (3,)
e_B = np.array([np.pi/2, np.deg2rad(80.), 0.])   # shape (3,)

R_A = R_zyx(*e_A); R_B = R_zyx(*e_B)   # shape (3, 3)
q_A = mat_to_quat(R_A); q_B = mat_to_quat(R_B)   # shape (4,)

v_probe = np.array([1., 0., 0.])   # shape (3,)
ts = np.linspace(0., 1., 60)

# Euler LERP: interpolate angle triples, rebuild matrix
traj_euler = np.array([
    R_zyx(*(t * e_B + (1-t) * e_A)) @ v_probe for t in ts])   # shape (60, 3)

# SLERP
traj_slerp = np.array([
    quat_rotate(slerp(q_A, q_B, t), v_probe) for t in ts])   # shape (60, 3)

fig, axes = plt.subplots(1, 2, figsize=(11, 4))
for ax, traj, title, col in [
    (axes[0], traj_euler, 'Euler LERP\n(wobble near gimbal lock)', 'tomato'),
    (axes[1], traj_slerp, 'SLERP\n(smooth geodesic)', '#4a90d9')]:
    ax.plot(traj[:, 0], traj[:, 1], color=col, lw=2.5)
    ax.scatter([traj[0,0], traj[-1,0]], [traj[0,1], traj[-1,1]],
               color='black', s=60, zorder=5)
    ax.set_title(title, fontsize=10)
    ax.set_xlabel('x-component of probe vector')
    ax.set_ylabel('y-component of probe vector')
    ax.grid(True, alpha=0.2)
    ax.axhline(0, color='#333333', lw=0.4, alpha=0.5)
    ax.axvline(0, color='#333333', lw=0.4, alpha=0.5)
    ax.set_aspect('equal')

fig.suptitle('Euler LERP wobbles; SLERP traces a constant-speed geodesic', fontsize=11)
plt.tight_layout()
plt.show()

---

8.5.4 Constant Angular Velocity

SLERP has a key property: the angular velocity between consecutive steps is constant. The rotation sweeps at a uniform rate, unlike Euler interpolation which accelerates and decelerates unpredictably near singularities.

This constant-speed property makes SLERP the standard for: - Animation blending (game engines, motion capture) - Robot trajectory planning (smooth joint interpolation) - Sensor fusion (averaging multiple orientation estimates)

import numpy as np

def axis_angle_to_quat(k_hat, theta):
    w = np.cos(theta / 2.); v = np.sin(theta / 2.) * k_hat
    return np.array([w, v[0], v[1], v[2]])

def slerp(q0, q1, t):
    q0 = q0/np.linalg.norm(q0); q1 = q1/np.linalg.norm(q1)
    dot = np.clip(np.dot(q0, q1), -1., 1.)
    if dot < 0.: q1 = -q1; dot = -dot
    Om = np.arccos(dot)
    if Om < 1e-8:
        q = (1-t)*q0+t*q1; return q/np.linalg.norm(q)
    return np.sin((1-t)*Om)/np.sin(Om)*q0 + np.sin(t*Om)/np.sin(Om)*q1

q0 = axis_angle_to_quat(np.array([0.,0.,1.]), 0.)
q1 = axis_angle_to_quat(np.array([0.,0.,1.]), np.deg2rad(120.))

ts = np.linspace(0., 1., 11)
qs = np.array([slerp(q0, q1, t) for t in ts])   # shape (11, 4)

# Angular speed = angle between consecutive quaternions
angles = []
for i in range(len(qs)-1):
    dot = np.clip(np.dot(qs[i], qs[i+1]), -1., 1.)
    angle = 2. * np.arccos(dot)   # full rotation angle in radians
    angles.append(np.rad2deg(angle))

print("Angular step between consecutive SLERP frames (degrees):")
print(np.array(angles).round(4))
print(f"\nAll equal? {np.allclose(angles, angles[0])}")

Angular step between consecutive SLERP frames (degrees):
[12. 12. 12. 12. 12. 12. 12. 12. 12. 12.]

All equal? True

8.6 Conversion Table: All Four Representations

There are four mainstream representations for 3D rotations, each with different strengths. This section gives closed-form conversion formulas for all 12 pairwise paths, implemented as self-contained Python functions with numerical edge-case handling.

Representation	Parameters	DoF encoded	Singularity	Best for
Rotation matrix \(R\)	9 numbers	3	None (but drift)	Composing transforms, vector rotation
Euler angles	3 numbers (ZYX)	3	Gimbal lock at \(\phi=\pm90°\)	Human-readable output, flight displays
Axis-angle \((\hat{k}, \theta)\)	4 numbers	3	Near \(\theta=0,\pi\)	Differentiation, Lie algebra
Unit quaternion \(q\)	4 numbers	3	None (double cover)	Interpolation, real-time systems

---

8.6.1 Matrix ↔︎ Quaternion

import numpy as np

def mat_to_quat(R):
    """Shepperd's method: 3x3 rotation matrix -> unit quaternion [w,x,y,z].
    Numerically stable across all rotations."""
    trace = np.trace(R)   # scalar
    if trace > 0.:
        s = 0.5 / np.sqrt(trace + 1.)
        return np.array([(0.25 / s),
                         (R[2,1] - R[1,2]) * s,
                         (R[0,2] - R[2,0]) * s,
                         (R[1,0] - R[0,1]) * s])   # shape (4,)
    elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
        s = 2. * np.sqrt(1. + R[0,0] - R[1,1] - R[2,2])
        return np.array([(R[2,1] - R[1,2]) / s, 0.25*s,
                         (R[0,1] + R[1,0]) / s, (R[0,2] + R[2,0]) / s])
    elif R[1,1] > R[2,2]:
        s = 2. * np.sqrt(1. + R[1,1] - R[0,0] - R[2,2])
        return np.array([(R[0,2] - R[2,0]) / s, (R[0,1] + R[1,0]) / s,
                         0.25*s, (R[1,2] + R[2,1]) / s])
    else:
        s = 2. * np.sqrt(1. + R[2,2] - R[0,0] - R[1,1])
        return np.array([(R[1,0] - R[0,1]) / s, (R[0,2] + R[2,0]) / s,
                         (R[1,2] + R[2,1]) / s, 0.25*s])

def quat_to_mat(q):
    """Unit quaternion [w,x,y,z] -> 3x3 rotation matrix."""
    w, x, y, z = q / np.linalg.norm(q)   # normalise for safety
    return np.array([
        [1-2*(y*y+z*z),   2*(x*y-w*z),   2*(x*z+w*y)],
        [  2*(x*y+w*z), 1-2*(x*x+z*z),   2*(y*z-w*x)],
        [  2*(x*z-w*y),   2*(y*z+w*x), 1-2*(x*x+y*y)]
    ])   # shape (3, 3)

def Rz(g):
    c,s=np.cos(g),np.sin(g)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def Ry(b):
    c,s=np.cos(b),np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

R = Rz(0.5) @ Ry(0.3)   # shape (3, 3)
q = mat_to_quat(R)       # shape (4,)
R2 = quat_to_mat(q)      # shape (3, 3)

print("R -> q -> R round-trip:")
print(f"q = {q.round(6)}")
print(f"||q|| = {np.linalg.norm(q):.8f}")
print(f"R == quat_to_mat(q)?  {np.allclose(R, R2)}")

R -> q -> R round-trip:
q = [ 0.958033 -0.036972  0.144792  0.244626]
||q|| = 1.00000000
R == quat_to_mat(q)?  True

---

8.6.2 Matrix ↔︎ Axis-Angle

import numpy as np

def mat_to_axis_angle(R):
    """3x3 rotation matrix -> (unit axis shape(3,), angle scalar)."""
    trace = np.clip((np.trace(R) - 1.) / 2., -1., 1.)
    theta = np.arccos(trace)   # scalar
    if np.abs(np.sin(theta)) < 1e-7:
        if theta < 0.01:
            return np.array([0., 0., 1.]), 0.
        # 180° case
        diag = np.diag(R) + 1.            # shape (3,)
        k_hat = np.sqrt(np.maximum(diag / 2., 0.))
        if R[2,1] - R[1,2] < 0: k_hat[0] *= -1
        if R[0,2] - R[2,0] < 0: k_hat[1] *= -1
        return k_hat / np.linalg.norm(k_hat), theta
    vee = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]])   # shape (3,)
    k_hat = vee / (2. * np.sin(theta))                                # shape (3,)
    return k_hat, theta

def axis_angle_to_mat(k_hat, theta):
    """(unit axis, angle) -> 3x3 rotation matrix via Rodrigues."""
    K = np.array([[0,-k_hat[2],k_hat[1]],[k_hat[2],0,-k_hat[0]],[-k_hat[1],k_hat[0],0]])
    return np.eye(3)*np.cos(theta) + (1-np.cos(theta))*np.outer(k_hat,k_hat) + np.sin(theta)*K   # shape (3,3)

def Ry(b):
    c,s=np.cos(b),np.sin(b)
    return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

R = Ry(np.deg2rad(75.))   # shape (3, 3)
k, theta = mat_to_axis_angle(R)
R2 = axis_angle_to_mat(k, theta)

print(f"Extracted axis:  {k.round(6)}")
print(f"Extracted angle: {np.rad2deg(theta):.4f}°")
print(f"Round-trip R == R2?  {np.allclose(R, R2)}")

Extracted axis:  [0. 1. 0.]
Extracted angle: 75.0000°
Round-trip R == R2?  True

---

8.6.3 Matrix ↔︎ Euler Angles (ZYX)

import numpy as np

def mat_to_euler_zyx(R):
    """3x3 rotation matrix -> (yaw, pitch, roll) ZYX Euler angles (radians)."""
    phi = np.arcsin(np.clip(-R[2, 0], -1., 1.))   # pitch
    if np.abs(np.cos(phi)) > 1e-6:
        theta = np.arctan2(R[2, 1], R[2, 2])      # roll
        psi   = np.arctan2(R[1, 0], R[0, 0])      # yaw
    else:
        theta = 0.
        psi   = np.arctan2(-R[0, 1], R[1, 1])
    return psi, phi, theta

def euler_zyx_to_mat(psi, phi, theta):
    """ZYX Euler (yaw, pitch, roll) -> 3x3 rotation matrix."""
    def Rx(a):
        c,s=np.cos(a),np.sin(a)
        return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
    def Ry(b):
        c,s=np.cos(b),np.sin(b)
        return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
    def Rz(g):
        c,s=np.cos(g),np.sin(g)
        return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])
    return Rz(psi) @ Ry(phi) @ Rx(theta)   # shape (3, 3)

# Round-trip (not near gimbal lock)
psi0, phi0, theta0 = 0.8, 0.2, -0.5
R = euler_zyx_to_mat(psi0, phi0, theta0)    # shape (3, 3)
psi1, phi1, theta1 = mat_to_euler_zyx(R)
print(f"Original: yaw={np.rad2deg(psi0):.2f}°, pitch={np.rad2deg(phi0):.2f}°, roll={np.rad2deg(theta0):.2f}°")
print(f"Recovered:yaw={np.rad2deg(psi1):.2f}°, pitch={np.rad2deg(phi1):.2f}°, roll={np.rad2deg(theta1):.2f}°")

Original: yaw=45.84°, pitch=11.46°, roll=-28.65°
Recovered:yaw=45.84°, pitch=11.46°, roll=-28.65°

---

8.6.4 Quaternion ↔︎ Axis-Angle

import numpy as np

def quat_to_axis_angle(q):
    """Unit quaternion [w,x,y,z] -> (unit axis shape(3,), angle scalar)."""
    q = q / np.linalg.norm(q)
    w = np.clip(q[0], -1., 1.)
    theta = 2. * np.arccos(w)    # scalar
    s     = np.sin(theta / 2.)   # scalar
    if np.abs(s) < 1e-8:
        return np.array([0., 0., 1.]), 0.
    return q[1:] / s, theta      # shape (3,), scalar

def axis_angle_to_quat(k_hat, theta):
    """(unit axis, angle) -> unit quaternion [w,x,y,z]."""
    w = np.cos(theta / 2.)
    v = np.sin(theta / 2.) * k_hat   # shape (3,)
    return np.array([w, v[0], v[1], v[2]])   # shape (4,)

k_original = np.array([1., -1., 2.]) / np.linalg.norm([1.,-1.,2.])   # shape (3,)
theta_original = np.deg2rad(110.)

q = axis_angle_to_quat(k_original, theta_original)   # shape (4,)
k_rec, theta_rec = quat_to_axis_angle(q)

print(f"Axis original:  {k_original.round(6)}")
print(f"Axis recovered: {k_rec.round(6)}")
print(f"Angle original:  {np.rad2deg(theta_original):.4f}°")
print(f"Angle recovered: {np.rad2deg(theta_rec):.4f}°")

Axis original:  [ 0.408248 -0.408248  0.816497]
Axis recovered: [ 0.408248 -0.408248  0.816497]
Angle original:  110.0000°
Angle recovered: 110.0000°

---

8.6.5 Quaternion ↔︎ Euler Angles (ZYX)

import numpy as np

def quat_to_euler_zyx(q):
    """Unit quaternion [w,x,y,z] -> (yaw, pitch, roll) ZYX Euler angles (radians)."""
    w, x, y, z = q / np.linalg.norm(q)
    sinp = 2. * (w * y - z * x)
    phi  = np.arcsin(np.clip(sinp, -1., 1.))
    if np.abs(np.cos(phi)) > 1e-6:
        psi   = np.arctan2(2.*(w*z + x*y), 1. - 2.*(y*y + z*z))
        theta = np.arctan2(2.*(w*x + y*z), 1. - 2.*(x*x + y*y))
    else:
        psi   = np.arctan2(-2.*(x*z - w*y), 1. - 2.*(y*y + x*x))
        theta = 0.
    return psi, phi, theta   # (yaw, pitch, roll)

def euler_zyx_to_quat(psi, phi, theta):
    """ZYX Euler (yaw, pitch, roll) -> unit quaternion [w,x,y,z]."""
    cy, sy = np.cos(psi/2.),   np.sin(psi/2.)
    cp, sp = np.cos(phi/2.),   np.sin(phi/2.)
    cr, sr = np.cos(theta/2.), np.sin(theta/2.)
    return np.array([
        cr*cp*cy + sr*sp*sy,
        sr*cp*cy - cr*sp*sy,
        cr*sp*cy + sr*cp*sy,
        cr*cp*sy - sr*sp*cy
    ])   # shape (4,)

psi0, phi0, theta0 = 1.1, 0.4, -0.7
q = euler_zyx_to_quat(psi0, phi0, theta0)   # shape (4,)
psi1, phi1, theta1 = quat_to_euler_zyx(q)
print(f"Original: ({np.rad2deg(psi0):.2f}°, {np.rad2deg(phi0):.2f}°, {np.rad2deg(theta0):.2f}°)")
print(f"Round-trip:({np.rad2deg(psi1):.2f}°, {np.rad2deg(phi1):.2f}°, {np.rad2deg(theta1):.2f}°)")
print(f"Match: {np.allclose([psi0,phi0,theta0],[psi1,phi1,theta1])}")

Original: (63.03°, 22.92°, -40.11°)
Round-trip:(63.03°, 22.92°, -40.11°)
Match: True

---

8.6.6 Full Conversion Chain Verification

Every path around the conversion graph should give the same rotation.

import numpy as np

# ---- helpers (all from above) ----
def Rx(a):
    c,s=np.cos(a),np.sin(a); return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def euler_to_mat(psi,phi,theta): return Rz(psi)@Ry(phi)@Rx(theta)
def mat_to_quat(R):
    tr=np.trace(R)
    if tr>0:
        s=0.5/np.sqrt(tr+1)
        return np.array([0.25/s,(R[2,1]-R[1,2])*s,(R[0,2]-R[2,0])*s,(R[1,0]-R[0,1])*s])
    elif R[0,0]>R[1,1] and R[0,0]>R[2,2]:
        s=2*np.sqrt(1+R[0,0]-R[1,1]-R[2,2])
        return np.array([(R[2,1]-R[1,2])/s,0.25*s,(R[0,1]+R[1,0])/s,(R[0,2]+R[2,0])/s])
    elif R[1,1]>R[2,2]:
        s=2*np.sqrt(1+R[1,1]-R[0,0]-R[2,2])
        return np.array([(R[0,2]-R[2,0])/s,(R[0,1]+R[1,0])/s,0.25*s,(R[1,2]+R[2,1])/s])
    else:
        s=2*np.sqrt(1+R[2,2]-R[0,0]-R[1,1])
        return np.array([(R[1,0]-R[0,1])/s,(R[0,2]+R[2,0])/s,(R[1,2]+R[2,1])/s,0.25*s])
def quat_to_mat(q):
    w,x,y,z=q/np.linalg.norm(q)
    return np.array([[1-2*(y*y+z*z),2*(x*y-w*z),2*(x*z+w*y)],
                     [2*(x*y+w*z),1-2*(x*x+z*z),2*(y*z-w*x)],
                     [2*(x*z-w*y),2*(y*z+w*x),1-2*(x*x+y*y)]])
def mat_to_axis_angle(R):
    tr=np.clip((np.trace(R)-1)/2,-1,1); theta=np.arccos(tr)
    if np.abs(np.sin(theta))<1e-7: return np.array([0.,0.,1.]),theta
    vee=np.array([R[2,1]-R[1,2],R[0,2]-R[2,0],R[1,0]-R[0,1]])
    return vee/(2*np.sin(theta)),theta
def axis_angle_to_mat(k,t):
    K=np.array([[0,-k[2],k[1]],[k[2],0,-k[0]],[-k[1],k[0],0]])
    return np.eye(3)*np.cos(t)+(1-np.cos(t))*np.outer(k,k)+np.sin(t)*K

# Start: a general rotation (not near any singularity)
R0 = euler_to_mat(0.6, 0.3, -0.4)   # shape (3, 3)

# Path 1: R → q → R
q1  = mat_to_quat(R0)
R1  = quat_to_mat(q1)

# Path 2: R → axis-angle → R
k2, t2 = mat_to_axis_angle(R0)
R2 = axis_angle_to_mat(k2, t2)

# Probe all paths
v = np.array([1., 2., 3.])   # shape (3,)
print("All paths rotate v = [1,2,3] identically:")
print(f"  R0            : {(R0 @ v).round(6)}")
print(f"  R → q → R    : {(R1 @ v).round(6)}  match: {np.allclose(R0@v, R1@v)}")
print(f"  R → aa → R   : {(R2 @ v).round(6)}  match: {np.allclose(R0@v, R2@v)}")

All paths rotate v = [1,2,3] identically:
  R0            : [-0.427325  3.355109  1.600198]
  R → q → R    : [-0.427325  3.355109  1.600198]  match: True
  R → aa → R   : [-0.427325  3.355109  1.600198]  match: True

---

8.6.7 Edge Cases and Numerical Notes

Conversion	Edge case	Symptom	Fix
Mat → Euler	\(\phi = \pm 90°\) (gimbal lock)	Infinite solutions	Set roll=0, recover yaw from \(R\)
Mat → Quat	\(\text{tr}(R) \approx -1\)	Near-zero denominator	Shepperd’s method (used above)
Quat → Mat	\(\|q\| \neq 1\)	Wrong rotation	Always normalise \(q\) before converting
Mat → AxisAngle	\(\theta \approx 0\)	Axis undefined	Return identity axis \([0,0,1]\)
Mat → AxisAngle	\(\theta \approx 180°\)	\(\sin\theta \approx 0\)	Use diagonal of \((R+I)/2\)

Every production library (scipy.spatial.transform, ROS2’s tf2, Eigen) handles these cases. When implementing from scratch, always add a small eps guard and a numerical test at the known singularity angles.

8.7 Application: Visualizing Camera Orientation

A RealSense D435 depth camera attached to a moving robot outputs a stream of 6-DoF poses. Each pose contains a rotation — and the way that rotation is stored and interpolated determines whether the on-screen trajectory looks smooth or stutters. This application animates an SO(3) trajectory and compares Euler-angle versus quaternion interpolation side by side.

---

8.7.1 Simulating a Camera Trajectory

We generate a synthetic trajectory: the camera pitches up 60°, yaws left 45°, then rolls 30° — a plausible “looking-around” motion.

import numpy as np

def Rx(a):
    c,s=np.cos(a),np.sin(a); return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def euler_to_mat(psi, phi, theta):
    """ZYX: yaw=psi, pitch=phi, roll=theta. Returns shape (3,3)."""
    return Rz(psi) @ Ry(phi) @ Rx(theta)

def mat_to_euler_zyx(R):
    phi = np.arcsin(np.clip(-R[2,0],-1.,1.))
    if np.abs(np.cos(phi)) > 1e-6:
        theta = np.arctan2(R[2,1], R[2,2])
        psi   = np.arctan2(R[1,0], R[0,0])
    else:
        theta = 0.
        psi   = np.arctan2(-R[0,1], R[1,1])
    return psi, phi, theta

# Key-frame rotations (yaw, pitch, roll in degrees)
keyframes_deg = np.array([
    [  0.,   0.,  0.],   # neutral
    [  0.,  60.,  0.],   # look up
    [-45.,  60.,  0.],   # look up-left
    [-45.,  60., 30.],   # tilt head
    [  0.,   0.,  0.],   # return to neutral
])   # shape (5, 3)

keyframes_rad = np.deg2rad(keyframes_deg)   # shape (5, 3)
R_keys = [euler_to_mat(*k) for k in keyframes_rad]   # list of (3,3)

print("Key-frame rotation matrices (first two):")
print(np.array(R_keys[0]).round(4))
print(np.array(R_keys[1]).round(4))

Key-frame rotation matrices (first two):
[[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]
[[ 0.5    0.     0.866]
 [ 0.     1.     0.   ]
 [-0.866  0.     0.5  ]]

---

8.7.2 Building the Trajectory: Euler LERP vs SLERP

We interpolate between consecutive key frames using both methods and record where the camera’s forward vector (\(+z\) axis) points.

import numpy as np

def Rx(a):
    c,s=np.cos(a),np.sin(a); return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])
def euler_to_mat(psi,phi,theta): return Rz(psi)@Ry(phi)@Rx(theta)

def mat_to_quat(R):
    tr=np.trace(R)
    if tr>0:
        s=0.5/np.sqrt(tr+1); return np.array([0.25/s,(R[2,1]-R[1,2])*s,(R[0,2]-R[2,0])*s,(R[1,0]-R[0,1])*s])
    elif R[0,0]>R[1,1] and R[0,0]>R[2,2]:
        s=2*np.sqrt(1+R[0,0]-R[1,1]-R[2,2])
        return np.array([(R[2,1]-R[1,2])/s,0.25*s,(R[0,1]+R[1,0])/s,(R[0,2]+R[2,0])/s])
    elif R[1,1]>R[2,2]:
        s=2*np.sqrt(1+R[1,1]-R[0,0]-R[2,2]); return np.array([(R[0,2]-R[2,0])/s,(R[0,1]+R[1,0])/s,0.25*s,(R[1,2]+R[2,1])/s])
    else:
        s=2*np.sqrt(1+R[2,2]-R[0,0]-R[1,1]); return np.array([(R[1,0]-R[0,1])/s,(R[0,2]+R[2,0])/s,(R[1,2]+R[2,1])/s,0.25*s])

def quat_to_mat(q):
    w,x,y,z=q/np.linalg.norm(q)
    return np.array([[1-2*(y*y+z*z),2*(x*y-w*z),2*(x*z+w*y)],
                     [2*(x*y+w*z),1-2*(x*x+z*z),2*(y*z-w*x)],
                     [2*(x*z-w*y),2*(y*z+w*x),1-2*(x*x+y*y)]])

def slerp(q0, q1, t):
    q0=q0/np.linalg.norm(q0); q1=q1/np.linalg.norm(q1)
    dot=np.clip(np.dot(q0,q1),-1.,1.)
    if dot<0.: q1=-q1; dot=-dot
    Om=np.arccos(dot)
    if Om<1e-8:
        q=(1-t)*q0+t*q1; return q/np.linalg.norm(q)
    return np.sin((1-t)*Om)/np.sin(Om)*q0 + np.sin(t*Om)/np.sin(Om)*q1

keyframes_deg = np.array([[0.,0.,0.],[0.,60.,0.],[-45.,60.,0.],[-45.,60.,30.],[0.,0.,0.]])
keyframes_rad = np.deg2rad(keyframes_deg)
R_keys = [euler_to_mat(*k) for k in keyframes_rad]
q_keys = [mat_to_quat(R) for R in R_keys]   # list of shape (4,)

N = 30   # frames per segment
forward = np.array([0., 0., 1.])   # camera +z axis

traj_euler  = []
traj_slerp  = []

for seg in range(len(R_keys) - 1):
    e0 = keyframes_rad[seg]      # shape (3,)
    e1 = keyframes_rad[seg + 1]  # shape (3,)
    q0 = q_keys[seg]             # shape (4,)
    q1 = q_keys[seg + 1]         # shape (4,)

    for i in range(N):
        t = i / N
        # Euler LERP
        e_t  = (1 - t) * e0 + t * e1   # shape (3,)
        R_e  = euler_to_mat(*e_t)        # shape (3, 3)
        traj_euler.append(R_e @ forward)

        # SLERP
        q_t  = slerp(q0, q1, t)         # shape (4,)
        R_q  = quat_to_mat(q_t)          # shape (3, 3)
        traj_slerp.append(R_q @ forward)

traj_euler = np.array(traj_euler)   # shape (120, 3)
traj_slerp = np.array(traj_slerp)   # shape (120, 3)

print(f"Trajectory frames: {len(traj_euler)}")
print(f"Max ||forward|| deviation from 1 (Euler): {np.abs(np.linalg.norm(traj_euler, axis=1) - 1).max():.2e}")
print(f"Max ||forward|| deviation from 1 (SLERP): {np.abs(np.linalg.norm(traj_slerp, axis=1) - 1).max():.2e}")

Trajectory frames: 120
Max ||forward|| deviation from 1 (Euler): 2.22e-16
Max ||forward|| deviation from 1 (SLERP): 2.22e-16

---

8.7.3 Plotting the Camera Orientation Trajectory

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D   # noqa: F401

def Rx(a):
    c,s=np.cos(a),np.sin(a); return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])
def euler_to_mat(psi,phi,theta): return Rz(psi)@Ry(phi)@Rx(theta)
def mat_to_quat(R):
    tr=np.trace(R)
    if tr>0:
        s=0.5/np.sqrt(tr+1); return np.array([0.25/s,(R[2,1]-R[1,2])*s,(R[0,2]-R[2,0])*s,(R[1,0]-R[0,1])*s])
    elif R[0,0]>R[1,1] and R[0,0]>R[2,2]:
        s=2*np.sqrt(1+R[0,0]-R[1,1]-R[2,2])
        return np.array([(R[2,1]-R[1,2])/s,0.25*s,(R[0,1]+R[1,0])/s,(R[0,2]+R[2,0])/s])
    elif R[1,1]>R[2,2]:
        s=2*np.sqrt(1+R[1,1]-R[0,0]-R[2,2]); return np.array([(R[0,2]-R[2,0])/s,(R[0,1]+R[1,0])/s,0.25*s,(R[1,2]+R[2,1])/s])
    else:
        s=2*np.sqrt(1+R[2,2]-R[0,0]-R[1,1]); return np.array([(R[1,0]-R[0,1])/s,(R[0,2]+R[2,0])/s,(R[1,2]+R[2,1])/s,0.25*s])
def quat_to_mat(q):
    w,x,y,z=q/np.linalg.norm(q)
    return np.array([[1-2*(y*y+z*z),2*(x*y-w*z),2*(x*z+w*y)],[2*(x*y+w*z),1-2*(x*x+z*z),2*(y*z-w*x)],[2*(x*z-w*y),2*(y*z+w*x),1-2*(x*x+y*y)]])
def slerp(q0,q1,t):
    q0=q0/np.linalg.norm(q0); q1=q1/np.linalg.norm(q1)
    dot=np.clip(np.dot(q0,q1),-1.,1.)
    if dot<0.: q1=-q1; dot=-dot
    Om=np.arccos(dot)
    if Om<1e-8:
        q=(1-t)*q0+t*q1; return q/np.linalg.norm(q)
    return np.sin((1-t)*Om)/np.sin(Om)*q0+np.sin(t*Om)/np.sin(Om)*q1

keyframes_deg = np.array([[0.,0.,0.],[0.,60.,0.],[-45.,60.,0.],[-45.,60.,30.],[0.,0.,0.]])
keyframes_rad = np.deg2rad(keyframes_deg)
R_keys = [euler_to_mat(*k) for k in keyframes_rad]
q_keys = [mat_to_quat(R) for R in R_keys]
N=30; forward=np.array([0.,0.,1.])
traj_euler=[]; traj_slerp=[]
for seg in range(len(R_keys)-1):
    e0=keyframes_rad[seg]; e1=keyframes_rad[seg+1]
    q0=q_keys[seg]; q1=q_keys[seg+1]
    for i in range(N):
        t=i/N
        traj_euler.append(euler_to_mat(*(1-t)*e0+t*e1)@forward)
        traj_slerp.append(quat_to_mat(slerp(q0,q1,t))@forward)
traj_euler=np.array(traj_euler); traj_slerp=np.array(traj_slerp)

# Key-frame positions of forward vector
kf_pts = np.array([R@forward for R in R_keys])   # shape (5, 3)

fig = plt.figure(figsize=(12, 5))
for idx, (traj, title, col) in enumerate([
    (traj_euler, 'Euler LERP (per-angle linear interp)', 'tomato'),
    (traj_slerp, 'SLERP (geodesic quaternion interp)',   '#4a90d9')]):
    ax = fig.add_subplot(1, 2, idx+1, projection='3d')
    ax.plot(traj[:,0], traj[:,1], traj[:,2], color=col, lw=2, alpha=0.9)
    ax.scatter(kf_pts[:,0], kf_pts[:,1], kf_pts[:,2],
               color='black', s=50, zorder=5, label='Key frames')
    # Draw unit sphere wireframe (light)
    u = np.linspace(0, 2*np.pi, 30)
    v = np.linspace(0, np.pi, 20)
    xs = np.outer(np.cos(u), np.sin(v))
    ys = np.outer(np.sin(u), np.sin(v))
    zs = np.outer(np.ones(len(u)), np.cos(v))
    ax.plot_surface(xs, ys, zs, alpha=0.04, color='#aaaaaa')
    ax.set_title(title, fontsize=9)
    ax.set_xlabel('x'); ax.set_ylabel('y'); ax.set_zlabel('z')
    ax.set_xlim(-1,1); ax.set_ylim(-1,1); ax.set_zlim(-1,1)

fig.suptitle('Camera +z axis trajectory on the unit sphere', fontsize=11)
plt.tight_layout()
plt.show()

---

8.7.4 Artefact Detection: Angular Speed Profile

SLERP sweeps at constant angular speed between key frames. Euler LERP produces speed spikes, visible as clusters of tightly packed points on the sphere.

import numpy as np
import matplotlib.pyplot as plt

def Rx(a):
    c,s=np.cos(a),np.sin(a); return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])
def euler_to_mat(psi,phi,theta): return Rz(psi)@Ry(phi)@Rx(theta)
def mat_to_quat(R):
    tr=np.trace(R)
    if tr>0:
        s=0.5/np.sqrt(tr+1); return np.array([0.25/s,(R[2,1]-R[1,2])*s,(R[0,2]-R[2,0])*s,(R[1,0]-R[0,1])*s])
    elif R[0,0]>R[1,1] and R[0,0]>R[2,2]:
        s=2*np.sqrt(1+R[0,0]-R[1,1]-R[2,2]); return np.array([(R[2,1]-R[1,2])/s,0.25*s,(R[0,1]+R[1,0])/s,(R[0,2]+R[2,0])/s])
    elif R[1,1]>R[2,2]:
        s=2*np.sqrt(1+R[1,1]-R[0,0]-R[2,2]); return np.array([(R[0,2]-R[2,0])/s,(R[0,1]+R[1,0])/s,0.25*s,(R[1,2]+R[2,1])/s])
    else:
        s=2*np.sqrt(1+R[2,2]-R[0,0]-R[1,1]); return np.array([(R[1,0]-R[0,1])/s,(R[0,2]+R[2,0])/s,(R[1,2]+R[2,1])/s,0.25*s])
def quat_to_mat(q):
    w,x,y,z=q/np.linalg.norm(q)
    return np.array([[1-2*(y*y+z*z),2*(x*y-w*z),2*(x*z+w*y)],[2*(x*y+w*z),1-2*(x*x+z*z),2*(y*z-w*x)],[2*(x*z-w*y),2*(y*z+w*x),1-2*(x*x+y*y)]])
def slerp(q0,q1,t):
    q0=q0/np.linalg.norm(q0); q1=q1/np.linalg.norm(q1)
    dot=np.clip(np.dot(q0,q1),-1.,1.)
    if dot<0.: q1=-q1; dot=-dot
    Om=np.arccos(dot)
    if Om<1e-8:
        q=(1-t)*q0+t*q1; return q/np.linalg.norm(q)
    return np.sin((1-t)*Om)/np.sin(Om)*q0+np.sin(t*Om)/np.sin(Om)*q1

keyframes_deg=np.array([[0.,0.,0.],[0.,60.,0.],[-45.,60.,0.],[-45.,60.,30.],[0.,0.,0.]])
keyframes_rad=np.deg2rad(keyframes_deg)
R_keys=[euler_to_mat(*k) for k in keyframes_rad]
q_keys=[mat_to_quat(R) for R in R_keys]

N=60; forward=np.array([0.,0.,1.])
traj_euler=[]; traj_slerp=[]
for seg in range(len(R_keys)-1):
    e0,e1=keyframes_rad[seg],keyframes_rad[seg+1]
    q0,q1=q_keys[seg],q_keys[seg+1]
    for i in range(N):
        t=i/N
        traj_euler.append(euler_to_mat(*(1-t)*e0+t*e1)@forward)
        traj_slerp.append(quat_to_mat(slerp(q0,q1,t))@forward)

traj_euler=np.array(traj_euler); traj_slerp=np.array(traj_slerp)

def angular_speed(traj):
    """Angle (degrees) swept between consecutive frames."""
    dots = np.einsum('ij,ij->i', traj[:-1], traj[1:]).clip(-1,1)
    return np.rad2deg(np.arccos(dots))

spd_e = angular_speed(traj_euler)
spd_s = angular_speed(traj_slerp)

frames = np.arange(len(spd_e))
fig, ax = plt.subplots(figsize=(10, 4))
ax.plot(frames, spd_e, color='tomato',   lw=1.5, label='Euler LERP (variable speed)')
ax.plot(frames, spd_s, color='#4a90d9', lw=1.5, label='SLERP (constant per segment)')
ax.set_xlabel('Frame index'); ax.set_ylabel('Angular step (degrees)')
ax.set_title('Angular speed profile: Euler LERP spikes; SLERP is smooth', fontsize=10)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.2)
ax.axhline(0, color='#333333', lw=0.4, alpha=0.5)
plt.tight_layout()
plt.show()

---

8.7.5 Kaggle Exploration

Dataset: Intel RealSense Camera IMU dataset — raw IMU readings with ground-truth quaternion pose logged at 200 Hz.

# Assumes: df loaded from CSV with columns 'qw','qx','qy','qz' (unit quaternions)
import numpy as np
import pandas as pd

df = pd.read_csv('realsense_imu_orientation.csv')
quats = df[['qw','qx','qy','qz']].values   # shape (N, 4)

# Normalise (IMU logs may have slight float drift)
norms = np.linalg.norm(quats, axis=1, keepdims=True)   # shape (N, 1)
quats = quats / norms                                    # shape (N, 4)

# Convert to ZYX Euler for comparison
def quat_to_euler_zyx(q):
    w,x,y,z=q
    sinp=2*(w*y-z*x)
    phi=np.arcsin(np.clip(sinp,-1,1))
    if np.abs(np.cos(phi))>1e-6:
        psi=np.arctan2(2*(w*z+x*y),1-2*(y*y+z*z))
        theta=np.arctan2(2*(w*x+y*z),1-2*(x*x+y*y))
    else:
        psi=np.arctan2(-2*(x*z-w*y),1-2*(y*y+x*x)); theta=0.
    return np.rad2deg([psi,phi,theta])

eulers = np.array([quat_to_euler_zyx(q) for q in quats])   # shape (N, 3)

# Plot yaw/pitch/roll over time
import matplotlib.pyplot as plt
t = np.arange(len(eulers)) / 200.   # 200 Hz -> seconds
fig, axes = plt.subplots(3, 1, figsize=(10, 7), sharex=True)
for ax, col, label in zip(axes, range(3), ['Yaw (°)', 'Pitch (°)', 'Roll (°)']):
    axes[col].plot(t, eulers[:,col], lw=0.8, color=['#4a90d9','tomato','#2ecc71'][col])
    axes[col].set_ylabel(label)
    axes[col].grid(True, alpha=0.2)
axes[-1].set_xlabel('Time (s)')
fig.suptitle('RealSense camera orientation from IMU quaternions', fontsize=11)
plt.tight_layout()
plt.show()

This exercise shows the full pipeline in a real system: quaternions from hardware → Euler angles for human readability → SLERP for smooth animation. The math is exactly Chapter 8.

8.8 Exercises and Solutions

8.8.1 Exercises

Exercise 8.1 (SO(3) properties) Let \(R_1 = R_z(30°)\) and \(R_2 = R_y(45°)\). (a) Verify that \(R_1 R_2 \in \text{SO}(3)\): check \(R^T R = I\) and \(\det R = +1\). (b) Is matrix multiplication in SO(3) commutative? Compute \(R_1 R_2\) and \(R_2 R_1\) and check whether they are equal. (c) What is the inverse of \(R_1 R_2\)? Compute it two ways: using \((R_1 R_2)^T\) and using \((R_2)^{-1}(R_1)^{-1}\), and verify they agree.

---

Exercise 8.2 (Gimbal lock) Consider the ZYX Euler angle formula for the pitch angle: \(\phi = \arcsin(-r_{31})\). (a) What value of \(r_{31}\) triggers the singularity? (b) Show that when \(\phi = 90°\), the rotation matrix \(R_{zyx}(\psi, 90°, \theta)\) can be written using only the sum \(\psi + \theta\), not \(\psi\) and \(\theta\) individually. (c) Compute \(R_{zyx}(30°, 90°, 60°)\) and \(R_{zyx}(90°, 90°, 0°)\) and verify they are equal.

---

Exercise 8.3 (Rodrigues’ formula) Apply Rodrigues’ formula directly (not via matrix multiplication) to verify that rotating \(\mathbf{v} = [1, 0, 0]^T\) by \(90°\) about \(\hat{k} = [0, 0, 1]^T\) gives \([0, 1, 0]^T\). Show each of the three terms in the formula explicitly.

---

Exercise 8.4 (Quaternion algebra) Let \(q_1 = \cos(30°) + \sin(30°)\mathbf{k}\) (rotation about \(z\) by \(60°\)) and \(q_2 = \cos(45°) + \sin(45°)\mathbf{j}\) (rotation about \(y\) by \(90°\)). (a) Compute the Hamilton product \(q_2 \otimes q_1\). (b) Convert both quaternions to \(3\times3\) rotation matrices and multiply them. (c) Verify that the product matrix equals the one derived from \(q_2 \otimes q_1\).

---

Exercise 8.5 (Double cover) (a) Show that \(q = [1, 0, 0, 0]\) and \(q = [-1, 0, 0, 0]\) produce the same rotation matrix. (b) Show that \(q = [0, 1, 0, 0]\) and \(q = [0, -1, 0, 0]\) produce the same rotation matrix. (c) What physical rotation does \([0, 1, 0, 0]\) represent?

---

Exercise 8.6 (SLERP verification) Let \(q_0\) be the identity quaternion and \(q_1\) the quaternion for \(120°\) about \([1,1,1]/\sqrt{3}\). (a) What does \(\text{slerp}(q_0, q_1, 1/3)\) represent geometrically? (b) Verify that applying \(\text{slerp}(q_0, q_1, 1/3)\) three times gives the same rotation as \(q_1\). (c) Check that intermediate unit quaternions all have \(\|q\| = 1\).

---

Exercise 8.7 (Conversion round-trip) Pick a rotation matrix \(R\) by composing \(R_z(72°) \cdot R_y(-35°) \cdot R_x(18°)\). (a) Convert \(R\) to a quaternion \(q\). (b) Convert \(q\) to Euler angles (ZYX). (c) Convert the Euler angles back to a rotation matrix \(R'\). (d) Verify \(R = R'\) and compute the maximum element-wise error.

---

Exercise 8.8 (Re-orthogonalisation) A rotation matrix \(R\) is perturbed: \(R' = R + 0.01 \cdot E\) where \(E\) is a random matrix. (a) How large is \(\|R'^T R' - I\|_F\)? (b) Project \(R'\) back onto SO(3) via SVD. What is \(\|R_\text{fixed}^T R_\text{fixed} - I\|_F\)? (c) How large is the angular error (in degrees) introduced by the perturbation, before and after correction?

---

8.8.2 Solutions

Solution 8.1

import numpy as np

def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

R1 = Rz(np.deg2rad(30.))   # shape (3, 3)
R2 = Ry(np.deg2rad(45.))   # shape (3, 3)

# (a)
R12 = R1 @ R2   # shape (3, 3)
print("(a) R1 @ R2:")
print(f"  R^T R = I?    {np.allclose(R12.T @ R12, np.eye(3))}")
print(f"  det = {np.linalg.det(R12):.8f}")

# (b)
R21 = R2 @ R1   # shape (3, 3)
print(f"\n(b) Commutative?  R1@R2 == R2@R1?  {np.allclose(R12, R21)}")
print("  R1@R2:\n", R12.round(4))
print("  R2@R1:\n", R21.round(4))

# (c) Inverse
R12_inv_T  = R12.T                     # shape (3, 3)
R12_inv_AB = R2.T @ R1.T               # shape (3, 3) — (R2^{-1})(R1^{-1})
print(f"\n(c) (R12)^T == R2^T @ R1^T?  {np.allclose(R12_inv_T, R12_inv_AB)}")
print(f"  R12 @ R12^T = I?  {np.allclose(R12 @ R12_inv_T, np.eye(3))}")

(a) R1 @ R2:
  R^T R = I?    True
  det = 1.00000000

(b) Commutative?  R1@R2 == R2@R1?  False
  R1@R2:
 [[ 0.6124 -0.5     0.6124]
 [ 0.3536  0.866   0.3536]
 [-0.7071  0.      0.7071]]
  R2@R1:
 [[ 0.6124 -0.3536  0.7071]
 [ 0.5     0.866   0.    ]
 [-0.6124  0.3536  0.7071]]

(c) (R12)^T == R2^T @ R1^T?  True
  R12 @ R12^T = I?  True

---

Solution 8.2

import numpy as np

def Rx(a):
    c,s=np.cos(a),np.sin(a); return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])
def R_zyx(psi,phi,theta): return Rz(psi)@Ry(phi)@Rx(theta)

# (a) r31 = -sin(phi); singularity when phi = ±90° i.e. r31 = ±1
print("(a) r31 = -sin(phi); gimbal lock when phi = ±90° → r31 = ∓1")

# (b) At phi=90°, Ry(90°) = [[0,0,1],[0,1,0],[-1,0,0]]
# R_zyx(psi,90°,theta) = Rz(psi)@Ry(90°)@Rx(theta)
# The (0,0) entry is sin(psi-theta) — depends only on psi+theta (or psi-theta)
# Specifically: can be shown to equal R_zyx(psi+theta, 90°, 0)
print("(b) At pitch=90°, R_zyx(psi,90°,theta) = R_zyx(psi+theta, 90°, 0)")

# (c) Verify
psi, phi, theta = np.deg2rad(30.), np.deg2rad(90.), np.deg2rad(60.)
Ra = R_zyx(psi, phi, theta)

psi2, phi2, theta2 = np.deg2rad(90.), np.deg2rad(90.), np.deg2rad(0.)
Rb = R_zyx(psi2, phi2, theta2)

print(f"\n(c) R_zyx(30°,90°,60°) == R_zyx(90°,90°,0°)?  {np.allclose(Ra, Rb)}")
print("Ra ="); print(Ra.round(6))
print("Rb ="); print(Rb.round(6))

(a) r31 = -sin(phi); gimbal lock when phi = ±90° → r31 = ∓1
(b) At pitch=90°, R_zyx(psi,90°,theta) = R_zyx(psi+theta, 90°, 0)

(c) R_zyx(30°,90°,60°) == R_zyx(90°,90°,0°)?  False
Ra =
[[ 0.        0.5       0.866025]
 [ 0.        0.866025 -0.5     ]
 [-1.        0.        0.      ]]
Rb =
[[ 0. -1.  0.]
 [ 0.  0.  1.]
 [-1.  0.  0.]]

---

Solution 8.3

import numpy as np

theta = np.pi / 2.
k_hat = np.array([0., 0., 1.])   # shape (3,)
v     = np.array([1., 0., 0.])   # shape (3,)

# Three terms of Rodrigues' formula applied to v:
# Term 1: cos(theta) * v
term1 = np.cos(theta) * v
# Term 2: (1-cos(theta)) * (k_hat . v) * k_hat
term2 = (1 - np.cos(theta)) * np.dot(k_hat, v) * k_hat
# Term 3: sin(theta) * (k_hat × v)
term3 = np.sin(theta) * np.cross(k_hat, v)

result = term1 + term2 + term3
print("Term 1 (cos θ · v):             ", term1.round(6))
print("Term 2 ((1-cosθ)(k̂·v)k̂):       ", term2.round(6))
print("Term 3 (sinθ · k̂×v):           ", term3.round(6))
print("Sum (should be [0,1,0]):        ", result.round(6))
print(f"Correct? {np.allclose(result, [0.,1.,0.])}")

Term 1 (cos θ · v):              [0. 0. 0.]
Term 2 ((1-cosθ)(k̂·v)k̂):        [0. 0. 0.]
Term 3 (sinθ · k̂×v):            [0. 1. 0.]
Sum (should be [0,1,0]):         [0. 1. 0.]
Correct? True

---

Solution 8.4

import numpy as np

def quat_mul(p, q):
    pw,px,py,pz=p; qw,qx,qy,qz=q
    return np.array([pw*qw-px*qx-py*qy-pz*qz, pw*qx+px*qw+py*qz-pz*qy,
                     pw*qy-px*qz+py*qw+pz*qx, pw*qz+px*qy-py*qx+pz*qw])

def quat_to_mat(q):
    w,x,y,z=q/np.linalg.norm(q)
    return np.array([[1-2*(y*y+z*z),2*(x*y-w*z),2*(x*z+w*y)],
                     [2*(x*y+w*z),1-2*(x*x+z*z),2*(y*z-w*x)],
                     [2*(x*z-w*y),2*(y*z+w*x),1-2*(x*x+y*y)]])

# q1: 60° about z → axis-angle theta=60° → quat w=cos30°, z=sin30°
theta1 = np.deg2rad(60.); q1 = np.array([np.cos(theta1/2),0.,0.,np.sin(theta1/2)])
# q2: 90° about y → w=cos45°, y=sin45°
theta2 = np.deg2rad(90.); q2 = np.array([np.cos(theta2/2),0.,np.sin(theta2/2),0.])

# (a) Hamilton product q2 ⊗ q1
q_prod = quat_mul(q2, q1)   # shape (4,)
print("(a) q2 ⊗ q1 =", q_prod.round(6))

# (b) Matrix product
R1 = quat_to_mat(q1); R2 = quat_to_mat(q2)
R_mat = R2 @ R1   # shape (3, 3)
print("\n(b) R2 @ R1:"); print(R_mat.round(6))

# (c) Matrix from q2 ⊗ q1
R_qprod = quat_to_mat(q_prod)   # shape (3, 3)
print("\n(c) quat_to_mat(q2⊗q1):"); print(R_qprod.round(6))
print(f"Match: {np.allclose(R_mat, R_qprod)}")

(a) q2 ⊗ q1 = [0.612372 0.353553 0.612372 0.353553]

(b) R2 @ R1:
[[-0.        0.        1.      ]
 [ 0.866025  0.5       0.      ]
 [-0.5       0.866025 -0.      ]]

(c) quat_to_mat(q2⊗q1):
[[ 0.        0.        1.      ]
 [ 0.866025  0.5       0.      ]
 [-0.5       0.866025  0.      ]]
Match: True

---

Solution 8.5

import numpy as np

def quat_to_mat(q):
    w,x,y,z=q/np.linalg.norm(q)
    return np.array([[1-2*(y*y+z*z),2*(x*y-w*z),2*(x*z+w*y)],
                     [2*(x*y+w*z),1-2*(x*x+z*z),2*(y*z-w*x)],
                     [2*(x*z-w*y),2*(y*z+w*x),1-2*(x*x+y*y)]])

qa  = np.array([ 1., 0., 0., 0.])
qa_neg = -qa
Qa  = quat_to_mat(qa);     Qa_neg = quat_to_mat(qa_neg)
print("(a) q=[1,0,0,0] and -q=[-1,0,0,0] give same R?", np.allclose(Qa, Qa_neg))
print("    R = I (identity rotation):", np.allclose(Qa, np.eye(3)))

qb  = np.array([0., 1., 0., 0.])
qb_neg = -qb
Qb  = quat_to_mat(qb);     Qb_neg = quat_to_mat(qb_neg)
print("\n(b) q=[0,1,0,0] and -q=[0,-1,0,0] give same R?", np.allclose(Qb, Qb_neg))
print("    R ="); print(Qb.round(6))

# (c) [0,1,0,0] → theta=2*arccos(0)=180°, axis=x
print("\n(c) [0,1,0,0] encodes: 180° rotation about x-axis")
print("    Geometric check — R @ e_y should give -e_y:")
print("    R @ e_y =", (Qb @ np.array([0.,1.,0.])).round(6))

(a) q=[1,0,0,0] and -q=[-1,0,0,0] give same R? True
    R = I (identity rotation): True

(b) q=[0,1,0,0] and -q=[0,-1,0,0] give same R? True
    R =
[[ 1.  0.  0.]
 [ 0. -1.  0.]
 [ 0.  0. -1.]]

(c) [0,1,0,0] encodes: 180° rotation about x-axis
    Geometric check — R @ e_y should give -e_y:
    R @ e_y = [ 0. -1.  0.]

---

Solution 8.6

import numpy as np

def axis_angle_to_quat(k_hat, theta):
    w=np.cos(theta/2.); v=np.sin(theta/2.)*k_hat
    return np.array([w,v[0],v[1],v[2]])

def slerp(q0, q1, t):
    q0=q0/np.linalg.norm(q0); q1=q1/np.linalg.norm(q1)
    dot=np.clip(np.dot(q0,q1),-1.,1.)
    if dot<0.: q1=-q1; dot=-dot
    Om=np.arccos(dot)
    if Om<1e-8:
        q=(1-t)*q0+t*q1; return q/np.linalg.norm(q)
    return np.sin((1-t)*Om)/np.sin(Om)*q0+np.sin(t*Om)/np.sin(Om)*q1

def quat_mul(p,q):
    pw,px,py,pz=p; qw,qx,qy,qz=q
    return np.array([pw*qw-px*qx-py*qy-pz*qz,pw*qx+px*qw+py*qz-pz*qy,
                     pw*qy-px*qz+py*qw+pz*qx,pw*qz+px*qy-py*qx+pz*qw])

q0 = np.array([1., 0., 0., 0.])   # identity
k1 = np.array([1.,1.,1.])/np.sqrt(3.)
q1 = axis_angle_to_quat(k1, np.deg2rad(120.))   # shape (4,)

# (a) slerp at t=1/3 represents 120°/3 = 40° about (1,1,1)/√3
q_third = slerp(q0, q1, 1./3.)
dot = np.clip(q0.dot(q_third), -1., 1.)
angle_third = np.rad2deg(2*np.arccos(dot))
print(f"(a) slerp(q0,q1,1/3): angle = {angle_third:.4f}°  (expected 40°)")
print(f"    axis ≈ (1,1,1)/√3?  {np.allclose(q_third[1:]/np.linalg.norm(q_third[1:]), k1)}")

# (b) Apply three times: q_third^3 should equal q1
q_cubed = quat_mul(quat_mul(q_third, q_third), q_third)   # shape (4,)
print(f"\n(b) q_third^3 == q1?  {np.allclose(np.abs(q_cubed), np.abs(q1))}")
print(f"    q_third^3 = {q_cubed.round(6)}")
print(f"    q1        = {q1.round(6)}")

# (c) All intermediate ||q|| = 1
ts = np.linspace(0, 1, 20)
norms = [np.linalg.norm(slerp(q0, q1, t)) for t in ts]
print(f"\n(c) Min ||q|| = {min(norms):.8f},  Max ||q|| = {max(norms):.8f}  (all = 1)")

(a) slerp(q0,q1,1/3): angle = 40.0000°  (expected 40°)
    axis ≈ (1,1,1)/√3?  True

(b) q_third^3 == q1?  True
    q_third^3 = [0.5 0.5 0.5 0.5]
    q1        = [0.5 0.5 0.5 0.5]

(c) Min ||q|| = 1.00000000,  Max ||q|| = 1.00000000  (all = 1)

---

Solution 8.7

import numpy as np

def Rx(a):
    c,s=np.cos(a),np.sin(a); return np.array([[1.,0.,0.],[0.,c,-s],[0.,s,c]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])
def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])

def mat_to_quat(R):
    tr=np.trace(R)
    if tr>0:
        s=0.5/np.sqrt(tr+1); return np.array([0.25/s,(R[2,1]-R[1,2])*s,(R[0,2]-R[2,0])*s,(R[1,0]-R[0,1])*s])
    elif R[0,0]>R[1,1] and R[0,0]>R[2,2]:
        s=2*np.sqrt(1+R[0,0]-R[1,1]-R[2,2]); return np.array([(R[2,1]-R[1,2])/s,0.25*s,(R[0,1]+R[1,0])/s,(R[0,2]+R[2,0])/s])
    elif R[1,1]>R[2,2]:
        s=2*np.sqrt(1+R[1,1]-R[0,0]-R[2,2]); return np.array([(R[0,2]-R[2,0])/s,(R[0,1]+R[1,0])/s,0.25*s,(R[1,2]+R[2,1])/s])
    else:
        s=2*np.sqrt(1+R[2,2]-R[0,0]-R[1,1]); return np.array([(R[1,0]-R[0,1])/s,(R[0,2]+R[2,0])/s,(R[1,2]+R[2,1])/s,0.25*s])

def quat_to_euler_zyx(q):
    w,x,y,z=q/np.linalg.norm(q)
    sinp=2*(w*y-z*x); phi=np.arcsin(np.clip(sinp,-1,1))
    if np.abs(np.cos(phi))>1e-6:
        psi=np.arctan2(2*(w*z+x*y),1-2*(y*y+z*z)); theta=np.arctan2(2*(w*x+y*z),1-2*(x*x+y*y))
    else:
        psi=np.arctan2(-2*(x*z-w*y),1-2*(y*y+x*x)); theta=0.
    return psi,phi,theta

def euler_to_mat(psi,phi,theta): return Rz(psi)@Ry(phi)@Rx(theta)

# (a)
R = Rz(np.deg2rad(72.)) @ Ry(np.deg2rad(-35.)) @ Rx(np.deg2rad(18.))   # shape (3,3)
q = mat_to_quat(R)   # shape (4,)
print("(a) q =", q.round(6))

# (b)
psi1,phi1,theta1 = quat_to_euler_zyx(q)
print(f"(b) ZYX Euler: yaw={np.rad2deg(psi1):.4f}°, pitch={np.rad2deg(phi1):.4f}°, roll={np.rad2deg(theta1):.4f}°")

# (c,d)
Rprime = euler_to_mat(psi1,phi1,theta1)   # shape (3,3)
max_err = np.abs(R - Rprime).max()
print(f"(d) R == R'?  {np.allclose(R, Rprime)}  Max element-wise error: {max_err:.2e}")

(a) q = [ 0.734424  0.295275 -0.152587  0.591736]
(b) ZYX Euler: yaw=72.0000°, pitch=-35.0000°, roll=18.0000°
(d) R == R'?  True  Max element-wise error: 1.39e-16

---

Solution 8.8

import numpy as np

rng = np.random.default_rng(7)

def Rz(g):
    c,s=np.cos(g),np.sin(g); return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])
def Ry(b):
    c,s=np.cos(b),np.sin(b); return np.array([[c,0.,s],[0.,1.,0.],[-s,0.,c]])

R = Rz(0.5) @ Ry(0.3)   # shape (3, 3) — clean rotation
E = rng.standard_normal((3, 3))   # shape (3, 3)
Rp = R + 0.01 * E                 # shape (3, 3) — perturbed

# (a)
orth_err_before = np.linalg.norm(Rp.T @ Rp - np.eye(3), 'fro')
print(f"(a) ||R'^T R' - I||_F before fix: {orth_err_before:.6f}")

# (b) Project back onto SO(3) via SVD
U, _, Vt = np.linalg.svd(Rp)
R_fixed = U @ Vt   # shape (3, 3)
if np.linalg.det(R_fixed) < 0:
    U[:,-1] *= -1; R_fixed = U @ Vt
orth_err_after = np.linalg.norm(R_fixed.T @ R_fixed - np.eye(3), 'fro')
print(f"(b) ||R_fixed^T R_fixed - I||_F after: {orth_err_after:.2e}")

# (c) Angular error: angle of R_fixed @ R^T (which should be identity if no error)
dR_before = Rp       @ R.T   # shape (3, 3)
dR_after  = R_fixed  @ R.T   # shape (3, 3)

def angle_of_R(M):
    """Angle of a near-rotation matrix."""
    return np.rad2deg(np.arccos(np.clip((np.trace(M)-1)/2, -1., 1.)))

print(f"(c) Angular error before fix: {angle_of_R(dR_before):.4f}°")
print(f"    Angular error after  fix: {angle_of_R(dR_after):.4f}°  (near 0)")

(a) ||R'^T R' - I||_F before fix: 0.027472
(b) ||R_fixed^T R_fixed - I||_F after: 7.04e-16
(c) Angular error before fix: 7.3619°
    Angular error after  fix: 0.6149°  (near 0)