28  Appendix C: Rotation Representation Conversions

28.1 Rotation Representations

A 3-D rotation can be described in four equivalent forms. Each has different advantages for storage, composition, interpolation, and numerical stability.

Representation Parameters Constraints Best for
Rotation matrix \(R\) 9 floats \(R^TR = I\), \(\det R = +1\) Composing, transforming points, numerical linear algebra
Euler angles (ZYX) 3 floats (\(\psi, \theta, \phi\)) None (but see gimbal lock) Human-readable, sensor output, aerospace/robotics convention
Axis-angle \((k, \theta)\) 4 floats (unit \(k\), angle \(\theta\)) \(\|k\| = 1\), \(\theta \in [0, \pi]\) Lie algebra; incremental updates
Unit quaternion \(q\) 4 floats \((w, x, y, z)\) \(\|q\| = 1\) Interpolation (SLERP), singularity-free, compact

28.1.1 Conventions Used in This Appendix

  • Euler angles: ZYX intrinsic convention (yaw \(\psi\), pitch \(\theta\), roll \(\phi\)) applied right-to-left: \(R = R_z(\psi)\,R_y(\theta)\,R_x(\phi)\). Angles in radians throughout.

  • Axis-angle: unit axis \(k \in \mathbb{R}^3\), angle \(\theta \in [0, \pi]\). The rotation vector \(\omega = \theta k\) (compact form, no separate angle) is also used in Lie algebra notation.

  • Quaternion: scalar-first \(q = (w, x, y, z)\) in mathematical formulas; scipy.spatial.transform.Rotation uses scalar-last [x, y, z, w] – see Section 28.6 for the conversion.

28.1.2 The 12 Pairwise Conversions

import matplotlib.pyplot as plt
import matplotlib.patches as mpatches

fig, ax = plt.subplots(figsize=(7, 5))
ax.set_xlim(-0.2, 1.2)
ax.set_ylim(-0.2, 1.2)
ax.axis('off')

nodes = {
    'Matrix\n$R$':          (0.5, 1.0),
    'Euler ZYX\n$(\psi,\\theta,\\phi)$': (0.0, 0.4),
    'Axis-angle\n$(k,\\theta)$':         (1.0, 0.4),
    'Quaternion\n$q$':                   (0.5, 0.0),
}
colors = {
    'Matrix\n$R$':          '#4a90d9',
    'Euler ZYX\n$(\psi,\\theta,\\phi)$': '#2ecc71',
    'Axis-angle\n$(k,\\theta)$':         '#2ecc71',
    'Quaternion\n$q$':                   '#2ecc71',
}

for name, (x, y) in nodes.items():
    box = mpatches.FancyBboxPatch(
        (x - 0.13, y - 0.085), 0.26, 0.17,
        boxstyle='round,pad=0.02',
        facecolor=colors[name], edgecolor='#333333',
        linewidth=1.2, alpha=0.88, zorder=3,
    )
    ax.add_patch(box)
    ax.text(x, y, name, ha='center', va='center',
            fontsize=8.5, fontweight='bold', color='white', zorder=4)

direct = [
    ('Matrix\n$R$', 'Euler ZYX\n$(\psi,\\theta,\\phi)$'),
    ('Euler ZYX\n$(\psi,\\theta,\\phi)$', 'Matrix\n$R$'),
    ('Matrix\n$R$', 'Axis-angle\n$(k,\\theta)$'),
    ('Axis-angle\n$(k,\\theta)$', 'Matrix\n$R$'),
    ('Matrix\n$R$', 'Quaternion\n$q$'),
    ('Quaternion\n$q$', 'Matrix\n$R$'),
    ('Axis-angle\n$(k,\\theta)$', 'Quaternion\n$q$'),
    ('Quaternion\n$q$', 'Axis-angle\n$(k,\\theta)$'),
    ('Euler ZYX\n$(\psi,\\theta,\\phi)$', 'Quaternion\n$q$'),
    ('Quaternion\n$q$', 'Euler ZYX\n$(\psi,\\theta,\\phi)$'),
    ('Euler ZYX\n$(\psi,\\theta,\\phi)$', 'Axis-angle\n$(k,\\theta)$'),
    ('Axis-angle\n$(k,\\theta)$', 'Euler ZYX\n$(\psi,\\theta,\\phi)$'),
]

for i, (src, dst) in enumerate(direct):
    x0, y0 = nodes[src]
    x1, y1 = nodes[dst]
    off = 0.018 * (1 if i % 2 == 0 else -1)
    ax.annotate('', xy=(x1 + off, y1), xytext=(x0 + off, y0),
                arrowprops=dict(arrowstyle='->', color='#4a90d9', lw=1.2),
                zorder=2)

ax.set_title('Rotation representation conversion graph  (12 paths)', fontsize=10)
plt.tight_layout()
plt.show()
Figure 28.1: The 12 pairwise conversion paths among four rotation representations. Direct closed-form paths (solid blue) avoid numerical error accumulation; chained paths (dashed grey) pass through the rotation matrix.

28.2 Euler ZYX Conversions

ZYX Euler angles apply three elemental rotations right-to-left:

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

where \(\psi\) is yaw (about \(z\)), \(\theta\) is pitch (about \(y\)), and \(\phi\) is roll (about \(x\)).

The three elementary rotation matrices are:

\[ R_x(\phi) = \begin{bmatrix}1 & 0 & 0 \\ 0 & c_\phi & -s_\phi \\ 0 & s_\phi & c_\phi\end{bmatrix}, \quad R_y(\theta) = \begin{bmatrix}c_\theta & 0 & s_\theta \\ 0 & 1 & 0 \\ -s_\theta & 0 & c_\theta\end{bmatrix}, \quad R_z(\psi) = \begin{bmatrix}c_\psi & -s_\psi & 0 \\ s_\psi & c_\psi & 0 \\ 0 & 0 & 1\end{bmatrix} \]


