三维空间中的几何变换(旋转矩阵、变换矩阵、旋转向量、欧拉角、四元数)

旋转矩阵表示形式

  由欧拉旋转定理可得：刚体在三位空间里的一般运动可以分解为刚体上某一点的平移，以及绕过该点旋转轴的转动；
只考虑旋转：
  三维空间中的点p在坐标系$(e_1,e_2,e_3)$中的坐标为$(a_1,a_2,a_3)$,当坐标系$(e_1,e_2,e_3)$旋转到坐标系$(e_1^{\prime },e_2^{\prime },e_3^{\prime })$,点p的坐标为$(a_1^{\prime },a_2^{\prime },a_3^{\prime })$，如式1所示：
$$\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\text{\hspace{1em}(1)}$$

等式两边同乘${\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]}^T$，由于${\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3$为正交向量有：
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\triangleq \mathbf{R}{\mathbf{a}}^{\prime }\text{\hspace{1em}(2)}$$

则称 $R$ 为旋转矩阵，其包含以下性质：

• R是正交矩阵: $R^{-1}=R,R^{T}R=I$
• R的行列式为1：$|R|=1$
  注：上式两个条件是充分必要条件，只要满足上式两个条件的矩阵都可以作为一个旋转矩阵，即：
  $$SO(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}\mid \mathbf{R}{\mathbf{R}}^T=\mathbf{I},\det (\mathbf{R})=1\right\}\text{\hspace{1em}(3)}$$

  将所有满足式3的矩阵称为 Special Orthognal Group （特殊正交群），下面只考虑三维空间中的特殊正交群，坐标系1到坐标系2的之间的旋转关系可由式4来表示：
$$\begin{gathered} a_1=R_{12}{a}_2 \\ {a}_2=R_{21}a1\text{\hspace{1em}(4)} \end{gathered}$$
其中 $R_{21}=R_{12}^{-1}=R_{12}^T$，若一个旋转矩阵取转置或取逆，则表示一个相反方向的旋转；
旋转+平移：
  若同时考虑平移，则两个坐标系下p点的坐标关系如式5所示：
$$a^{\prime }=Ra+t\text{\hspace{1em}(5)}$$
具体表示形式为：
$$\begin{gathered} a_2=R_{21}\cdot a_1+t_{21} \\ {a}_1=R_{12}\cdot a_2+t_{12} \end{gathered}$$
注：$t_{12}\ne -t_{21}$

齐次坐标表示：
由于刚体在三维空间中可能发生多次旋转与平移，如式6所示：
$$\mathbf{b}={\mathbf{R}}_{1}a+t_1,c={\mathbf{R}}_{2}b+t_2.⟹c={\mathbf{R}}_2\left({\mathbf{R}}_{1}a+t_1\right)+t_2.\text{\hspace{1em}(6)}$$
为了方便表达这种运动，引入了其次坐标表示形式如式7所示：
$$\left[\begin{array}{l}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]\triangleq \mathbf{T}\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]$$
其中$T=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$ 为变换矩阵(同时包含旋转与平移),变换矩阵的逆${\mathbf{T}}^{-1}$为：
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^{T}t\\ 0^T& 1\end{array}\right]$$

将变换矩阵的集合称为特殊欧式群SE(3) (Special Euclidean Group)：
$$\mathrm{SE}(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in \mathrm{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}\text{\hspace{1em}(7)}$$

其中$\left[\begin{array}{l}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]$ 为齐次坐标，齐次坐标乘以任意非零常熟仍然表示三维空间中的同一个坐标：
$$\tilde{a}=\left[\begin{array}{l}a\\ 1\end{array}\right]=k\left[\begin{array}{l}a\\ 1\end{array}\right]\text{\hspace{1em}(8)}$$

则多次的旋转与平移可以如式9所示：
$$\tilde{b}={\mathbf{T}}_1\tilde{\mathbf{a}},\tilde{\mathbf{c}}={\mathbf{T}}_2\tilde{\mathbf{b}}\Rightarrow \tilde{\mathbf{c}}={\mathbf{T}}_2{\mathbf{T}}_1\tilde{\mathbf{a}}\text{\hspace{1em}(9)}$$

旋转向量

  旋转矩阵用9个元素的旋转矩阵仅仅表示3个自由度的信息，表达形式过于复杂，不节省内存空间。从直观上看，旋转可以表示为绕某个轴 $n$ 旋转一定的角度 $\theta$，为此引入了旋转向量(Rotation Vector) 或角轴/轴角（Angle Axis), 旋转矩阵与旋转向量是等价的，可以互相转换：
