四元数是由爱尔兰数学家威廉·卢云·哈密顿在1843年创立出的数学概念。单位四元数（Unit quaternion）可以用于表示三维空间里的旋转。它与常用的另外两种表示方式（三维正交矩阵和欧拉角）是等价的，但是避免了欧拉角表示法中的万向锁问题。比起三维正交矩阵表示，四元数表示能够更方便地给出旋转的转轴与旋转角。

欧拉角

欧拉角（Euler Angles）是一种描述三维旋转的方式，根据欧拉旋转定理，任何一个旋转都可以用三个旋转的参数来表示。但欧拉角的描述方式有很多种，并没有一个统一标准。对于定义一个欧拉角，需要明确的内容包括：

三个旋转角的组合方式（是xyz还是yzx还是zxy）
旋转角度的参考坐标系统（旋转是相对于固定的坐标系还是相对于自身的坐标系）
使用旋转角度是左手系还是右手系
三个旋转角的记法

旋转角的记法

顺序	飞行器	望远镜	符号	角速度
第一	heading	azimuth	$θ$	yaw
第二	attitude	elevation	$ϕ$	pitch
第三	bank	tilt	$ψ$	roll

旋转矩阵

对于两个三维点 $p_1(x_1, y_1, z_1)$，$p_2(x_2,y_2,z_2)$，由点 $p_1$ 经过旋转矩阵 $R$ 旋转到 $p_2$，则有：

$$R = \left[ \begin{matrix}
r_{11} & r_{12} & r_{13}\
r_{21} & r_{22} & r_{23}\
r_{31} & r_{32} & r_{33}
\end{matrix} \right]$$

$$\left[ \begin{matrix}
x_2 \
y_2 \
z_2
\end{matrix} \right] = R
\left[ \begin{matrix}
x_1 \
y_1 \
z_1
\end{matrix} \right]$$

注：旋转矩阵为正交矩阵$RR^T=E$

绕x轴旋转：

$$R_x(\theta) = \left[ \begin{matrix}
1 & 0 & 0\
0 & cos\theta & -sin\theta\
0 & sin\theta & cos\theta
\end{matrix} \right]$$

绕y轴旋转:

$$R_y(\theta) = \left[ \begin{matrix}
cos\theta & 0 & sin\theta\
0 & 1 & 0\
-sin\theta & 0 & cos\theta
\end{matrix} \right]$$

绕z轴旋转：

$$R_z(\theta) = \left[ \begin{matrix}
cos\theta & -sin\theta & 0\
sin\theta & cos\theta & 0\
0 & 0 & 1
\end{matrix} \right]$$

任意旋转矩阵：

任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。

由欧拉角求旋转矩阵

设三个轴$x，y，z$的欧拉角分别为$θ_x,θ_y,θ_z$，正弦值、余弦值分别为$s_x, c_x, s_y, c_y, s_z, c_z$那么旋转矩阵为：

$$R(\theta_x,\theta_y,\theta_z)=R_z(\theta_z)R_y(\theta_y)R_x(\theta_x) = \left[ \begin{matrix}
c_y c_z & c_z s_x s_y - c_x s_z & s_x s_z + c_x c_z s_y\
c_y s_z & c_x c_z + s_x s_y s_z & c_x s_y s_z - c_z s_z\
-s_y & c_y s_x & c_x c_y
\end{matrix} \right]$$

由旋转矩阵求欧拉角

$$R = \left[ \begin{matrix}
r_{11} & r_{12} & r_{13}\
r_{21} & r_{22} & r_{23}\
r_{31} & r_{32} & r_{33}
\end{matrix} \right] = \left[ \begin{matrix}
c_y c_z & c_z s_x s_y - c_x s_z & s_x s_z + c_x c_z s_y\
c_y s_z & c_x c_z + s_x s_y s_z & c_x s_y s_z - c_z s_z\
-s_y & c_y s_x & c_x c_y
\end{matrix} \right]$$

解方程可得：

$$\theta_x = atan2(r_{32}, r_{33})$$
$$\theta_y = atan2(-r_{31}, \sqrt{r_{32}^2+r_{33}^2})$$
$$\theta_z = atan2(r_{21}, r_{11})$$

注意：atan2()为C++中的函数，atan2(y，x) 求的是y/x的反正切，其返回值为[-pi,+pi]之间的一个数。

四元数

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量$(x,y,z)$来表示，$θ$可以用一个角度值来表示，直观来讲，一个四维向量$(θ,x,y,z)$就可以表示出三维空间任意的旋转。注意，这里的三维向量$(x,y,z)$只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值$θ$。这样以来，可以用一个三维向量$(θ∗x,θ∗y,θ∗z)$就可以表示出三维空间任意的旋转，前提是其中$(x,y,z)$是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是这种表示方法来表示旋转(见OpenCV相机标定部分的rvec)。

单位向量(x,y,z)旋转θ角度后的四元数：

$$(cos \frac{\theta}{2}, xsin \frac{\theta}{2}, ysin \frac{\theta}{2}, z*sin \frac{\theta}{2})$$

对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。

四元数求旋转矩阵

已知四元数:

$$\mathbf{q} = q_0 + q_1 i + q_2 j + q_3 k = [s, \mathbf{v}]$$

利用Rodrigues公式可以由四元数求得旋转矩阵R:

$$R = \left[ \begin{matrix}
1 - 2 q_2^2 - 2 q_3^2 & 2q_1 q_2 - 2q_0 q_3 & 2 q_1 q_3 + 2 q_0 q_2 \
2q_1 q_2 + 2q_0 q_3 & 1 - 2 q_1^2 - 2 q_3^2 & 2 q_2 q_3 - 2 q_0 q_1 \
2 q_1 q_3 - 2 q_0 q_2 & 2 q_2 q_3 + 2 q_0 q_1 & 1 - 2 q_1^2 - 2 q_2^2
\end{matrix} \right ]$$

旋转矩阵求四元数

给出其中一种情况的计算方法：

$$q_0 = \frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}$$
$$q_1 = \frac{r_{32}-r_{23}}{4q_0}$$
$$q_2 = \frac{r_{13}-r_{31}}{4q_0}$$
$$q_3 = \frac{r_{21}-r_{12}}{4q_0}$$

其中要满足 $q_0 \neq 0$，$1+r_{11}+r_{22}+r_{33}>0$，即 $1+tr(R)>0$

四元数进行姿态变换

假设坐标系O1上的点P1(x1, y1, z1), 存在变换矩阵Ｒ, 可计算P1点在坐标系O2上的坐标值为P2(x2, y2, z2)：
$$P2=R∗P1$$

矩阵Ｒ对应的四元数为q, 则使用四元数计算为:
首先三维空间点用一个虚四元数来描述：P1=[0, x1, y1, z1], P2=[0, x2, y2, z2]
则P2和P1将计算关系为：

$$P_2=qP_1q^{-1}$$

参考链接

四元数,by wikipedia.
四元数与空间旋转,by wikipedia.
从旋转矩阵计算欧拉角,by PengChao.
旋转变换（一）旋转矩阵,by csxiaoshui.
旋转矩阵、欧拉角、四元数理论及其转换关系,by jason_ql.
旋转变换（二）欧拉角,by csxiaoshui.
欧拉角,by wikipedia.
欧拉角细节/旋转顺序/内旋外旋,by 能儿.
四元数运算与姿态变换,by Yoyo_wym.
Flight Parameters and Quaternion Math,by mathwork.