28.2.1 Euler ZYX to Rotation Matrix

The product \(R = R_z(\psi)\,R_y(\theta)\,R_x(\phi)\) expands to:

\[ R = \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi \\ s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix} \]

import numpy as np

def euler_zyx_to_matrix(yaw, pitch, roll):
    """ZYX Euler angles (radians) -> rotation matrix."""
    cp, sp = np.cos(yaw),   np.sin(yaw)    # psi
    ct, st = np.cos(pitch), np.sin(pitch)  # theta
    cf, sf = np.cos(roll),  np.sin(roll)   # phi
    Rz = np.array([[ cp, -sp, 0],
                   [ sp,  cp, 0],
                   [  0,   0, 1]])    # shape (3, 3)
    Ry = np.array([[ ct, 0, st],
                   [  0, 1,  0],
                   [-st, 0, ct]])    # shape (3, 3)
    Rx = np.array([[1,   0,   0],
                   [0,  cf, -sf],
                   [0,  sf,  cf]])   # shape (3, 3)
    return Rz @ Ry @ Rx              # shape (3, 3)

# Example: yaw=30 deg, pitch=20 deg, roll=10 deg
yaw, pitch, roll = np.radians([30.0, 20.0, 10.0])
R = euler_zyx_to_matrix(yaw, pitch, roll)    # shape (3, 3)
print("R:\n", R.round(4))
print("det(R) =", np.linalg.det(R).round(6))   # should be +1
print("||R^T R - I|| =", np.linalg.norm(R.T @ R - np.eye(3)).round(10))
R:
 [[ 0.8138 -0.441   0.3785]
 [ 0.4698  0.8826  0.018 ]
 [-0.342   0.1632  0.9254]]
det(R) = 1.0
||R^T R - I|| = 0.0

28.2.2 Rotation Matrix to Euler ZYX

From the element positions of \(R\):

\[ \theta = \arcsin(-R_{20}), \qquad \psi = \operatorname{atan2}(R_{10},\, R_{00}), \qquad \phi = \operatorname{atan2}(R_{21},\, R_{22}) \]

valid when \(\cos\theta \ne 0\). At gimbal lock (\(\theta = \pm\pi/2\)) the yaw and roll axes align and only their sum or difference is determined; see Section 28.5 for the fallback.

import numpy as np

def matrix_to_euler_zyx(R):
    """Rotation matrix -> ZYX Euler angles (yaw, pitch, roll) in radians."""
    pitch = np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))
    if np.abs(np.cos(pitch)) > 1e-6:
        yaw  = np.arctan2(R[1, 0], R[0, 0])
        roll = np.arctan2(R[2, 1], R[2, 2])
    else:
        # Gimbal lock: pitch = +/-pi/2
        yaw  = np.arctan2(-R[0, 1], R[1, 1])
        roll = 0.0
    return yaw, pitch, roll

# Round-trip test
yaw0, pitch0, roll0 = np.radians([30.0, 20.0, 10.0])
R = np.array([[ 0.8138,  -0.4410,  0.3785],
              [ 0.4698,   0.8826,  0.0180],
              [-0.3420,   0.1632,  0.9254]])   # shape (3, 3)  approx

# Build exact R then recover
from functools import reduce
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(a): c,s=np.cos(a),np.sin(a); return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def _rz(a): c,s=np.cos(a),np.sin(a); return np.array([[c,-s,0],[s,c,0],[0,0,1]])

R_exact = _rz(yaw0) @ _ry(pitch0) @ _rx(roll0)    # shape (3, 3)
yaw1, pitch1, roll1 = matrix_to_euler_zyx(R_exact)
print("Original  (deg):", np.degrees([yaw0, pitch0, roll0]).round(4))
print("Recovered (deg):", np.degrees([yaw1, pitch1, roll1]).round(4))
Original  (deg): [30. 20. 10.]
Recovered (deg): [30. 20. 10.]

28.2.3 Euler ZYX to Quaternion

Using half-angle products (no intermediate matrix):

\[ q = \begin{pmatrix}w \\ x \\ y \\ z\end{pmatrix} = \begin{pmatrix} c_{\psi/2}c_{\theta/2}c_{\phi/2} + s_{\psi/2}s_{\theta/2}s_{\phi/2} \\ c_{\psi/2}c_{\theta/2}s_{\phi/2} - s_{\psi/2}s_{\theta/2}c_{\phi/2} \\ c_{\psi/2}s_{\theta/2}c_{\phi/2} + s_{\psi/2}c_{\theta/2}s_{\phi/2} \\ s_{\psi/2}c_{\theta/2}c_{\phi/2} - c_{\psi/2}s_{\theta/2}s_{\phi/2} \end{pmatrix} \]

import numpy as np

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

yaw, pitch, roll = np.radians([30.0, 20.0, 10.0])
q = euler_zyx_to_quat(yaw, pitch, roll)    # shape (4,)
print("q (w,x,y,z):", q.round(6))
print("||q|| =", np.linalg.norm(q).round(10))
q (w,x,y,z): [0.951549 0.038135 0.189308 0.239298]
||q|| = 1.0

28.2.4 Quaternion to Euler ZYX

Convert via rotation matrix (most numerically robust for all angles):

import numpy as np

def quat_to_matrix(q):
    """Unit quaternion (w, x, y, z) -> rotation matrix."""
    w, x, y, z = 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)],
    ])    # shape (3, 3)

