第3讲旋转向量、欧拉角、四元数

旋转向量

从上一篇中已经知道，旋转可以用旋转矩阵来表示，变换可以用变换矩阵来表示，那么为什么还需要旋转向量呢？

仔细想一下，矩阵表示方式至少有以下几个缺点：

$SO(3)$ 的旋转矩阵有9个量，但是一次旋转只有3个自由度，因此这种表达方式是冗余的。同理，变换矩阵用16个量来表示6自由度的变换也是冗余的。我们需要一种更紧凑的表达方式。
旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当想要估计或者优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得十分困难。

综合上面几点原因，我们希望找到一种更紧凑的方式来表达旋转和平移。

通过前面的学习，我们已经知道了可以用外积来表示两个向量间的旋转。

顺着这个思路下去，看一下如何用一个三维向量来表示旋转？

我们知道，任意一个旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向和旋转轴一致，长度等于旋转角。这种向量就称为旋转向量。通过这种表达方式，通过一个三维向量就可以表示旋转了。

同理，对于变换矩阵，我们使用一个旋转向量+平移向量即可表达一次变换。

思路有了，剩下的问题就是旋转矩阵和旋转向量之间是如何转换的？

从旋转向量到旋转矩阵的转换过程由Rodrigues's Formula表示： $R=\cos \theta I + \left ( 1-\cos \theta \right )\vec{n}\vec{n}^{T}+\sin \theta \vec{n}^{\Lambda }$ 。（符号^是从向量到反对称矩阵的转换符，前面已经说明过这样的转换方式）。通过这个公式，由一个旋转向量和旋转角就可以得到旋转矩阵了。

反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。

对于旋转角 $\theta$ ：

$R=\cos \theta I + \left ( 1-\cos \theta \right )\vec{n}\vec{n}^{T}+\sin \theta \vec{n}^{\Lambda }$

$\Rightarrow$ $tr(R)=\cos \theta tr(I) + \left ( 1-\cos \theta \right )tr(\vec{n}\vec{n}^{T})+\sin \theta tr(\vec{n}^{\Lambda })$

$\Rightarrow$ $tr(R)=3\cos \theta + \left ( 1-\cos \theta \right )$

$\Rightarrow$ $tr(R)=1+2 \cos \theta$

$\Rightarrow$ $\theta = \arccos(\frac{tr(R)-1}{2})$

对于旋转轴 $\vec{n}$ ：

我们知道，旋转轴上的向量经过旋转之后不改变，说明 $R\vec{n}=\vec{n}$

因此，转轴 $\vec{n}$ 是矩阵R的关于特征值1的特征向量

求解矩阵方程 $R\vec{n}=\vec{n}$ ，然后归一化，就得到了旋转轴。

欧拉角

无论是旋转矩阵还是旋转向量，它们虽然能够描述旋转，但是对我们人类是非常不直观的。当我们看到一个旋转矩阵或者旋转向量时，很难想象出这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是朝哪个方向转动。

欧拉角提供了一种非常直观的方式来描述旋转---它使用了三个分离的转角，把一个旋转分解成3次绕不同轴的旋转。注：欧拉角的分解方式有很多种，因此欧拉角也会有不同的定义方法，但是思想都是一样的。

下面介绍一种比较常用的欧拉角：用偏航角 - 俯仰角 - 翻滚角（yaw - pitch - roll）三个角度来描述一个旋转。

由于它等价于ZYX轴的旋转，因此就以ZYX为例。

假设一个刚体的前方（朝向我们的方向）为X轴，右侧为Y轴，上方为Z轴，如下图所示：

那么，ZYX转角相当于把任意旋转分解成以下三个轴上的转角：

1、绕物体的Z轴旋转，得到偏航角yaw

2、绕旋转之后的Y轴旋转，得到俯仰角pitch

3、绕旋转之后的X轴旋转，得到翻滚角roll

此时，可以使用 $[r,p,y]^{T}$ 这样一个三维的向量来描述任意旋转。这个向量是非常直观的，我们可以从这个向量中想象出旋转的过程。

