三维重建基础知识：四元素、旋转矩阵与旋转向量

旋转矩阵

1、坐标系绕X轴旋转$\varphi$角（Pitch 俯仰角）
$$R_x=\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi & cos\varphi \end{array}\right]\end{array}$$
2、坐标系绕Y轴旋转$\theta$角（Yaw 偏航角）
$$R_y=\begin{array}{c}\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\end{array}$$
3、坐标系绕Z轴旋转$\psi$角（Roll 翻滚角）
$$R_z=\begin{array}{c}\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]\end{array}$$
整个坐标系的旋转矩阵是绕各个轴旋转的旋转矩阵的乘积。
$$R=R_x\ast R_y\ast R_z$$
旋转矩阵必须是正交的。

Rodrigue旋转向量

设旋转向量的单位向量为$r=(r_x,r_y,r_z)$(旋转轴),模为$\theta$（旋转角），三维点$p$在旋转向量$r$的作用下变成$p^{\prime }$:
$$p^{\prime }=cos\theta \cdot p+(1-cos\theta )(p\cdot r)\cdot r+sin\theta \cdot r\times p$$
旋转向量转旋转矩阵：
$$R=I+(1-cos\theta )\ast P^2+sin\theta \ast P$$
其中
$$P=\begin{array}{c}\left[\begin{array}{ccc}0& -r_z& r_y\\ r_z& 0& -r_x\\ -r_y& r_x& 0\end{array}\right]\end{array}$$

四元素

四元素是三维复数域，可表示为Q=[x,y,z,w]或Q=w+xi+yj+z*k,其中w是实部，[x,y,z]是虚部，[i,j,k]是虚轴，四元数是单位向量。描述了旋转轴$r=(r_x,r_y,r_z)$和角$\theta$，对应的四元数为：
$$Q=[cos(\theta /2),r_x\ast sin(\theta /2),r_y\ast sin(\theta /2),r_z\ast sin(\theta /2)]$$
两个四元素叉乘的物理含义：先绕第一个四元素旋转，再绕第二个旋转。

Citation

https://blog.csdn.net/tchenjiant/article/details/51485745
https://blog.csdn.net/hongbin_xu/article/details/78929006
https://zhuanlan.zhihu.com/p/93563218