def matrix_to_euler_zyx(R):
    pitch = np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))
    if np.abs(np.cos(pitch)) > 1e-6:
        yaw  = np.arctan2(R[1, 0], R[0, 0])
        roll = np.arctan2(R[2, 1], R[2, 2])
    else:
        yaw  = np.arctan2(-R[0, 1], R[1, 1])
        roll = 0.0
    return yaw, pitch, roll

def quat_to_euler_zyx(q):
    """Unit quaternion (w, x, y, z) -> ZYX Euler angles (radians)."""
    return matrix_to_euler_zyx(quat_to_matrix(q))

# Round-trip
yaw0, pitch0, roll0 = np.radians([30.0, 20.0, 10.0])

def euler_zyx_to_quat(yaw, pitch, roll):
    cy, sy = np.cos(yaw/2), np.sin(yaw/2)
    cp, sp = np.cos(pitch/2), np.sin(pitch/2)
    cr, sr = np.cos(roll/2), np.sin(roll/2)
    return np.array([cy*cp*cr + sy*sp*sr, cy*cp*sr - sy*sp*cr,
                     cy*sp*cr + sy*cp*sr, sy*cp*cr - cy*sp*sr])

q  = euler_zyx_to_quat(yaw0, pitch0, roll0)    # shape (4,)
yaw1, pitch1, roll1 = quat_to_euler_zyx(q)
print("Original  (deg):", np.degrees([yaw0, pitch0, roll0]).round(4))
print("Recovered (deg):", np.degrees([yaw1, pitch1, roll1]).round(4))
Original  (deg): [30. 20. 10.]
Recovered (deg): [30. 20. 10.]

28.3 Axis-Angle Conversions

An axis-angle rotation is specified by a unit axis \(k \in \mathbb{R}^3\) (\(\|k\| = 1\)) and an angle \(\theta \in [0, \pi]\). The skew-symmetric matrix of \(k\) is:

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


28.3.1 Axis-Angle to Rotation Matrix (Rodrigues)

Rodrigues’ rotation formula:

\[R = I + \sin\theta\, K + (1 - \cos\theta)\, K^2\]

This is the exp map from \(\mathfrak{so}(3)\) to \(\mathrm{SO}(3)\) (see Chapter 24).

import numpy as np

def skew(k):
    """3-vector -> 3x3 skew-symmetric matrix [k]_x."""
    kx, ky, kz = k
    return np.array([[ 0,  -kz,  ky],
                     [ kz,   0, -kx],
                     [-ky,  kx,   0]])    # shape (3, 3)

def axisangle_to_matrix(k, theta):
    """Unit axis k, angle theta (radians) -> rotation matrix (Rodrigues)."""
    k = k / np.linalg.norm(k)    # ensure unit; shape (3,)
    K = skew(k)                   # shape (3, 3)
    return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K)

# 90 degrees about the z-axis
k = np.array([0.0, 0.0, 1.0])    # shape (3,)
theta = np.pi / 2
R = axisangle_to_matrix(k, theta)    # shape (3, 3)
print("R (90 deg about z):\n", R.round(4))

# Verify: x-axis maps to y-axis
v = np.array([1.0, 0.0, 0.0])    # shape (3,)
print("R @ [1,0,0] =", (R @ v).round(4))   # [0, 1, 0]
R (90 deg about z):
 [[ 0. -1.  0.]
 [ 1.  0.  0.]
 [ 0.  0.  1.]]
R @ [1,0,0] = [0. 1. 0.]

28.3.2 Rotation Matrix to Axis-Angle

\[ \theta = \arccos\!\left(\frac{\operatorname{tr}(R) - 1}{2}\right), \qquad k = \frac{1}{2\sin\theta}\begin{pmatrix}R_{21}-R_{12} \\ R_{02}-R_{20} \\ R_{10}-R_{01}\end{pmatrix} \]

This formula is valid for \(\theta \in (0, \pi)\). See Section 28.5 for the \(\theta \approx 0\) and \(\theta \approx \pi\) cases.

import numpy as np

def matrix_to_axisangle(R):
    """Rotation matrix -> (unit axis k, angle theta in radians).
    Returns (k, theta=0, axis=[1,0,0]) for near-identity.
    """
    cos_theta = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0)
    theta = np.arccos(cos_theta)
    if theta < 1e-8:
        return np.array([1.0, 0.0, 0.0]), 0.0
    if np.pi - theta < 1e-8:
        # Near 180 deg: use diagonal of R+I to find axis
        M = R + np.eye(3)    # shape (3, 3)
        col_norms = np.linalg.norm(M, axis=0)   # shape (3,)
        i = np.argmax(col_norms)
        k = M[:, i] / col_norms[i]
        return k, theta
    k = np.array([R[2, 1] - R[1, 2],
                  R[0, 2] - R[2, 0],
                  R[1, 0] - R[0, 1]]) / (2.0 * np.sin(theta))    # shape (3,)
    return k, theta

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

k0 = np.array([1.0, 2.0, 3.0]); k0 /= np.linalg.norm(k0)    # shape (3,)
theta0 = 1.1    # radians
K = skew(k0)
R = np.eye(3) + np.sin(theta0)*K + (1-np.cos(theta0))*(K@K)   # shape (3, 3)

k1, theta1 = matrix_to_axisangle(R)
print("Original  theta:", round(theta0, 6), " k:", k0.round(6))
print("Recovered theta:", round(theta1, 6), " k:", k1.round(6))
Original  theta: 1.1  k: [0.267261 0.534522 0.801784]
Recovered theta: 1.1  k: [0.267261 0.534522 0.801784]

28.3.3 Axis-Angle to Quaternion

\[ q = \begin{pmatrix}w \\ x \\ y \\ z\end{pmatrix} = \begin{pmatrix}\cos(\theta/2) \\ k_x\sin(\theta/2) \\ k_y\sin(\theta/2) \\ k_z\sin(\theta/2)\end{pmatrix} \]

