Eigen · Orientation Difference and SO(3) Matrices

May 19, 2024 | Tech Math

Preface

Exponential and Logarithm in SO(3), SE(3), Quaternion Space discussed the theory and physics behind orientation difference. Spatial Kinematics and Transformations mentioned the concept of skew symmetric matrix that is closely related to rotation matrices. This post presents their implementation in C++ for future reference.

Notations

With reference to 2020@Huber , the following notations are employed; given an angular velocity vector $$ \omega = (\omega_x, \omega_y, \omega_z) \in \mathbb{R}^3 $$ and a pure quaternion representing the angular velocity $$ q = (0, \omega) $$ one has $$ [\omega]_\times \triangleq \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \triangleq [\omega] $$ $$ \Big[ [\omega]_\times \Big]_\vee \triangleq \omega $$ where $[\cdot]_\times: \mathbb{R}^3 \to so(3)$ and $[\cdot]_\vee: so(3) \to \mathbb{R}^3$. $$ [\omega]_{\mathcal{Q}} \triangleq [0, \omega] $$ $$ \Big[[0, \omega]\Big]_{\mathcal{Q}^{-1}} \triangleq \omega $$ where $[\cdot]_{\mathcal{Q}}: \mathbb{R}^3 \to \mathbb{H}$ and $[\cdot]_{\mathcal{Q}^{-1}}: \mathbb{H} \to \mathbb{R}^3$. $$ \text{log}(R) \triangleq [\hat{\omega}] \theta $$ where $R = (\hat{\omega}, \theta)$ is the angle-axis representation of a rotation matrix, and $\hat{\omega} = \frac{\omega}{||\omega||}$ is its rotation axis of unit length.

Orientation Difference and Rotation Sequences

The code below realizes $$ \omega_{s} = \Big[ \text{log}(R_d R^T) \Big]_{\vee} = 2\Big[ \text{log}(q_d q^*) \Big]_{\mathcal{Q}^{-1}} $$ and $$ \omega_{b} = \Big[ \text{log}(R^T R_d ) \Big]_{\vee} = 2\Big[ \text{log}(q^* q_d ) \Big]_{\mathcal{Q}^{-1}} $$

Note that one can also rotate a given angular velocity to get its representation in another frame, instead of computing using rotation or quaternion logarithm, for example $$ \omega_{s} = R \omega_b = R \Big[ \text{log}(R^T R_d ) \Big]_{\vee} = \Big[ \text{log}(R_d R^T) \Big]_{\vee} $$ which is demonstrated in the implementation below as well.

Notice also that in 2017@Lynch , Appendix B.1.1 Algorithm for Computing the ZYX Euler Angles, and B.2 Roll-Pitch-Yaw Angles, it stated that a sequence of body-fixed ZYX Euler angles is equivalent to a sequence of space-fixed roll-pitch-yaw angles, that is, $$ R_{\small{\text{Euler-ZYX}}} = \underbrace{R_z(\theta_z)}_{1} \underbrace{R_y(\theta_y)}_{2} \underbrace{R_x(\theta_x)}_{3} = \underbrace{R_z(\text{yaw})}_{3} \underbrace{R_y(\text{pitch})}_{2} \underbrace{R_x(\text{roll})}_{1} = R_{\text{roll-pitch-yaw}} $$ which is also demonstrated in the implementation below. One can verify the rotation results using 3D Rotation Convert.

Skew Symmetric Matrices

Below code snippet implements getting the corresponding skew symmetric matrix from an $\mathbb{R}^3$ vector, i.e., obtaining $$ [a]_\times \triangleq \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \in so(3) \in \mathbb{R}^{3\times3} $$ from $a = \begin{bmatrix} a_1 & a_2 & a_3 \end{bmatrix}^T \in \mathbb{R}^3$; and its inverse.