旋转向量到旋转矩阵的变换： 罗德里格斯公式(Rodrigus’s Formula):
$$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }\text{\hspace{1em}(10)}$$
其中$\cos \theta \mathbf{I}$ 为3x3矩阵，$(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T$也为3x3的矩阵，${\mathbf{n}}^{\wedge }$表示旋转向量 $n$ 的叉积。
旋转矩阵到旋转向量：
$$\begin{gathered} \theta =\arccos \left(\frac{\mathrm{tr}(\mathbf{R})-1}{2}\right) \\ Rn=n\text{\hspace{1em}(11)} \end{gathered}$$
$n$ 为 $R$ 矩阵对应特征值为1的特征向量；

欧拉角(Euler Angles)

  旋转向量表示的旋转方式不够直观，无法从旋转向量中直接看出旋转轴的位置，欧拉角将旋转分解为三个方向上的旋转（如Z-Y-X）方向，轴可以为定轴或者动轴，轴的顺序也可以改变，如最常用绕动轴的Z-Y-X旋转方式:

1. 绕物体的Z轴转动，得到偏航角yaw；
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch；
3. 绕旋转之后的X轴旋转，得到滚转角roll；

万向锁（Gimbal Lock）：欧拉角在某些旋转情况下，旋转自由度会减1，存在奇异性；由于万向锁的存在，欧拉角不适合做插值或迭代，应用不广泛；

四元数

四元素基础

  四元数是一种既节省空间，又不存在奇异性的刚体旋转表示方式；
在复数平面坐标系下，可以用单位复数来表达旋转操作：
$$z=x+iy=\rho e^{i\theta }\text{\hspace{1em}(12)}$$
当 $z$ 乘以一个纯虚数 $i$ 表示原来的向量旋转90度：
$$zi=(x+iy)i=\rho e^{i(\theta +\frac{\pi }{2})}\text{\hspace{1em}(13)}$$
在三维情况下， 四元数可以作为复数的扩充：
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k\text{\hspace{1em}(14)}$$
四元数包含1个实部与3个虚部，虚部之间满足以下的关系（自己与自己运算像复数运算，自己与别人运算像叉乘）：
$$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}\text{\hspace{1em}(15)}$$
单位四元数可以表达旋转：
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k,\mathbf{q}=[s,\mathbf{v}],s=q_0\in \mathbb{R},\mathbf{v}={\left[q_1,q_2,q_3\right]}^T\in {\mathbb{R}}^3\text{\hspace{1em}(16)}$$

基本运算如下所示：
加减法：
$$\begin{array}{rl}{\mathbf{q}}_a\pm {\mathbf{q}}_b=& \left[s_a\pm s_b,{\mathbf{v}}_a\pm {\mathbf{v}}_b\right]\end{array}$$
乘法：
$$\begin{array}{rl}{\mathbf{q}}_a{\mathbf{q}}_b=& s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +\left(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b\right)i\\ & +\left(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b\right)j\\ & +\left(s_a{z}_b+x_a{y}_b-y_b{x}_a+z_a{s}_b\right)k\end{array}$$
$$\begin{array}{r}{\mathbf{q}}_a{\mathbf{q}}_b=\left[s_a{s}_b-{\mathbf{v}}_a^T{\mathbf{v}}_b,s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\right]\end{array}$$
共轭：
$${\mathbf{q}}_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=\left[s_a,-{\mathbf{v}}_a\right]$$
取模长：
$$\Vert {\mathbf{q}}_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$
取逆：
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/\Vert \mathbf{q}{\Vert }^2$$
数乘：
$$k\mathbf{q}=[ks,k\mathbf{v}]$$
点乘：
$${\mathbf{q}}_a\cdot {\mathbf{q}}_b=s_a{s}_b+x_a{x}_{b}i+y_a{y}_{b}j+z_a{z}_{b}k$$

旋转角转换为四元数：
$$\mathbf{q}={\left[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}\right]}^T\text{\hspace{1em}(17)}$$

四元数转为旋转向量：
$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^T={\left[q_1,q_2,q_3\right]}^T/\sin \frac{\theta }{2}\end{array}\text{\hspace{1em}(18)}$$

使用四元数表示三维空间中的旋转

  若点 $p$ 经过一次以四元数 $q$ 表示的旋转后得 $p^{\prime }$ ，包含以下两个步骤：

1. 将 $p$ 坐标用纯虚的四元数表示：$p=[0,x,y,z]$
2. 使用四元数q将其旋转(前面乘以 $q$ ，后面乘以 $q^{-1}$ )：$$p^{\prime }=qp{q}^{-1}\text{\hspace{1em}(19)}$$