This is the most direct and numerically stable conversion between these two representations.

import numpy as np

def axisangle_to_quat(k, theta):
    """Unit axis k, angle theta (radians) -> unit quaternion (w, x, y, z)."""
    k = k / np.linalg.norm(k)          # ensure unit; shape (3,)
    half = theta / 2.0
    w = np.cos(half)
    xyz = np.sin(half) * k             # shape (3,)
    return np.concatenate([[w], xyz])  # shape (4,)

k = np.array([0.0, 0.0, 1.0])    # shape (3,)
theta = np.pi / 2
q = axisangle_to_quat(k, theta)    # shape (4,)
print("q (w,x,y,z):", q.round(6))
print("||q||       :", np.linalg.norm(q).round(10))
q (w,x,y,z): [0.707107 0.       0.       0.707107]
||q||       : 1.0

28.3.4 Quaternion to Axis-Angle

\[ \theta = 2\arccos(w), \qquad k = \frac{(x, y, z)}{\sin(\theta/2)} \]

Ensure \(w \ge 0\) before extracting (negate \(q\) if needed — \(q\) and \(-q\) represent the same rotation).

import numpy as np

def quat_to_axisangle(q):
    """Unit quaternion (w, x, y, z) -> (unit axis k, angle theta in radians)."""
    q = q / np.linalg.norm(q)     # normalize; shape (4,)
    if q[0] < 0:
        q = -q                     # canonical: w >= 0, theta in [0, pi]
    w = np.clip(q[0], -1.0, 1.0)
    theta = 2.0 * np.arccos(w)
    s = np.sin(theta / 2.0)
    if s < 1e-8:
        return np.array([1.0, 0.0, 0.0]), 0.0
    k = q[1:] / s                  # shape (3,)
    return k, theta

# Round-trip
k0 = np.array([0.0, 1.0, 0.0])    # shape (3,)
theta0 = np.radians(60.0)
q = np.array([np.cos(theta0/2), *(np.sin(theta0/2)*k0)])   # shape (4,)
k1, theta1 = quat_to_axisangle(q)
print("Original  theta:", np.degrees(theta0).round(4), " k:", k0)
print("Recovered theta:", np.degrees(theta1).round(4),  " k:", k1.round(6))
Original  theta: 60.0  k: [0. 1. 0.]
Recovered theta: 60.0  k: [0. 1. 0.]

28.3.5 Axis-Angle to Euler ZYX

No single closed-form formula spans all three angles; route via the rotation matrix.

import numpy as np

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

def axisangle_to_matrix(k, theta):
    k = k/np.linalg.norm(k); K = skew(k)
    return np.eye(3) + np.sin(theta)*K + (1-np.cos(theta))*(K@K)

def matrix_to_euler_zyx(R):
    pitch = np.arcsin(np.clip(-R[2,0], -1.0, 1.0))
    if np.abs(np.cos(pitch)) > 1e-6:
        yaw  = np.arctan2(R[1,0], R[0,0])
        roll = np.arctan2(R[2,1], R[2,2])
    else:
        yaw = np.arctan2(-R[0,1], R[1,1]); roll = 0.0
    return yaw, pitch, roll

def axisangle_to_euler_zyx(k, theta):
    """Axis-angle -> ZYX Euler angles (radians) via rotation matrix."""
    return matrix_to_euler_zyx(axisangle_to_matrix(k, theta))

k = np.array([1.0, 1.0, 1.0]); k /= np.linalg.norm(k)    # shape (3,)
theta = np.radians(45.0)
yaw, pitch, roll = axisangle_to_euler_zyx(k, theta)
print("yaw={:.4f} pitch={:.4f} roll={:.4f} (deg)".format(
      np.degrees(yaw), np.degrees(pitch), np.degrees(roll)))
yaw=32.1545 pitch=18.0964 roll=32.1545 (deg)

28.3.6 Euler ZYX to Axis-Angle

Likewise routes via the rotation matrix.

import numpy as np

def euler_zyx_to_matrix(yaw, pitch, roll):
    cp,sp = np.cos(yaw),np.sin(yaw)
    ct,st = np.cos(pitch),np.sin(pitch)
    cf,sf = np.cos(roll),np.sin(roll)
    return np.array([
        [cp*ct, cp*st*sf-sp*cf, cp*st*cf+sp*sf],
        [sp*ct, sp*st*sf+cp*cf, sp*st*cf-cp*sf],
        [-st,   ct*sf,          ct*cf          ],
    ])    # shape (3, 3)

def matrix_to_axisangle(R):
    cos_theta = np.clip((np.trace(R)-1)/2, -1.0, 1.0)
    theta = np.arccos(cos_theta)
    if theta < 1e-8:
        return np.array([1.0,0.0,0.0]), 0.0
    if np.pi - theta < 1e-8:
        M = R + np.eye(3); col_norms = np.linalg.norm(M, axis=0)
        i = np.argmax(col_norms); return M[:,i]/col_norms[i], theta
    k = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]])/(2*np.sin(theta))
    return k, theta

def euler_zyx_to_axisangle(yaw, pitch, roll):
    """ZYX Euler angles (radians) -> (unit axis k, angle theta) via rotation matrix."""
    return matrix_to_axisangle(euler_zyx_to_matrix(yaw, pitch, roll))

yaw, pitch, roll = np.radians([30.0, 20.0, 10.0])
k, theta = euler_zyx_to_axisangle(yaw, pitch, roll)
print("k:", k.round(4))
print("theta:", np.degrees(theta).round(4), "deg")
k: [0.124  0.6156 0.7782]
theta: 35.8171 deg

28.4 Quaternion Conversions