其他定义的欧拉角也是通过这种方式，把旋转分解到3个轴上，得到一个三维向量，只是选用的轴和顺序不同。

欧拉角存在一个重大的缺点：著名的万向锁问题

可以看一下这里，帮助理解什么是万向锁。

可以证明：只要想用3个实数来表达三维旋转时，都会不可避免的碰到万向锁问题。因此很少在SLAM程序中直接使用欧拉角来表达姿态。

四元数

单位四元数（unit quaternion）可以用于表示三维空间里的旋转。它与常用的另外两种表示方式（三维正交矩阵和欧拉角）是等价的，但是避免了欧拉角表示法中的万向锁问题，比起三维正交矩阵表示，四元数表示能够更方便的给出旋转的转轴和旋转角。

暂时先不管四元数的含义，先来看看四元数的形式：

一个四元数 $q$ 拥有1个实部和3个虚部，像下面这样： $q=q_{0}+q_{1}i+q_{2}j+q_{3}k$ ，其中i、j、k为四元数的3个虚部，这三个虚部满足：

由于四元数的这种特殊的表示形式，有时也会有人用一个标量和一个向量来表达四元数： $q=[s,\vec{v}],s=q_{0}\epsilon R,\vec{v}=[q_{1},q_{2},q_{3}]^{T}\epsilon \mathbb{R}^{3\times 3}$

如果一个四元数的实部为0，则将它称为虚四元数（纯四元数）；如果一个四元数的虚部为0，则将它称为实四元数。

四元数的含义（关于四元数的更多资料可以看这里）

四元数可以用来表示三维空间里的点，也可以用来表示三维空间的旋转

1、四元数表示三维空间中的点

若三维空间中的一个点的笛卡尔坐标为 $(x,y,z)$ ，则用纯四元数表示为： $xi+yj+zk$

2、单位四元数表示一个三维空间旋转

设 q 为一个单位四元数，而 p 是一个纯四元数，定义 $R_{q}(p)=qpq^{-1}$ ，

则 Rq(p) 也是一个纯四元数，可以证明 Rq 确实表示一个旋转，这个旋转将空间的点 p 旋转为空间的另一个点 Rq(p)。

四元数表示旋转

用单元四元数表示旋转和用正交矩阵表示旋转是等价的，这可以通过直接的代数计算得到。

1、从旋转向量到四元数的转换

假设某个旋转是绕单位向量 $\vec{n}=[n_{x},n_{y},n_{z}]^{T}$ 进行了角度为 $\theta$ 的旋转，那么这个旋转的四元数形式为

$q=\cos \frac{\theta}{2}+n_{x}\sin \frac{\theta}{2} i+n_{y}\sin \frac{\theta}{2} j+n_{z}\sin \frac{\theta}{2} k$

如果写成向量的形式就是： $q=[\cos \frac{\theta}{2},n_{x}\sin \frac{\theta}{2} ,n_{y}\sin \frac{\theta}{2} ,n_{z}\sin \frac{\theta}{2} ]^{T}$

2、从四元数到旋转轴、旋转角的转换

假设某个旋转用单位四元数 $q=q_{0}+q_{1}i+q_{2}j+q_{3}k$ 表示，可以通过下列公式计算出旋转轴和夹角：

$\theta = 2 \arccos q_{0}$

$[n_x,n_y,n_z]^{T}=[q_0,q_1,q_2]^{T}/\sin\frac{\theta}{2}$

3、用四元数表示旋转

