1.三维空间刚体的旋转

参考资料：视觉SLAM十四讲
https://mp.weixin.qq.com/s/De-fkRVlqvYN6W9zYHS1_A

概念梳理

刚体在三维空间中的运动可以通过如下四种方式描述：
旋转矩阵，旋转向量，欧拉角和四元数。

标准正交基

在线性代数中，一个内积空间的正交基事元素两两正交的基。我们称基中的元素为基向量。假若，一个正交基的基向量的模长都是单位长度1，则称为正交基为标准正交基。

正交矩阵

正交矩阵即逆为自身转置的矩阵。

反对称矩阵

对于三维空间中的向量a,b,外积可用来描述a到b之间是如何旋转的，其中外积的方向即为旋转矩阵的方向。外积可表示成：

旋转矩阵

旋转矩阵有比较强的约束条件。旋转矩阵R具有正交性，R和R的转置乘积是单位阵，且行列式值为1.
旋转矩阵R的逆矩阵表示了一个和R相反的旋转。
引入齐次坐标是为了可以方便的描述连续的欧氏变换

点，向量，坐标系

刚体不光有位置，还有自身的姿态，位置是指刚体在空间中的哪个地方，姿态是指刚体的朝向。

如果我们确定了一个坐标系，也就是一个线性空间的基（e1,e2,e3）,那么就可以讨论向量a在这组基下的坐标了。
a = [ e 1 e 2 e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 a = \begin{bmatrix} e_1 & e_2 & e_3 \\ \end {bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \\ \end{bmatrix} = a_1e_1+a_2e_2+a_3e_3 a=[e1e2e3]⎣ ⎡a1a2a3⎦ ⎤=a1e1+a2e2+a3e3

内积可以写成：
a ⃗ ⋅ b ⃗ = a ⃗ T b ⃗ = ∣ a ⃗ ∣ ∣ b ⃗ ∣ cos ⁡ < a ⃗ , b ⃗ > \vec {a} \cdot \vec{b} = \vec{a}^T\vec{b}=|\vec{a}||\vec{b}|\cos<\vec{a},\vec{b}> a ⋅b =a Tb =∣a ∣∣b ∣cos<a ,b >
内积可以描述向量间的关系。

外积的方向垂直于这两个向量，大小为|a||b|sin<a,b>,是两个向量张成的四边形的有向面积。外积只对三维向量存在定义，我们还能用外积表示向量的旋转。

欧式变换

相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧式变换。这样的欧式变换由一个旋转和一个平移组成。

假设有一组正交基为 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) (e1,e2,e3)下某个向量的值为 [ a 1 , a 2 , a 3 ] T [a_1,a_2,a_3]^T [a1,a2,a3]T,那么在 ( e 1 ′ , e 2 ′ , e 3 ′ ) (e_1',e_2',e_3') (e1′,e2′,e3′)下,这个向量的坐标为 [ a 1 ′ , a 2 ′ , a 3 ′ ] [a_1',a_2',a_3'] [a1′,a2′,a3′]

有

于是有

中间的矩阵拿出来定义为旋转矩阵R。这个矩阵刻画了旋转前后同一个向量的坐标变化关系,只要旋转时一样的,这个矩阵就是一样的,我们把R称为旋转矩阵另外,旋转矩阵是一个行列式为1的正交矩阵,可以描述相机的旋转。反之，行列式为1的正交矩阵也是一个旋转矩阵。

由于旋转矩阵为正交矩阵，它的逆和转置相同，故有：

所以 R T R^T RT是 R R R的逆运算.
R T R^T RT刻画了一个相反的旋转。
综上,向量 a ⃗ \vec{a} a ,经过一次旋转(用 R R R描述)和一次平移t之后,得到的 a ⃗ ′ \vec{a}' a ′
a ⃗ ′ = R a ⃗ + t \vec{a}' = R\vec{a} + t a ′=Ra +t
其中t为平移向量。
总结：这里我们就用了一个旋转矩阵和一个平移向量完整地描述了一个欧式空间的坐标变换关系。

变换矩阵和齐次坐标

旋转向量和欧拉角

旋转向量

旋转向量是旋转矩阵的另一种表达方式，使用一个三维向量来描述旋转，由旋转轴n和旋转角theta来刻画，也成为轴角。因为旋转角度有一定的周期性，所以这种表达方式具有奇异性。

从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明，具体形式如下：

从旋转矩阵求旋转向量（旋转角）：

t r ( R ) tr(R) tr(R)是矩阵 R R R的迹.

由于旋转轴上的向量在旋转偶不发生改变（欧式变换），说明：
R n ⃗ = n ⃗ R\vec{n} = \vec{n} Rn =n
即转轴n是旋转矩阵R特征值1对应的特征向量。

需要注意的是，旋转向量和旋转矩阵的转换关系，对应于李代数和李群的映射。

欧拉角

欧拉角提供了一种非常直观的方式来描述旋转，它使用三个分离的转角，即将一个旋转分解成三次饶不同轴的旋转。

ZYX 转角相当于把任意旋转分解成以下三个轴上的转角 :
1、绕物体的 Z 轴旋转，得到偏航角 yaw
2、绕旋转之后的 Y 轴旋转，得到俯仰角 pitch
3、绕旋转之后的 X 轴旋转，得到滚转角 roll
但是欧拉角的一个重大缺陷是会碰到万向锁问题：在俯仰角为±±90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失一个自由度。

三次roll将使用同一个轴,使得系统丢失一个自由度.
所以,欧拉角不适于插值和迭代,往往只用于人机交互.

四元数

四元数是Hamilton找到的一种扩展的复数。一个四元数拥有一个实部和三个虚部。