A unit quaternion \(q = (w, x, y, z)\) with \(\|q\| = 1\) represents the same rotation as \(-q\). The canonical form used here has \(w \ge 0\) (equivalently, \(\theta \in [0, \pi]\)).


28.4.1 Quaternion to Rotation Matrix

\[ R(q) = \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} \]

This formula is \(O(1)\) and numerically stable for any unit quaternion.

import numpy as np

def quat_to_matrix(q):
    """Unit quaternion (w, x, y, z) -> 3x3 rotation matrix."""
    q = q / np.linalg.norm(q)    # normalize; shape (4,)
    w, x, y, z = 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) ],
    ])    # shape (3, 3)

# 90-degree rotation about z-axis: q = [cos(45), 0, 0, sin(45)]
q = np.array([np.cos(np.pi/4), 0.0, 0.0, np.sin(np.pi/4)])    # shape (4,)
R = quat_to_matrix(q)    # shape (3, 3)
print("R:\n", R.round(4))
print("det(R) =", np.linalg.det(R).round(6))          # +1
print("||R^T R - I|| =", np.linalg.norm(R.T@R - np.eye(3)).round(10))
R:
 [[-0. -1.  0.]
 [ 1. -0.  0.]
 [ 0.  0.  1.]]
det(R) = 1.0
||R^T R - I|| = 0.0

28.4.2 Rotation Matrix to Quaternion (Shepperd’s Method)

Naive inversion of the formula above can divide by a small number. Shepperd’s method picks the largest component to extract first, avoiding near-zero denominators.

\[ w = \tfrac{1}{2}\sqrt{1 + R_{00} + R_{11} + R_{22}}, \quad x = \frac{R_{21} - R_{12}}{4w}, \quad y = \frac{R_{02} - R_{20}}{4w}, \quad z = \frac{R_{10} - R_{01}}{4w} \]

(formula valid when \(w\) is the largest component; the full method branches on which diagonal element is largest).

import numpy as np

def matrix_to_quat(R):
    """3x3 rotation matrix -> unit quaternion (w, x, y, z), Shepperd's method."""
    trace = R[0,0] + R[1,1] + R[2,2]
    if trace > 0:
        s = 0.5 / np.sqrt(trace + 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])    # shape (4,)
    return q / np.linalg.norm(q)

# Round-trip through a random rotation
rng = np.random.default_rng(7)
v = rng.standard_normal((3, 3))
R0, _ = np.linalg.qr(v)                        # shape (3, 3)  random orthogonal
if np.linalg.det(R0) < 0:
    R0[:, 0] *= -1                              # ensure det = +1

q  = matrix_to_quat(R0)                        # shape (4,)
# Reconstruct R from q
w, x, y, z = q
R1 = 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)

print("||R0 - R1|| =", np.linalg.norm(R0 - R1).round(10))
print("q:", q.round(6))
print("||q|| =", np.linalg.norm(q).round(10))
||R0 - R1|| = 0.0
q: [ 0.537257 -0.458984 -0.422322  0.567744]
||q|| = 1.0

28.4.3 Summary: All Quaternion Paths

From To Method
Quaternion Rotation matrix Direct formula (this section)
Quaternion Euler ZYX Via rotation matrix (Section 28.2.4)
Quaternion Axis-angle \(\theta = 2\arccos w\), \(k = (x,y,z)/\sin(\theta/2)\) (Section 28.3.4)
Rotation matrix Quaternion Shepperd’s method (this section)
Euler ZYX Quaternion Half-angle products (Section 28.2.3)
Axis-angle Quaternion \(q = (\cos\tfrac{\theta}{2},\, k\sin\tfrac{\theta}{2})\) (Section 28.3.3)

28.5 Numerical Edge Cases

Every conversion formula has at least one degenerate configuration where the naive formula divides by zero or loses precision. This section catalogs each case and the correct fix.


28.5.1 Gimbal Lock: Pitch Near \(\pm\pi/2\)

When \(\theta = \pm\pi/2\) the \(R_{20}\) element equals \(\mp 1\) and \(\cos\theta = 0\), so the yaw and roll denominators \(R_{00}\) and \(R_{22}\) both vanish. Only the sum \(\psi + \phi\) (or difference) is determined.

Fallback (sets roll to zero, puts all freedom into yaw):

\[ \theta = +\pi/2: \quad \psi = \operatorname{atan2}(-R_{01},\, R_{11}), \quad \phi = 0 \]

\[ \theta = -\pi/2: \quad \psi = \operatorname{atan2}(R_{01},\, R_{11}), \quad \phi = 0 \]

import numpy as np

def matrix_to_euler_zyx(R):
    """R -> (yaw, pitch, roll) in radians; handles gimbal lock."""
    pitch = np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))
    cos_pitch = np.cos(pitch)
    if np.abs(cos_pitch) > 1e-6:
        yaw  = np.arctan2(R[1, 0], R[0, 0])
        roll = np.arctan2(R[2, 1], R[2, 2])
    else:
        # Gimbal lock: roll is undetermined, set to 0
        yaw  = np.arctan2(-R[0, 1], R[1, 1])
        roll = 0.0
    return yaw, pitch, roll

# Pitch exactly at +90 deg
pitch_lock = np.pi / 2
R_lock = np.array([
    [0., -1., 0.],
    [0.,  0., -1.],
    [1.,  0.,  0.],
])    # shape (3, 3)  -- Ry(90) * Rz(0) * Rx(0) collapses yaw+roll

yaw_r, pitch_r, roll_r = matrix_to_euler_zyx(R_lock)
print(f"Pitch at 90 deg -> yaw={np.degrees(yaw_r):.2f} pitch={np.degrees(pitch_r):.2f} roll={np.degrees(roll_r):.2f}")
print("det(R_lock) =", np.linalg.det(R_lock).round(6))
Pitch at 90 deg -> yaw=90.00 pitch=-90.00 roll=0.00
det(R_lock) = 1.0

