Quaternion
| Quaternion type |
Hamilton |
JPL |
| Components order |
\(\begin{bmatrix} q_{w} \\ \mathbf{q}_{v} \end{bmatrix}\) |
\(\begin{bmatrix} \mathbf{q}_{v} \\ q_{w}\end{bmatrix}\) |
| Algebra |
\(ij=k\) |
\(ij=-k\) |
| Handedness |
Right-handed |
Left-handed |
| Function (Passive: rotating frames; Active: rotating vectors) |
Passive |
Passive |
| Right-to-left products mean |
Local-to-Global (\(\mathbf{q}_{\text{GL}}\)) |
Global_to_Local (\(\mathbf{q}_{\text{LG}}\)) |
| Transformation to Rotation |
\(\begin{bmatrix} 1 \\ \boldsymbol{\theta} \end{bmatrix} = \mathbf{R} \approx \mathbf{I} + \frac{1}{2}\boldsymbol{\theta}\) |
\(\begin{bmatrix} \boldsymbol{\theta} \\ 1 \end{bmatrix} = \mathbf{R} \approx \mathbf{I} - \frac{1}{2}\boldsymbol{\theta}\) |
Computation (Hamilton)
\[\mathbf{q} = \begin{bmatrix}
q_{w} \\
q_{x} \\
q_{y} \\
q_{z}
\end{bmatrix} = \begin{bmatrix}
q_{w} \\
\mathbf{q}_{v}
\end{bmatrix}\]
\[\mathbf{q}_{1} \otimes \mathbf{q}_{2} = [\mathbf{q}_{1}]_{L}\mathbf{q}_{2} = [\mathbf{q}_{2}]_{R}\mathbf{q}_{1}\]
\[[\mathbf{q}]_{L} = q_{w} \mathbf{I} + \begin{bmatrix}
0 & -\mathbf{q}_{v}^{\text{T}} \\
\mathbf{q}_{v} & [\mathbf{q}_{v}]_{\times}
\end{bmatrix} = \begin{bmatrix}
q_{w} & -q_{x} & -q_{y} & -q_{z} \\
q_{x} & q_{w} & -q_{z} & q_{y} \\
q_{y} & q_{z} & q_{w} & -q_{x} \\
q_{z} & -q_{y} & q_{x} & q_{w} \\
\end{bmatrix}\]
\[[\mathbf{q}]_{R} = q_{w} \mathbf{I} + \begin{bmatrix}
0 & -\mathbf{q}_{v}^{\text{T}} \\
\mathbf{q}_{v} & -[\mathbf{q}_{v}]_{\times}
\end{bmatrix} = \begin{bmatrix}
q_{w} & -q_{x} & -q_{y} & -q_{z} \\
q_{x} & q_{w} & q_{z} & -q_{y} \\
q_{y} & -q_{z} & q_{w} & q_{x} \\
q_{z} & q_{y} & -q_{x} & q_{w} \\
\end{bmatrix}\]
\[[\mathbf{p}]_{R}[\mathbf{q}]_{L} = [\mathbf{q}]_{L}[\mathbf{p}]_{R}\]
\[\mathbf{q}\mathbf{p}\mathbf{q}^{*} = [\mathbf{q}]_{L}[\mathbf{q}]_{R}^{\text{T}}\mathbf{p} = \begin{bmatrix}
p_{w} \\
\mathbf{R}\mathbf{p}_{v}
\end{bmatrix}\]
Euler Angles (Yaw, Pitch, Roll)
\[\begin{aligned}
\mathbf{q} &=\begin{bmatrix}
\cos (\psi / 2) \\
0 \\
0 \\
\sin (\psi / 2)
\end{bmatrix}\begin{bmatrix}
\cos (\theta / 2) \\
0 \\
\sin (\theta / 2) \\
0
\end{bmatrix}\begin{bmatrix}
\cos (\phi / 2) \\
\sin (\phi / 2) \\
0 \\
0
\end{bmatrix} \\
&=\begin{bmatrix}
\cos (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\
\sin (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)-\cos (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\
\cos (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \cos (\theta / 2) \sin (\psi / 2) \\
\cos (\phi / 2) \cos (\theta / 2) \sin (\psi / 2)-\sin (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)
\end{bmatrix}
\end{aligned}\]
Rotation Matrix
From Euler Angles ZYX
\[\begin{aligned}
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix} &=R_{z}(\psi) R_{y}(\theta) R_{x}(\phi)\begin{bmatrix}
X \\
Y \\
Z
\end{bmatrix} \\
&=\begin{bmatrix}
\cos \psi & -\sin \psi & 0 \\
\sin \psi & \cos \psi & 0 \\
0 & 0 & 1
\end{bmatrix}\begin{bmatrix}
\cos \theta & 0 & \sin \theta \\
0 & 1 & 0 \\
-\sin \theta & 0 & \cos \theta
\end{bmatrix}\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos \phi & -\sin \phi \\
0 & \sin \phi & \cos \phi
\end{bmatrix}\begin{bmatrix}
X \\
Y \\
Z
\end{bmatrix} \\
&=\begin{bmatrix}
\cos \theta \cos \psi & -\cos \phi \sin \psi+\sin \phi \sin \theta \cos \psi & \sin \phi \sin \psi+\cos \phi \sin \theta \cos \psi \\
\cos \theta \sin \psi & \cos \phi \cos \psi+\sin \phi \sin \theta \sin \psi & -\sin \phi \cos \psi+\cos \phi \sin \theta \sin \psi \\
-\sin \theta & \sin \phi \cos \theta & \cos \phi \cos \theta
\end{bmatrix}\begin{bmatrix}
X \\
Y \\
Z
\end{bmatrix}
\end{aligned}\]
\[\psi = \arctan \frac{\mathbf{R}_{10}}{\mathbf{R}_{00}}\]
\[\theta = \arctan \frac{-\mathbf{R}_{20}}{\mathbf{R}_{00} \cos \psi + \mathbf{R}_{10} \sin \psi}\]
\[\phi = \arctan \frac{\mathbf{R}_{02} \sin \psi - \mathbf{R}_{12} \cos \psi}{-\mathbf{R}_{01} \sin \psi + \mathbf{R}_{11} \cos \psi}\]
Eigen::Matrix3d YawPitchRoll2Rotation(const Eigen::Vector3d& ypr) {
const double y = ypr(0) / 180.0 * M_PI;
const double p = ypr(1) / 180.0 * M_PI;
const double r = ypr(2) / 180.0 * M_PI;
Eigen::Matrix3d Rz;
Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1;
Eigen::Matrix3d Ry;
Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p);
Eigen::Matrix3d Rx;
Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r);
return Rz * Ry * Rx;
}
Eigen::Vector3d Rotation2YawPitchRoll(const Eigen::Matrix3d& R) {
const Eigen::Vector3d n = R.col(0);
const Eigen::Vector3d o = R.col(1);
const Eigen::Vector3d a = R.col(2);
Eigen::Vector3d ypr(3);
const double y = atan2(n(1), n(0));
const double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
const double r =
atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
ypr(0) = y;
ypr(1) = p;
ypr(2) = r;
return ypr / M_PI * 180.0;
}
From Euler Angles XYZ
\[\begin{aligned}
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix} &= R_{x}(\phi) R_{y}(\theta) R_{z}(\psi)\begin{bmatrix}
X \\
Y \\
Z
\end{bmatrix} \\
&=\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos \phi & -\sin \phi \\
0 & \sin \phi & \cos \phi
\end{bmatrix}\begin{bmatrix}
\cos \theta & 0 & \sin \theta \\
0 & 1 & 0 \\
-\sin \theta & 0 & \cos \theta
\end{bmatrix}\begin{bmatrix}
\cos \psi & -\sin \psi & 0 \\
\sin \psi & \cos \psi & 0 \\
0 & 0 & 1
\end{bmatrix}\begin{bmatrix}
X \\
Y \\
Z
\end{bmatrix} \\
&=\begin{bmatrix}
\cos \theta \cos \psi & -\cos \theta \sin \psi & \sin \theta \\
\cos \phi \sin \psi + \sin \phi \sin \theta \cos \psi & \cos \phi \cos \psi-\sin \phi \sin \theta \sin \psi & -\sin \phi \cos \theta\\
\sin \phi \sin \psi-\cos \phi \sin \theta \cos \psi& \sin \phi \cos \psi + \cos \phi \sin \theta \sin \psi & \cos \phi \cos \theta
\end{bmatrix}\begin{bmatrix}
X \\
Y \\
Z
\end{bmatrix}
\end{aligned}\]
\[\psi = \arctan \frac{-\mathbf{R}_{01}}{\mathbf{R}_{00}}\]
\[\theta = \arctan \frac{\mathbf{R}_{02} \cos \psi}{\mathbf{R}_{00}}\]
\[\phi = \arctan \frac{-\mathbf{R}_{12}}{\mathbf{R}_{22}}\]
From Quaternion
\[\begin{aligned}
\mathbf{R}&=
\begin{bmatrix}
1-2\left(q_{y}^{2}+q_{z}^{2}\right) & 2\left(q_{x} q_{y}-q_{w} q_{z}\right) & 2\left(q_{w} q_{y}+q_{x} q_{z}\right) \\
2\left(q_{x} q_{y}+q_{w} q_{z}\right) & 1-2\left(q_{x}^{2}+q_{z}^{2}\right) & 2\left(q_{y} q_{z}-q_{w} q_{x}\right) \\
2\left(q_{x} q_{z}-q_{w} q_{y}\right) & 2\left(q_{w} q_{x}+q_{y} q_{z}\right) & 1-2\left(q_{x}^{2}+q_{y}^{2}\right)
\end{bmatrix}\\
&=\begin{bmatrix}
q_{w}^{2}+q_{x}^{2}-q_{y}^{2}-q_{z}^{2} & 2\left(q_{x} q_{y}-q_{w} q_{z}\right) & 2\left(q_{w} q_{y}+q_{x} q_{z}\right) \\
2\left(q_{x} q_{y}+q_{w} q_{z}\right) & q_{w}^{2}-q_{x}^{2}+q_{y}^{2}-q_{z}^{2} & 2\left(q_{y} q_{z}-q_{w} q_{x}\right) \\
2\left(q_{x} q_{z}-q_{w} q_{y}\right) & 2\left(q_{w} q_{x}+q_{y} q_{z}\right) & q_{w}^{2}-q_{x}^{2}-q_{y}^{2}+q_{z}^{2}
\end{bmatrix}
\end{aligned}\]
Computation
\[\mathbf{R}[\mathbf{p}]_{\times}\mathbf{R}^{\text{T}} = [\mathbf{R}\mathbf{p}]_{\times}\]
Angle-Axis (Lie Algebra)
\[\boldsymbol{\theta} = \theta \boldsymbol{\omega} = \theta \begin{bmatrix}
\omega_{x} \\
\omega_{y} \\
\omega_{z}
\end{bmatrix}\]
\[\omega_{x}^{2} + \omega_{y}^{2} + \omega_{z}^{2} = 1\]
\[[\omega]_{\times} = \begin{bmatrix}
0 & -\omega_{z} & \omega_{y} \\
\omega_{z} & 0 & -\omega_{x} \\
-\omega_{y} & \omega_{x} & 0
\end{bmatrix}\]
\[\begin{aligned}
\mathbf{R} &= \exp([\theta \boldsymbol{\omega}]_{\times}) \\
&= \mathbf{I} + [\boldsymbol{\omega}]_{\times} \sin\theta + [\boldsymbol{\omega}]_{\times}^{2}(1 - \cos \theta) \\
&= \begin{bmatrix}
\cos\theta + \omega_{x}^{2}(1 - \cos\theta) & -\omega_{z}\sin\theta + \omega_{x}\omega_{y}(1-\cos\theta) & \omega_{y}\sin\theta + \omega_{x}\omega_{z}(1-\cos\theta) \\ \omega_{z}\sin\theta + \omega_{x}\omega_{y}(1-\cos\theta) & \cos\theta + \omega_{y}^{2}(1 - \cos\theta) & -\omega_{x}\sin\theta + \omega_{y}\omega_{z}(1-\cos\theta) \\
-\omega_{y}\sin\theta + \omega_{x}\omega_{z}(1-\cos\theta) & \omega_{x}\sin\theta + \omega_{y}\omega_{z}(1-\cos\theta) & \cos\theta + \omega_{z}^{2}(1 - \cos\theta)
\end{bmatrix} \\
&= \begin{bmatrix}
R_{00} & R_{01} & R_{02} \\
R_{10} & R_{11} & R_{12} \\
R_{20} & R_{21} & R_{22}
\end{bmatrix}
\end{aligned}\]
\[\cos \theta = \frac{\text{tr}(\mathbf{R}) - 1}{2}\]
\[\theta = \cos^{-1}(\frac{\text{tr}(\mathbf{R}) - 1}{2}) + 2\pi m\]
\[\boldsymbol{\omega} = \frac{1}{2\sin\theta}\begin{bmatrix}
R_{21} - R_{12} \\
R_{02} - R_{20} \\
R_{10} - R_{01}
\end{bmatrix}\]
\[\boldsymbol{\theta} = \theta \boldsymbol{\omega} = \frac{\theta}{2\sin\theta}\begin{bmatrix}
R_{21} - R_{12} \\
R_{02} - R_{20} \\
R_{10} - R_{01}
\end{bmatrix}\]
Small angle
\[\boldsymbol{\theta} = \lim_{\theta\rightarrow0} \frac{\theta}{2\sin\theta}\begin{bmatrix}
R_{21} - R_{12} \\
R_{02} - R_{20} \\
R_{10} - R_{01}
\end{bmatrix} = \frac{1}{2}\begin{bmatrix}
R_{21} - R_{12} \\
R_{02} - R_{20} \\
R_{10} - R_{01}
\end{bmatrix}\]
Application
Alignment of Gravity
Eigen::Matrix3d RotateGravity(const Eigen::Vector3d& g) {
const Eigen::Vector3d normal1 = g.normalized();
const Eigen::Vector3d normal2{0, 0, 1};
Eigen::Matrix3d R =
Eigen::Quaterniond::FromTwoVectors(normal1, normal2).toRotationMatrix();
const double yaw = Rotation2YawPitchRoll(R).x();
R = YawPitchRoll2Rotation(Eigen::Vector3d{-yaw, 0, 0}) * R;
return R;
}
Literature
