Rotation in 3D
[euler rotation rodrigue quanterion matrix 3d This is my note on rotation in 3D space. There are many different ways of representating the rotation in 3D space, e.g., 3x3 rotation matrix, Euler angle (pitch, yaw and roll), Rodrigues axis-angle representation and quanterion. The relationship and conversion between those representation will be described as below. You could also use scipy.spatial.transform.Rotation to convert between methods.
Let us assumpe a coordinate in 3 dimension space could be written as \(v=\begin{bmatrix} x \\ y \\ z \\ \end{bmatrix}\in\mathbb{R}^3\).
Rotation Matrix
The point v could be rotated to other point v’ via a rotation matrix R:
\[v' = R v = \begin{bmatrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \\ \end{bmatrix}v\]R will be an orthornomal matrix, i.e., \(RR^T=I\). Thus R actually only has three degree of freedom. The matrix R is a member of the three-dimensional special orthogonal group, SO(3).
from scipy.spatial.transform import Rotation
import numpy as np
# assume v is 3x1 vector and R is 3x3 matrix
r = Rotation.from_matrix(R)
v2 = R @ v
print(v, v2)
Euler Angle
The Euler angles are three angles (\(\alpha\),\(\beta\),\(\gamma\)) introduced by Leonhard Euler to describe the orientation of a rigid body with respect to a fixed coordinate system. It is a more compact than rotation matrix. However, besides the three angles itself, Euler angle is sensitive to the order of the rotation as well, e.g., rotating in the order of \(\alpha\),\(\beta\),\(\gamma\) could have totally different result over \(\beta\),\(\alpha\),\(\gamma\). As a result, when using Euler angle, we always need to state the rotation order besides the three angles.
Euler angles can be defined by elemental geometry or by composition of rotations. The geometrical definition demonstrates that three composed elemental rotations (rotations about the axes of a coordinate system) are always sufficient to reach any target frame.
The three elemental rotations may be extrinsic (rotations about the axes xyz of the original coordinate system, which is assumed to remain motionless), or intrinsic (rotations about the axes of the rotating coordinate system XYZ, solidary with the moving body, which changes its orientation after each elemental rotation). Note xyz = ZYX.
Euler angles are typically denoted as α, β, γ, or ψ, θ, φ. Different authors may use different sets of rotation axes to define Euler angles, or different names for the same angles. Therefore, any discussion employing Euler angles should always be preceded by their definition. For details, please refer to Euler angles.
Any orientation can be achieved by composing three elemental rotations, starting from a known standard orientation. Equivalently, any rotation matrix R can be decomposed as a product of three elemental rotation matrices. For instance:
\[\begin{align*} R&=X(\alpha)Y(\beta)Z(\beta) \\ &= \begin{bmatrix}1&0&0\\0&cos(\alpha)&-\sin(\alpha))\\0&\sin(\alpha)&\cos(\alpha)\end{bmatrix}\begin{bmatrix}\cos(\beta)&0&\sin(\beta)\\0&1&0\\-\sin(\beta)&0&\cos(\beta)\end{bmatrix}\begin{bmatrix}\cos(\gamma)&-\sin(\gamma)&0\\\sin(\gamma)&\cos(\gamma)&0\\0&0&1\end{bmatrix}\\ &= \begin{bmatrix} \cos(\beta)\cos(\gamma) & -\cos(\beta)\sin(\gamma) & \sin(\beta) \\ \cos(\alpha)\sin(\gamma)+\cos(\gamma)\sin(\alpha)\sin(\beta) & \cos(\alpha)\cos(\gamma)-\sin(\alpha)\sin(\beta)\sin(\gamma) & -\cos(\beta)\sin(\alpha) \\ \sin(\alpha)\sin(\gamma)-\cos(\alpha)\cos(\gamma)\sin(\beta) & \cos(\gamma)\sin(\alpha)+\cos(\alpha)\sin(\beta)\sin(\gamma) & \cos(\alpha)\cos(\beta) \\ \end{bmatrix} \end{align*}\]is a rotation matrix that may be used to represent a composition of extrinsic rotations about axes z, y, x, (in that order), or a composition of intrinsic rotations about axes x-y′-z″ (in that order). However, both the definition of the elemental rotation matrices X, Y, Z, and their multiplication order depend on the choices taken by the user about the definition of both rotation matrices and Euler angles.
Thus we could also convert from rotation matrix to Euler angle as:
\[\begin{align*} \alpha &= \arctan(-\frac{R_{23}}{R_{33}})\\ \beta&=\arctan(\frac{R_{13}}{\sqrt{1-R_{13}^2}}) \\ \gamma&= \arctan(-\frac{R_{12}}{R_{11}}) \end{align*}\]euler = r.as_euler("xyz", degrees=True)
print(euler)
R2 = R.from_euler("xyz", euler, degrees=True).as_matrix()
# compare it
assert np.allclose(R, R2)
Rodrigues
In the theory of three-dimensional rotation, Rodrigues’ rotation formula, named after Olinde Rodrigues, is an efficient algorithm for rotating a vector in space, given an axis and angle of rotation. By extension, this can be used to transform all three basis vectors to compute a rotation matrix in SO(3), the group of all rotation matrices, from an axis–angle representation. In other words, the Rodrigues’ formula provides an algorithm to compute the exponential map from so(3), the Lie algebra of SO(3), to SO(3) without actually computing the full matrix exponential.
For the 3D location v=(x,y,z), it could be rotated around axis k by angle \(\theta\) as:
\[\begin{align*}v'&=v\cos(\theta)+(k\times v)\sin(\theta)+k(k\cdot v)\sin(1-\cos(\theta))\\&=(I+\sin(\theta)K+(1-\cos(\theta))K^2)v\\\mbox{where k}&=\begin{bmatrix}0&-k_z&k_y\\k_z&0&-k_x\\-k_y&k_x&0\end{bmatrix}\end{align*}\]rodrigues = r.as_mrp()
print(rodrigues)
# get the rotation angle
print(4 * np.arctan(np.norm(rot)))
# get the rotation axis
print(rot / np.norm(rot))
R3 = R.from_mrp(rodrigues).as_matrix()
# compare it
assert np.allclose(R, R3)
in scipy.spatial.transform.Rotation, MRPs are a 3 dimensional vector co-directional to the axis of rotation and whose magnitude is equal to
tan(theta / 4), wherethetais the angle of rotation (in radians)
Rotation Vector
In mathematics, the axis–angle representation of a rotation parameterizes a rotation in a three-dimensional Euclidean space by two quantities: a unit vector e indicating the direction of an axis of rotation, and an angle θ describing the magnitude of the rotation about the axis.
rot = r.as_rotvec(degrees=True)
print(rot)
# get the rotation angle
print(np.norm(rot))
# get the rotation axis
print(rot / np.norm(rot))
R4 = R.from_rotvec(rot, degrees=True).as_matrix()
# compare it
assert np.allclose(R, R4)
Quanterion
Unit quaternions, known as versors, provide a convenient mathematical notation for representing spatial orientations and rotations of elements in three dimensional space. Specifically, they encode information about an axis-angle rotation about an arbitrary axis. Compared to rotation matrices, quaternions are more compact, efficient, and numerically stable. Compared to Euler angles, they are simpler to compose.
The rotation of angle around the axis defined by the unit vector (x,y, z) could be also written as xi+yj+zk, where i, j, k are unit vectors representing the three Cartesian axes. It can be represented by a quaternion. This can be done using an extension of Euler’s formula:
\[e^{\frac{\theta}{2}(xi+yj+zk)}=\cos(\frac{\theta}{2})+(xi+yj+zk)\sin(\frac{\theta}{2})\]With quanterion q, rotation for a point v=(x,y,z) could be written as:
\[v'=qvq^{-1}\]The quanterion q could be converted to a rotation matrix as:
\[\begin{align*}R&=\begin{bmatrix}1-2s(q_j^2+q_k^2)&2s(q_iq_j-q_kq_r)&2s(q_iq_k+q_jq_r)\\2s(q_iq_j+q_kq_r)&1-2s(q_i^2+q_k^2)&2s(q_jq_k-q_iq_r)\\2s(q_iq_k-q_jq_r)&2s(q_jq_k+q_iq_r)&1-2s(q_i^2+q_j^2)\end{bmatrix}\\\mbox{where s}&=\lVert q\rVert^{-2}\end{align*}\]quat = r.as_quat()
print(quat)
# get the rotation angle
print(quat[3:])
# get the rotation axis
print(quat[:3])
R5 = R.from_quat(quat).as_matrix()
# compare it
assert np.allclose(R, R5)