Key insight: Gimbal lock is not a defect of the formula but of the Euler representation itself – two degrees of freedom collapse into one when pitch is \(\pm 90^\circ\). Quaternions and axis-angle do not have this singularity.


28.5.2 Axis-Angle: \(\theta \approx 0\) (Near Identity)

As \(\theta \to 0\), \(\sin\theta \to 0\) and the axis extraction formula \(k = \text{skew part} / (2\sin\theta)\) becomes \(0/0\).

Fix: For \(\theta < \varepsilon\) (e.g., \(10^{-8}\) rad) return a default axis (e.g., \([1, 0, 0]\)) and \(\theta = 0\).

For the rotation-vector form \(\omega = \theta k\), extracting axis and angle from a small vector is better done via:

\[ \|\omega\| < \varepsilon \Rightarrow R \approx I + [\omega]_\times \]

import numpy as np

def rotvec_to_matrix(omega):
    """Rotation vector omega=theta*k -> rotation matrix (handles small angle)."""
    theta = np.linalg.norm(omega)    # scalar
    if theta < 1e-8:
        # First-order approximation avoids 0/0
        kx, ky, kz = omega
        return np.array([[1,   -kz,  ky],
                         [kz,   1,  -kx],
                         [-ky,  kx,   1]])    # shape (3, 3)
    k = omega / theta                  # shape (3,)
    kx, ky, kz = k
    K = np.array([[0,-kz,ky],[kz,0,-kx],[-ky,kx,0]])   # shape (3, 3)
    return np.eye(3) + np.sin(theta)*K + (1-np.cos(theta))*(K@K)

# Very small rotation
omega_small = np.array([1e-10, 2e-10, 3e-10])    # shape (3,)
R_small = rotvec_to_matrix(omega_small)            # shape (3, 3)
print("||R_small - I|| =", np.linalg.norm(R_small - np.eye(3)))

# Normal rotation
omega_normal = np.array([0.3, 0.4, 0.5])    # shape (3,)
R_normal = rotvec_to_matrix(omega_normal)   # shape (3, 3)
print("det(R_normal) =", np.linalg.det(R_normal).round(6))
||R_small - I|| = 5.291502622129181e-10
det(R_normal) = 1.0

28.5.3 Axis-Angle: \(\theta \approx \pi\) (Near 180 Degrees)

At \(\theta = \pi\) the formula \(k = (R - R^T) / (2\sin\theta)\) again fails because \(\sin\pi = 0\). Instead, use the symmetric part \(R + I\):

\[ R + I = 2\bigl(I + K^2\bigr) = 2\bigl(I + kk^T - I\bigr) + 2I = 2kk^T + 2(1-1)I \]

which gives \(R + I = 2kk^T\), so any column (scaled to unit length) is the axis.

import numpy as np

def matrix_to_axisangle(R):
    """R -> (k, theta) with correct handling at theta near 0 and pi."""
    cos_theta = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0)
    theta = np.arccos(cos_theta)
    if theta < 1e-8:
        return np.array([1.0, 0.0, 0.0]), 0.0
    if np.pi - theta < 1e-8:
        # Near 180 deg: recover k from R + I = 2 k k^T
        M = R + np.eye(3)                        # shape (3, 3)
        col_norms = np.linalg.norm(M, axis=0)    # shape (3,)
        i = np.argmax(col_norms)                 # pick largest-norm column
        k = M[:, i] / col_norms[i]               # shape (3,)
        return k, theta
    k = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]])
    k /= 2.0 * np.sin(theta)    # shape (3,)
    return k, theta

# 180-degree rotation about the [1,1,0]/sqrt(2) axis
k0 = np.array([1.0, 1.0, 0.0]) / np.sqrt(2)    # shape (3,)
theta0 = np.pi
kx, ky, kz = k0
K = np.array([[0,-kz,ky],[kz,0,-kx],[-ky,kx,0]])    # shape (3, 3)
R_pi = np.eye(3) + np.sin(theta0)*K + (1-np.cos(theta0))*(K@K)    # shape (3, 3)

k1, theta1 = matrix_to_axisangle(R_pi)
print("Original  theta:", np.degrees(theta0).round(4), " k:", k0.round(4))
print("Recovered theta:", np.degrees(theta1).round(4), " k:", k1.round(4))
Original  theta: 180.0  k: [0.7071 0.7071 0.    ]
Recovered theta: 180.0  k: [0. 0. 0.]

28.5.4 Antipodal Quaternions

\(q\) and \(-q\) encode the same rotation. This matters when:

  • Interpolating: SLERP between \(q_1\) and \(q_2\) should use the shorter arc. If \(q_1 \cdot q_2 < 0\), negate one quaternion before interpolating.
  • Averaging: the mean of \(q\) and \(-q\) is the zero vector, not a rotation.
  • Comparing: \(\|q_1 - q_2\|\) is not a rotation distance; use \(\min(\|q_1 - q_2\|, \|q_1 + q_2\|)\) or \(2\arccos|q_1 \cdot q_2|\).
import numpy as np

def slerp(q1, q2, t):
    """Spherical linear interpolation; handles antipodal pairs."""
    q1 = q1 / np.linalg.norm(q1)    # shape (4,)
    q2 = q2 / np.linalg.norm(q2)    # shape (4,)
    dot = np.dot(q1, q2)
    if dot < 0:
        q2 = -q2       # use the shorter arc
        dot = -dot
    dot = np.clip(dot, -1.0, 1.0)
    omega = np.arccos(dot)           # angle between quaternions
    if omega < 1e-8:
        return (1-t)*q1 + t*q2      # linear fallback for nearly equal quats
    return (np.sin((1-t)*omega)*q1 + np.sin(t*omega)*q2) / np.sin(omega)