上面已经介绍了四元数和旋转轴、旋转角之间的相互转换，那么如果已经知道了点p，以及旋转q（用单位四元数表示的），如果计算旋转后得到的点 $p^{'}$ 的坐标呢？

假设一个空间三维点 $\vec{p}=[x,y,z]\epsilon \mathbb{R}^{3}$ ，以及一个由轴角 $\vec{n},\theta$ 指定的旋转，它们之间的关系可以用下列式子来表达：

首先，把三维空间点用一个纯四元数来描述： $\vec{p}=[0,x,y,z]^{T}=[0,\vec{v}]$
然后，用四元数来表示旋转： $q=[\cos\frac{\theta}{2},\vec{n}\sin\frac{\theta}{2}]$ （用上面的1中给出的公式计算得到）
旋转之后的点 $p^{'}$ 可以表示为： $p^{'}=qpq^{-1}$

可以验证，计算结果的实部为0，也就是一个纯四元数。虚部的3个分量表示旋转后的点的坐标。

四元数和旋转矩阵之间的转换

这一部分有较多的公式的推导，暂时先不看了。先对整体框架有个完整的了解之后在补把。

做个标记，书本的55页。

实践部分：Eigen几何模块

现在，我们通过编程在Eigen中使用四元数、欧拉角和旋转矩阵，演示它们之间的变换方式。

这里，要接触到Eigen中的一个新的模块Geometry，Eigen/Geometry模块提供了各种旋转和平移的表示。

#include <iostream>
#include "Eigen/Geometry"

using namespace std;

int main(int argc, char **argv)
{
    //使用Matrix3d定义一个旋转矩阵
    Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity();    //Eigen::Matrix3d::Identity()返回一个单位矩阵，不一定是方阵

    //使用AngleAxisd来定义一个旋转向量，它底层不直接是Matrix，但是可以当做矩阵来进行运算（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));

    cout.precision(3);  //输出时，小数点之后保留3位小数

    //将旋转向量转换为旋转矩阵输出
    cout<<"rotation_vector ---> rotation_matrix"<<endl;
    cout<<rotation_vector.matrix()<<endl;

    //也可以通过调用toRotationMatrix()方法转换为旋转矩阵直接赋值
    rotation_matrix=rotation_vector.toRotationMatrix();
    cout<<"rotation_vector ---> rotation_matrix"<<endl;
    cout<<rotation_matrix<<endl;

    //用旋转向量进行坐标转换
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_ratated=rotation_vector*v;
    cout<<"made a rotation by rotation_vector:"<<endl;
    cout<<"(1, 0, 0) after rotation = "<<v_ratated.transpose()<<endl;

    //用旋转矩阵来进行旋转
    v_ratated=rotation_matrix*v;
    cout<<"made a rotation by rotation_matrix:"<<endl;
    cout<<"(1, 0, 0) after rotation = "<<v_ratated.transpose()<<endl;

    //欧拉角：将旋转矩阵直接转换为欧拉角
    Eigen::Vector3d euler_angles=rotation_matrix.eulerAngles(2,1,0);    //参数2,1,0表示安装ZYX顺序，即yaw,pitch,roll顺序
    cout<<"rotation_matrix ---> eulerAngles:"<<endl;
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    //欧式变换矩阵使用Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();    //虽然是3d，实际上是4*4矩阵
    cout<<"Transform matrix :"<<endl;
    T.rotate(rotation_vector);  //进行旋转
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;
    T.pretranslate(Eigen::Vector3d(1, 3, 4));   //进行平移
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;

    //用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;    //这里的变换包括了上面的两种：旋转和平移
    cout<<"v transformed = "<<v_transformed.transpose()<<endl;

    //四元数的使用：可以直接把旋转向量赋值给四元数，反之亦然
    Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;    //coeffs的顺序为（x,y,z,w），前三者为虚部，w为实部

    //也可以把旋转矩阵赋值给四元数
    q=Eigen::Quaterniond(rotation_matrix);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;

    //使用四元数来旋转一个向量
    v_ratated=q*v;
    cout<<"(1, 0, 0) after rotation by quaternion = "<<v_ratated.transpose()<<endl;

    return 0;
}

运行该程序可以发现：通过旋转向量、旋转矩阵、四元数来表示旋转的计算结果是相同的，也就印证了上面讲的：这三种方式是等价的。

第三讲学习到这里，明天开始学习第四讲：李群与李代数。