旋转向量与欧拉角、四元数的转换及Eigen实践

开篇介绍

在之前了解到旋转向量与旋转矩阵、变换矩阵的关系后，我们进行了Eigen库上基本矩阵的操作实践。接下来本篇将介绍的是旋转向量与欧拉角、四元数之间的转换以及向量旋转在Eigen上的表达。

一、旋转向量

在上篇有了旋转矩阵和变换矩阵描述了向量的旋转后，我们发现假如一直沿用矩阵来表达向量旋转的话，会有几个明显的缺点：①旋转矩阵具有九个量，但是一次旋转具有三个自由度，变换矩阵用了十六个量表达了六个自由度的变换，显然这是一种冗余的表达方式。②旋转矩阵必须是正交矩阵且行列式为1。变换矩阵也是在旋转矩阵的基础上得到的，因此当我们想要估计或者优化一个旋转矩阵或变换矩阵时，这些制约会使求解变得更加困难。
于是，我们希望有一种更加紧凑的方式来描述旋转和平移。首先，我们知道任意旋转都能由一个旋转轴和旋转角来刻画。于是，我们可以使用一个向量，其方向和旋转轴一致，而长度等于旋转角，我们称之为旋转向量(或轴角)。此时，我们只需一个三维向量即可描述旋转。对于变换矩阵，我们可以用一个旋转向量和一个平移向量即可表达一次变换。
假设有一个旋转轴为$n$，角度为$\theta$的旋转，显然，它对应的旋转向量为$\theta n$。由旋转向量到旋转矩阵的过程由罗德里格斯公式表明(后续抽空更新其推导)，其转换结果为：
$$R=I+(1-\cos \theta )n{n}^T+\sin \theta n^{\Lambda }$$
因此，对于转角$\theta$，有$$\begin{array}{rl}tr(R)& =\cos \theta tr(I)+(1-\cos \theta )tr(n{n}^T)+\sin \theta tr(n^{\Lambda })\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta \end{array}$$
因此，有$$\theta =\arccos (\frac{tr(R)-1}{2})$$
而对于旋转轴$n$，由于旋转轴上的向量在旋转后不发生改变，说明$$Rn=n$$，转轴$n$是矩阵$R$特征值1对应的特征向量。求解此方程，再归一化，即得到了旋转轴。

二、欧拉角

无论是旋转矩阵、旋转向量，虽然都能描述旋转，但对我们人类是非常不直观的。当我们看到一个旋转矩阵或旋转向量时，很难想象出这个旋转究竟是如何的。==而欧拉角提供了一种非常直观的方式来描述旋转——它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。一般，我们把ZYX轴顺序(也即偏航角yaw-俯仰角pitch-滚转角roll)旋转称为欧拉角。==但由于欧拉角存在一个万向锁问题。在俯仰角为±90°时，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度，这也称为奇异性问题。于是我们不会在滤波或者优化中使用欧拉角表达旋转。不过，若想验证自己算法是否有错误时，转换成欧拉角能快速辨认结果的正确与否。

三、四元数

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但是具有奇异性。==三维旋转是一个三维流形，想要无奇异地表达它，用三个量是不够的，因此我们需要引入新的表达：四元数。==四元数q拥有一个实部和三个虚部：$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$其中$i,j,k$为四元数的三个虚部。这三个虚部满足关系式：
$$\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：$$q=[s,\mathbf{v}],s=q_0\in R,\mathbf{v}=[q_1,q_2,q_3{]}^T\in R^3$$，这里，s称为四元数的实部，而$\mathbf{v}$称为它的虚部。如果一个四元数虚部为$0$，称之为实四元数。反之，若它的实部为0，称之为虚四元数。
假设某个旋转是绕单位向量$\mathbf{n}=[n_x,n_y,n_z{]}^T$进行了角度为$\theta$的旋转，那么这个旋转的四元数形式为：$$\mathbf{q}=[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}{]}^T$$
反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：
$\begin{array}{r}\{\begin{array}{cc}\theta & =2\arccos q_0\\ [n_x,n_y,n_z{]}^T& =[q_1,q_2,q_3{]}^T/\sin \frac{\theta }{2}\end{array}\end{array}$
这式子给我们一种微妙的“转了一半”的感觉。若对$q$中的$\theta$加上$2\pi$，我们可得到一个相同的旋转，但是此时四元数变成了$-q$。因此，在四元数表达中，任意的旋转都可以由两个互为相反数的四元数表示。

四、Eigen实践

熟悉了旋转向量、欧拉角和四元数之间的关系后，我们在Kdevelop上进行实践：(代码里已添加注释)

在构建项目完成后，点击执行，得出调试结果如下：

五、总结

在熟悉了向量的旋转表达后，我们知道了四元数能在非奇异的形式下表达向量的旋转，而欧拉角能直观的显示出最终结果，以便验证我们的算法。在后续博客中将更新李群与李代数。