# Identity and 180-deg rotation about z; these are antipodal-adjacent
q1 = np.array([1.0, 0.0, 0.0, 0.0])                # shape (4,)  identity
q2 = np.array([0.0, 0.0, 0.0, 1.0])                # shape (4,)  180 deg z

# Without antipodal fix:
dot_raw = np.dot(q1, q2)
print(f"dot(q1, q2) = {dot_raw:.4f}  (positive -> short arc already)")

q_mid = slerp(q1, q2, 0.5)                         # shape (4,)
print("q_mid:", q_mid.round(4))
print("||q_mid|| =", np.linalg.norm(q_mid).round(6))

# Rotation distance between two quaternions
def quat_distance_rad(q1, q2):
    """Angular distance in radians between two unit quaternions."""
    d = np.abs(np.dot(q1 / np.linalg.norm(q1), q2 / np.linalg.norm(q2)))
    return 2.0 * np.arccos(np.clip(d, 0.0, 1.0))

print(f"Rotation distance (deg): {np.degrees(quat_distance_rad(q1, q2)):.2f}")
dot(q1, q2) = 0.0000  (positive -> short arc already)
q_mid: [0.7071 0.     0.     0.7071]
||q_mid|| = 1.0
Rotation distance (deg): 180.00

28.5.5 Numerical Checklist

Check Code Expected
\(\det R = +1\) np.linalg.det(R) 1.0 (up to float eps)
\(R^T R = I\) np.linalg.norm(R.T @ R - np.eye(3)) < 1e-10
\(\|q\| = 1\) np.linalg.norm(q) 1.0
Canonical sign q[0] >= 0 True
\(\|k\| = 1\) np.linalg.norm(k) 1.0
\(\theta \in [0, \pi]\) 0 <= theta <= np.pi True

After long chains of rotations (compose-then-extract), re-normalize \(R\) via QR (Q, _ = np.linalg.qr(R); R = Q) or re-normalize \(q\) via q /= np.linalg.norm(q).

28.6 Using scipy.spatial.transform.Rotation

scipy.spatial.transform.Rotation provides a unified object that can be constructed from any representation and converted to any other. It handles all 12 paths internally and is the recommended interface when you need a quick, validated conversion in production code.

Convention warning: scipy uses scalar-last quaternions [x, y, z, w], the opposite of the scalar-first convention (w, x, y, z) used throughout this appendix. Always reorder when crossing the boundary.


28.6.1 The Rotation Object

Constructor Input Notes
Rotation.from_matrix(R) (3, 3) or (N, 3, 3) Normalizes to SO(3)
Rotation.from_euler(seq, angles) string + scalars or array seq='ZYX', degrees=False
Rotation.from_rotvec(omega) (3,) or (N, 3) omega = theta * k
Rotation.from_quat(q) [x, y, z, w] scalar-last Normalizes automatically
Converter Output Notes
.as_matrix() (3, 3) or (N, 3, 3) Orthogonal, det +1
.as_euler(seq) (3,) or (N, 3) seq='ZYX', radians
.as_rotvec() (3,) or (N, 3) theta * k, theta in [0, pi]
.as_quat() [x, y, z, w] scalar-last Unit quaternion

28.6.2 From/To Rotation Matrix

import numpy as np
from scipy.spatial.transform import Rotation

# Build a rotation matrix (90 deg about z)
theta = np.pi / 2
R = np.array([[ 0., -1.,  0.],
              [ 1.,  0.,  0.],
              [ 0.,  0.,  1.]])    # shape (3, 3)

rot = Rotation.from_matrix(R)

# Round-trip
R2 = rot.as_matrix()    # shape (3, 3)
print("||R - R2|| =", np.linalg.norm(R - R2).round(10))

# Euler angles (ZYX, radians)
yaw, pitch, roll = rot.as_euler('ZYX')
print(f"yaw={np.degrees(yaw):.2f} pitch={np.degrees(pitch):.2f} roll={np.degrees(roll):.2f} deg")

# Rotation vector
omega = rot.as_rotvec()    # shape (3,)
print("rotvec (deg):", np.degrees(np.linalg.norm(omega)).round(4), "about", (omega/np.linalg.norm(omega)).round(4))

# Quaternion (scalar-last: [x, y, z, w])
q_sl = rot.as_quat()    # shape (4,)  [x, y, z, w]
print("scipy quat [x,y,z,w]:", q_sl.round(6))

# Convert to scalar-first (w, x, y, z) used in this appendix
q_sf = np.array([q_sl[3], q_sl[0], q_sl[1], q_sl[2]])    # shape (4,)
print("book quat  (w,x,y,z):", q_sf.round(6))
||R - R2|| = 0.0
yaw=90.00 pitch=0.00 roll=0.00 deg
rotvec (deg): 90.0 about [0. 0. 1.]
scipy quat [x,y,z,w]: [0.       0.       0.707107 0.707107]
book quat  (w,x,y,z): [0.707107 0.       0.       0.707107]

28.6.3 From/To Euler Angles

import numpy as np
from scipy.spatial.transform import Rotation

yaw, pitch, roll = np.radians([45.0, 30.0, 15.0])

# scipy expects angles in the order matching seq read right-to-left applied
# For ZYX intrinsic: pass (yaw, pitch, roll) with seq='ZYX'
rot = Rotation.from_euler('ZYX', [yaw, pitch, roll])

# Recover
angles_out = rot.as_euler('ZYX')    # shape (3,)  [yaw, pitch, roll]
print("Input  (deg):", np.degrees([yaw, pitch, roll]).round(4))
print("Output (deg):", np.degrees(angles_out).round(4))

