旋转矩阵、旋转向量、欧拉角、四元数【SLAM】

1、旋转矩阵

$n$维旋转矩阵的集合定义：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n) = \{ R \in \mathbb{R}^{n \times n} \mid RR^{T} = I, det(R) = 1 \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
$SO(n)$是特殊正交群（Special Orthogonal Group）。$R$是正交矩阵，满足$R{R}^T=I$。行列式为$1$是人为定义的，实际行列式可以为$\pm 1$，行列式为$-1$的称为瑕旋转，即一次旋转加一次反射，三维矩阵行列式的意义是变换后的体积与原体积的比，行列式为$1$说明体积不变，旋转变换就是体积不变。

缺点：
1、$SO(3)$的旋转矩阵有$9$个量，一次旋转只有$3$个自由度，所以这种表达方式是冗余的。
2、旋转矩阵自身带有约束，必须满足正交矩阵和行列式为$1$，在优化旋转矩阵的情况中给求解带来困难。

2、旋转向量

用旋转轴和旋转角来表示一个旋转。使用一个向量，方向与旋转轴一致，长度等于旋转角，称为旋转向量，或轴角/角轴（Axis-Angle）。

旋转向量与旋转矩阵的转换

$\theta \mathbf{n}$为旋转向量，转换为旋转矩阵用罗德里格斯公式：
$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{\wedge }$$

旋转矩阵$R$转换为旋转向量:
$$\theta =arccos\frac{tr(R)-1}{2}$$
$\mathbf{n}$是矩阵$R$特征值为$1$的特征向量。

3、欧拉角

把一个旋转分解成三次绕不同的轴旋转。
缺点：万向锁问题，例如ZYX欧拉角，Y为90度时，Z和X在旋转时是同一个直线上的轴，使系统丢失一个自由度。理论上可以证明，只要想用3个实数表达三维旋转就会碰到奇异性问题，旋转向量也具有奇异性，发生在转角$\theta$超过$2\pi$而产生周期性时。

4、四元数

旋转矩阵表示有冗余性，旋转向量和欧拉角表示具有奇异性。四元数是紧凑的，且没有奇异性。四元数分为JPL四元数和Hamilton四元数，JPL四元数是$ij=-k$，Hamilton四元数是$ij=k$，这里用Hamilton四元数。

三维旋转用单位四元数表示。
三维空间点$p$用虚四元数表示，给一个旋转变换$q$，结果为：
$$p^{\prime }=qp{q}^{-1}$$
结果一定是虚四元数。

四元数到旋转矩阵的变换

单位四元数$q=[s,{\mathbf{v}}^T{]}^T$，旋转矩阵为：
$$R=\mathbf{v}{\mathbf{v}}^T+s^{2}I+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2$$

四元数到旋转向量的变换

$$\theta =2arccos(s)$$
$$\mathbf{n}=\frac{\mathbf{v}}{sin\frac{\theta }{2}}$$

旋转向量到四元数的变换

$$q=[cos\frac{\theta }{2},{\mathbf{n}}^{T}sin\frac{\theta }{2}{]}^T$$