Appendix A — SO(3) - Rotations in 3D
A.1 Quaternions
A.1.1 Introduction
For rotations without singularities, rotation matrices and quaternions are common. The special orthogonal group SO(3) is often defined using a constrained set of \(3\times 3\) rotation matrices: \[ SO(3)=\{ R \in \mathbb R^{3 \times 3} | R^\top R = R R^\top = I \land \det R = 1 \} \tag{A.1}\]
Quaternions have the advantage of a lower memory requirement (4 floats vs. 9 in a rotation matrix), often lower computational effort, and that their constraints are easier to fulfill when integrating (one norm constraint rather than the two constraints above). This document summarizes the mathematical operations for quaternions and provides useful references for derivations.
Notable implementations for quaternions are:
| Language | Link |
|---|---|
| Python | Rowan |
| C++ | Eigen, Sophus |
| C | cmath3d |
Notable other references are: Särkkä (2007), Toussaint (2024), Jia (2024).
A.1.2 Definition
A quaternion for SO(3) is a 4D vector with the constraint to have norm 1:
\[ \mathbf{q}=\begin{pmatrix} q_w\\ q_x\\ q_y\\ q_z \end{pmatrix}= \begin{pmatrix} q_w\\ \vec{\mathbf{q}} \end{pmatrix}\in \mathbb R^4 \,\, s.t. \| \mathbf{q}\|_2 = 1 \tag{A.2}\]
One can augment or promote a vector \(\mathbf{v}\in \mathbb R^3\) to be a quaternion: \[ \bar{\mathbf{v}} = \begin{pmatrix} 0\\ \mathbf{v} \end{pmatrix} \tag{A.3}\]
One can extract the vector or imaginary part of a quaternion: \[ \vec{\mathbf{q}} = \mathop{\mathrm{Im}}(\mathbf{q}) \tag{A.4}\]
Note that the order of the components is not uniquely defined. Some software packages put \(q_w\) first, some last in the vector.
A.1.3 Conversion
A.1.3.1 To Rotation Matrix
\[ R(\mathbf{q}) = \begin{pmatrix} q_w^2 + q_x^2 - q_y^2 - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_w q_y + q_x q_z) \\ 2(q_x q_y + q_w q_z) & q_w^2 - q_x^2 + q_y^2 - q_z^2 & 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) & 2( q_w q_x + q_y q_z) & q_w^2 - q_x^2 - q_y^2 + q_z^2 \end{pmatrix} \tag{A.5}\]
A.1.3.2 From Rotation Matrix
A.1.4 Low-Level Operations
A.1.4.1 Negation
(same as any vector):
\[ -\mathbf{q}= \begin{pmatrix} -q_w\\ -q_x\\ -q_y\\ -q_z \end{pmatrix} \tag{A.6}\]
\(\mathbf{q}\) and \(-\mathbf{q}\) represent the same rotation.
A.1.4.2 Addition, Substraction, Multiplication/Devision with Scalar
same as any vector, e.g.,:
\[ \mathbf{q}+ \mathbf{p}= \begin{pmatrix} q_w + p_w\\ q_x + p_x\\ q_y + p_y\\ q_z + p_z \end{pmatrix} \]
A.1.4.3 Conjugate
\[ \mathbf{q}^* = \begin{pmatrix} q_w\\ -q_x\\ -q_y\\ -q_z \end{pmatrix} \]
This is the inverse for normalized quaternions.
A.1.4.4 Norm
(regular \(L_2\) norm on the vector)
\[ \| \mathbf{q}\| = \sqrt{q_w^2 + q_x^2 + q_y^2 + q_z^2} \]
This should be always 1 for normalized quaternions.
A.1.4.5 Exponential
\[ \exp{(\mathbf{q})} = \exp{(q_w)} \begin{pmatrix} \cos{(\| \vec{\mathbf{q}} \|)}\\ \frac{\vec{\mathbf{q}}}{\| \vec{\mathbf{q}} \|} \sin{(\| \vec{\mathbf{q}} \|)} \end{pmatrix} \]
A.1.4.6 Logarithm
\[ \ln{(\mathbf{q})} = \begin{pmatrix} \ln{(\| \mathbf{q}\|)}\\ \frac{\vec{\mathbf{q}}}{\| \vec{\mathbf{q}} \|} \arccos{\left( \frac{q_w}{\| \mathbf{q}\| }\right)} \end{pmatrix} \]
A.1.4.7 Power with Real Number
\[ \mathbf{q}^p = \exp{(\ln(\mathbf{q}) p)} \]
A.1.4.8 Power with Quaternion
\[ \mathbf{q}^\mathbf{p}= \exp{(\ln(\mathbf{q}) \otimes \mathbf{p})} \]
A.1.5 Useful Operations
A.1.5.1 Multiplication
Similar to rotation matrices, this can be used to concatenate rotations.
\[ \mathbf{q}\otimes \mathbf{p}= \begin{pmatrix} q_w p_w - q_x p_x - q_y p_y - q_z p_z\\ q_x p_w + q_w p_x - q_z p_y + q_y p_z\\ q_y p_w + q_z p_x + q_w p_y - q_x p_z\\ q_z p_w - q_y p_x + q_x p_y + q_w p_z \end{pmatrix} \tag{A.7}\]
Generally \(\mathbf{q}\otimes \mathbf{p}\neq \mathbf{p}\otimes \mathbf{q}\).
Be careful when looking at papers - sometimes it’s defined inverse!
A.1.6 Vector Rotation
With a rotation matrix \(\mathbf R(\mathbf{q})\), we can rotate a vector \(\mathbf v\) as \(\mathbf v_{rotated} = \mathbf R(\mathbf{q}) \mathbf v\). Similarily, for quaternions we have:
\[ \mathbf{v}_{rotated} = \mathbf{q}\odot \mathbf{v}= \mathop{\mathrm{Im}}(\mathbf{q}\otimes \overline{\mathbf{v}} \otimes \mathbf{q}^*) \tag{A.8}\]
(I.e., augment \(\mathbf v\) to a quaternion, use two quaternion multiplications, and one conjugate, and then extract the vector part of the result.)
A.1.7 Derivatives and Integration
Assuming \(\boldsymbol{\omega}\in \mathbb R^3\) is the angular velocity in (e.g., gyroscope): \[ \dot{\mathbf{q}} = \frac{1}{2} \mathbf{q}\otimes \overline{\boldsymbol{\omega}} \]
Assuming \(\boldsymbol{\omega}\) in world frame: \[ \dot{\mathbf{q}} = \frac{1}{2} \overline{\boldsymbol{\omega}} \otimes \mathbf{q} \]
This can also be used to numerically estimate \(\boldsymbol{\omega}\). One can approximate \(\dot{\mathbf{q}} \approx (\mathbf{q}(t+\Delta t) - \mathbf{q}(t)) / \Delta t\) and then proceed (remembering to use \(\mathbf{q}^*\) as inverse of \(\mathbf{q}\)). A nicer solution can be derived to reach the following (body frame)1: \[ \boldsymbol{\omega} \approx 2 \mathop{\mathrm{Im}}\left( \frac{\mathbf{q}^*(t) \otimes \mathbf{q}(t+\Delta t)}{\Delta t} \right) \]
Or similarly in world frame: \[ \boldsymbol{\omega} \approx 2 \mathop{\mathrm{Im}}\left( \frac{ \mathbf{q}(t+\Delta t) \otimes \mathbf{q}^*(t)}{\Delta t} \right) \]
For integration, one can use Euler or RK4 to integrate \(\mathbf{q}\), followed by normalizing the quaternion. Alternatively, the exponential map can be used as a more accurate, explicit integrator (body frame):
\[ \mathbf{q}_{t+\Delta t} = \mathbf{q}_t \otimes \begin{pmatrix} \cos{(\| \boldsymbol{\omega} \| \Delta t/2)}\\ \frac{\boldsymbol{\omega}}{\| \boldsymbol{\omega} \|} \sin{(\|\boldsymbol{\omega}\| \Delta t/2)} \end{pmatrix} \]
A derivation is in Narayan (2017).
Some libraries do not specify which frame they operate in, e.g., the Python library rowan assumes world frame for its methods.
A.1.8 Interpolation
If we want to linearly interpolate from a rotation \(\mathbf{q}(0)\) to another rotation \(\mathbf{q}(1)\), we can use the slerp operation: \[ \mathbf{q}(t) = \left( \mathbf{q}(1) \otimes \mathbf{q}(0)^* \right)^t \otimes \mathbf{q}(0) \,\,\, \forall t \in [0, 1] \]
A.1.9 Metrics
A metric measures the difference between two rotations as a scalar value. A comparison of different metrics is in Huynh (2009). Below are common metrics.
The following is metric 2 in Huynh (2009) and sym_distance in rowan: \[ d(\mathbf{q}, \mathbf{p}) = \min \left( \| \mathbf{q}- \mathbf{p}\|, \| \mathbf{q}+ \mathbf{p}\| \right) \in [0, \sqrt 2] \]
The following is metric 3 in Huynh (2009), is used by OMPL, and called sym_intrinsic_distance in rowan: \[ d(\mathbf{q}, \mathbf{p}) = \arccos{| \mathbf{q}\cdot \mathbf{p}|} \in [0, \pi/2] \]
The following is metric 4 in Huynh (2009): \[ d(\mathbf{q}, \mathbf{p}) = 1 - | \mathbf{q}\cdot \mathbf{p}| \in [0, 1] \]
A.1.10 Random Generation
Sampling rotations uniformly is not trivial. Here is one approach Shoemake (1992): \[ \begin{aligned} r_1, r_2, r_3 &\sim \mathcal U(0, 1)\\ \mathbf{q}&=\begin{pmatrix} \sqrt{1-r_1} \sin 2\pi r_2\\ \sqrt{1-r_1} \cos 2\pi r_2\\ \sqrt{r_1} \sin 2\pi r_3\\ \sqrt{r_1} \cos 2\pi r_3 \end{pmatrix} \end{aligned} \]
Note, this is also used inside OMPL.
A.2 Rotation Matrices
A.2.1 Skew-symmetric Mapping
The hat-map constructs a metrix from a vector: \(\mathbb R^3 \to \mathbb R^{3\times 3}\) \[ \hat{\boldsymbol{\omega}} = \begin{bmatrix} 0 & -\boldsymbol{\omega}_z & \boldsymbol{\omega}_y\\ \boldsymbol{\omega}_z & 0 & -\boldsymbol{\omega}_x\\ -\boldsymbol{\omega}_y & \boldsymbol{\omega}_x & 0 \end{bmatrix} \tag{A.9}\]
The vee-map \(\mathbf{R}^{\vee}\) does the inverse operation.
A.3 Machine Learning
Schuck, Samy, and Schoellig (2025)