# Cross-check with rotation matrix
R = rot.as_matrix()    # shape (3, 3)
print("det(R) =", np.linalg.det(R).round(8))
print("||R^T R - I|| =", np.linalg.norm(R.T @ R - np.eye(3)).round(10))
Input  (deg): [45. 30. 15.]
Output (deg): [45. 30. 15.]
det(R) = 1.0
||R^T R - I|| = 0.0

28.6.4 From/To Axis-Angle (Rotation Vector)

scipy’s from_rotvec / as_rotvec uses the rotation-vector form \(\omega = \theta k\), identical to the \(\omega\) notation in Chapters 21 and 24.

import numpy as np
from scipy.spatial.transform import Rotation

# Axis-angle -> rotation vector
k = np.array([1.0, 1.0, 0.0]) / np.sqrt(2)    # shape (3,)  unit axis
theta = np.radians(120.0)
omega = theta * k    # shape (3,)  rotation vector

rot = Rotation.from_rotvec(omega)

# Recover axis and angle
omega2 = rot.as_rotvec()    # shape (3,)
theta2 = np.linalg.norm(omega2)
k2     = omega2 / theta2    # shape (3,)

print("Original  theta:", np.degrees(theta).round(4), " k:", k.round(4))
print("Recovered theta:", np.degrees(theta2).round(4), " k:", k2.round(4))
print("||omega - omega2|| =", np.linalg.norm(omega - omega2).round(10))
Original  theta: 120.0  k: [0.7071 0.7071 0.    ]
Recovered theta: 120.0  k: [0.7071 0.7071 0.    ]
||omega - omega2|| = 0.0

28.6.5 Quaternion Convention Bridge

The only friction point with scipy is the scalar-last order.

import numpy as np
from scipy.spatial.transform import Rotation

# Scalar-first (w, x, y, z) used in this appendix
q_sf = np.array([np.cos(np.pi/4), 0.0, 0.0, np.sin(np.pi/4)])    # shape (4,)  90 deg about z

# Convert to scalar-last [x, y, z, w] for scipy
q_sl = np.array([q_sf[1], q_sf[2], q_sf[3], q_sf[0]])    # shape (4,)

rot = Rotation.from_quat(q_sl)

# Round-trip back to scalar-first
q_sl2 = rot.as_quat()    # shape (4,)  [x, y, z, w]
if q_sl2[3] < 0:
    q_sl2 = -q_sl2       # canonical: w >= 0
q_sf2 = np.array([q_sl2[3], q_sl2[0], q_sl2[1], q_sl2[2]])    # shape (4,)

print("Original  (w,x,y,z):", q_sf.round(6))
print("Recovered (w,x,y,z):", q_sf2.round(6))
print("||q_sf - q_sf2|| =", np.linalg.norm(q_sf - q_sf2).round(10))
Original  (w,x,y,z): [0.707107 0.       0.       0.707107]
Recovered (w,x,y,z): [0.707107 0.       0.       0.707107]
||q_sf - q_sf2|| = 0.0

28.6.6 SLERP via Slerp

scipy.spatial.transform.Slerp interpolates a sequence of rotations at arbitrary parameter values – handling antipodal pairs and normalization automatically.

import numpy as np
from scipy.spatial.transform import Rotation, Slerp

# Two key-frame rotations
R0 = Rotation.from_euler('ZYX', [0.0,  0.0,  0.0])           # identity
R1 = Rotation.from_euler('ZYX', [np.radians(90.0), 0.0, 0.0])  # 90 deg yaw

times_key = [0.0, 1.0]
rotations = Rotation.concatenate([R0, R1])

slerp = Slerp(times_key, rotations)

# Sample at 5 evenly-spaced times
t_query = np.linspace(0, 1, 5)
r_interp = slerp(t_query)

yaw_deg = np.degrees(r_interp.as_euler('ZYX')[:, 0])    # shape (5,)
print("Interpolated yaw (deg):", yaw_deg.round(2))
print("Expected:             ", np.linspace(0, 90, 5).round(2))
Interpolated yaw (deg): [ 0.  22.5 45.  67.5 90. ]
Expected:              [ 0.  22.5 45.  67.5 90. ]

28.6.7 Full Conversion API Summary

Task scipy call
Matrix -> Euler ZYX Rotation.from_matrix(R).as_euler('ZYX')
Euler ZYX -> matrix Rotation.from_euler('ZYX', [yaw,pitch,roll]).as_matrix()
Euler ZYX -> quat Rotation.from_euler('ZYX', angles).as_quat() – scalar-last
Quat -> Euler ZYX Rotation.from_quat([x,y,z,w]).as_euler('ZYX')
Quat -> matrix Rotation.from_quat([x,y,z,w]).as_matrix()
Axis-angle -> matrix Rotation.from_rotvec(theta*k).as_matrix()
Matrix -> axis-angle rot.as_rotvec() gives theta*k; split with np.linalg.norm
Interpolate Slerp(times, rotations)(t_query)

Reorder helper (scalar-first <-> scalar-last):

import numpy as np

def sf_to_sl(q):
    """Scalar-first (w,x,y,z) -> scalar-last [x,y,z,w] for scipy."""
    return np.array([q[1], q[2], q[3], q[0]])    # shape (4,)

def sl_to_sf(q):
    """Scalar-last [x,y,z,w] from scipy -> scalar-first (w,x,y,z)."""
    return np.array([q[3], q[0], q[1], q[2]])    # shape (4,)

q_sf = np.array([0.9239, 0.0, 0.0, 0.3827])    # shape (4,)  ~45 deg about z
q_sl = sf_to_sl(q_sf)                           # shape (4,)
q_back = sl_to_sf(q_sl)                         # shape (4,)
print("Round-trip match:", np.allclose(q_sf, q_back))
Round-trip match: True