史上最简SLAM零基础解读(8.1) - 旋转矩阵、旋转向量、欧拉角推导与相互转换

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

为了大家方便查询，先结论，再给出推导过程(四元数的相关知识，公式转换与过程推导在下一篇博客)
$$\begin{array}{lcccc}& 旋转矩阵& 旋转向量& 欧拉角& 四元数\\ 旋转矩阵& 1& & & \\ 旋转向量& & & & \\ 欧拉角& & & & \\ 四元数& & & & \end{array}$$

1、旋转矩阵→旋转向量、欧拉角：

2、旋转向量→旋转矩阵、欧拉角：

3、欧拉角→旋转矩阵、旋转向量：

 

二、旋转矩阵

关于旋转矩阵的推导，大家可以参考之前的博客：
史上最简SLAM零基础解读(1) - 旋转平移矩阵→欧式变换推导
这里就不再次进行讲解了
 

三、旋转向量→旋转矩阵

1、理论阅读

首先要了解什么是旋转向量，来看看《视觉SLAM十四讲》第二版第3讲(三维空间刚体运动)：有了旋转矩阵来描述旋转，有了变换矩阵描述一个 自由度的三维刚体运动，是不是已经足够了呢?矩阵表示方式至少有以下两个缺点:
        1.SO(3) 的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。同理，变换矩阵用 16 个量表达了 自由度的变换。那么，是否有更紧凑的表示呢?
        2. 旋转矩阵自身带有约束:它必须是个正交矩阵，且行列式为 。变换矩阵也是如此。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变得更困难。

因此，我们希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用一个六维向量表达变换，可行吗?事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角/角轴， Axis-Angle) ，只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。

考虑某个用 $\mathbf{R}$ 表示的旋转。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量 $\mathbf{n}$ 角度为 那么向量。"也可以描述这个旋转。于是，我们要问，两种表达方式之间有什么联系吗?事实上推导它们的转换关系并不难。从旋转向量到旋转矩阵的转换过程由罗德里格斯公式(Rodrigues’s Formula) 表明，由于推导过程比较复杂，这里不做描述，只给出转换的结果:
$$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }.\text{\hspace{1em}(01)}$$从这里，对于旋转矩阵有了大致的了解，那么下面就来推导以下 罗德里格斯公式 吧！
 

2、公式推导→基础简介

下面的推导摘录 https://krasjet.github.io/quaternion/quaternion.pdf，本人为了熟悉推导过程，所以抄写了一遍。

图一
$(1)$ 假设我们有一个经过原点的(如果旋转轴不经过原点我们可以先将旋转轴平移到原点，进行旋转，再平移回原处)，旋转轴 $\mathbf{u}=(x,y,z{)}^T$，我们希望将一个向量 $\mathbf{v}$，沿着这个旋转轴旋转 $\theta$ 度，变换到 ${\mathbf{v}}^{\prime }$, 如上图所示。

$(2)$ 注意，在这篇教程中我们将使用右手坐标系统，并且我们将使用右手定则来定义旋转的正方向，你可以将右手拇指指向旋转轴 $\mathbf{u}$ 的正方向，这时其它四个手指弯曲的方向即为旋转的正方向．在上图中即为逆时针方向。

$(3)$ 在轴角的表示方法中，一个旋转的定义需要使用到四个变量：旋转轴 $\mathbf{v}$ 的 $x,y,z$ 坐标，以及一个旋转角 $\theta$，也就是我们一共有四个自由度 (Degree of Freedom)。这很明显是多于欧拉角的三个自由度的，实际上，任何三维中的旋转只需要三个自由度就可以定义了，为什么这里我们会多出一个自由度呢？其实，在我们定义旋转轴 $\mathbf{v}$ 的 $x,y,z$ 坐标的同时，我们就定义了 $\mathbf{u}$ 的模长（长度）．然而，通常情况下，如果我们说绕着一个向量 $\mathbf{u}$ 旋转，我们其实指的是绕着 $\mathbf{u}$ 所指的方向进行旋转。回忆一下向量的定义：向量是同时具有大小和方向的量，但是在这里它的大小(长度)并不重要。我们可以说绕着 ${\mathbf{u}}_1=(0,0,1{)}^T$ 这个轴进行旋转，也可以说绕着 ${\mathbf{u}}_3=(0,0,3{)}^T$ 旋转，虽然这两个向量完全不同，但是它们指向的都是同一个方向（即 𝑧 轴的方向）：

在三维空间中定义一个方向只需要用到两个量就可以了(与任意两个坐标轴之间的夹角)，最简单的例子就是地球的经纬度，我们仅仅使用经度和纬度两个自由度就可以定义地球上任意一个方位。而如果我们要表示某一个方位上的特定一个点，则还需要添加海拔这个自由度。为了消除旋转轴 $\mathbf{u}$ 模长这个多余的自由度， 我们可以规定旋转轴 $\mathbf{u}$ 的模长为 $\Vert \mathbf{u}\Vert =\sqrt{x^2+y^2+z^2}=1$，也就是说 $\mathbf{u}$ 是一个单位向量，这样一来，空间中任意一个方向上的单位向量就唯一代表了这个方向．我们其实可以将模长规定为任意的常量，但是规定 $||\mathbf{u}||=1$ 能为我们之后的推导带来很多的便利，这也是数学和物理中对方向定义的惯例．在实际代码的实现中，你也可以让用户输入一个非单位长度的旋转轴向量 $\mathbf{u}$，但是在进行任何的计算之前你必须要记得先将它转化为一个单位向量：
$$\hat{\mathbf{u}}=\frac{\mathbf{u}}{\Vert \mathbf{u}\Vert }\text{\hspace{1em}(02)}$$

3、公式推导→旋转的分解

$(1)$ 有上面这个约束，我们现在就可以开始思考怎么样进行这个旋转了，首先，我们可以将 $\mathbf{v}$ 分解为平行于旋转轴 $\mathbf{u}$ 以及正交(垂直)于 $\mathbf{u}$ 的两个分量，${\mathbf{v}}_{||}$ 与 ${\mathbf{v}}_{\perp }$，即
$$\mathbf{v}={\mathbf{v}}_{\Vert }+{\mathbf{v}}_{\perp }\text{\hspace{1em}(03)}$$

$(2)$ 可以分别旋转这两个分向量，再将它们旋转的结果相加获得旋转后的向量:
$${\mathbf{v}}^{\prime }={\mathbf{v}}_{\Vert }^{\prime }+{\mathbf{v}}_{\perp }^{\prime }\text{\hspace{1em}(04)}$$下面是分解的示意图：

$(3)$可以看到，${\mathbf{v}}_{||}$ 其实就是 $\mathbf{v}$ 在 $\mathbf{u}$ 上的正交投影 (Orthogonal Projection)，根据正交投影的公式，我们可以得出：$$\begin{array}{rl}{\mathbf{v}}_{\Vert }& ={\mathrm{proj}}_{\mathbf{u}}(\mathbf{v})=\frac{\mathbf{u}\cdot \mathbf{v}}{\mathbf{u}\cdot \mathbf{u}}\mathbf{u}=\frac{\mathbf{u}\cdot \mathbf{v}}{\Vert \mathbf{u}{\Vert }^2}\mathbf{u}=(\mathbf{u}\cdot \mathbf{v})\mathbf{u}\end{array}\text{\hspace{1em}(05)}$$其中 $\Vert \mathbf{u}{\Vert }^2=\mathbf{u}\cdot \mathbf{u}$ 与 $\Vert \mathbf{u}\Vert =1$。再结合(03)式 $\mathbf{v}={\mathbf{v}}_{\Vert }+{\mathbf{v}}_{\perp }$，可以得到$${\mathbf{v}}_{\perp }=\mathbf{v}-{\mathbf{v}}_{\Vert }=\mathbf{v}-(\mathbf{u}\cdot \mathbf{v})\mathbf{u}\text{\hspace{1em}(06)}$$既然我们已经知道怎么分解 $\mathbf{v}$，接下来我们只需要分别讨论对 ${\mathbf{v}}_{||}$ 和 ${\mathbf{v}}_{\perp }$ 的旋转就可以了。
 

4、公式推导→${\mathbf{v}}_{||}$旋转

首先，我们来看一下平行于 $\mathbf{u}$ 的 ${\mathbf{v}}_{||}$，这种情况其实非常简单，从之前的图示中就可以看到，${\mathbf{v}}_{||}$ 其实根本就没有被旋转，仍然与旋转轴 $\mathbf{u}$ 重合，也就是说→当 ${\mathbf{v}}_{||}$ 平行于旋转轴 $\mathbf{u}$ 时，旋转 $\theta$ 角度之后的 ${\mathbf{v}}_{||}^{\prime }$ 为：$${\mathbf{v}}_{||}={\mathbf{v}}_{||}^{\prime }\text{\hspace{1em}(07)}$$

5、公式推导→${\mathbf{v}}_{\perp }$ 旋转

接下来，我们需要处理正交于 $\mathbf{u}$ 的 ${\mathbf{v}}_{\perp }$．因为这两个向量是正交的，这个旋转可以看做是平面内的一个旋转。因为旋转不改变 ${\mathbf{v}}_{\perp }$ 的长度，所以路径是一个圆．下面是这个旋转的示意图，右侧的为俯视图：

现在，3D 的旋转就被我们转化为了 2D 平面上的旋转．由于在这个平面上我们只有一个向量 ${\mathbf{v}}_{\perp }$，用它来表示一个旋转是不够的，我们还需要构造一个同时正交于 $\mathbf{u}$ 和 ${\mathbf{v}}_{\perp }$ 的向量 $\mathbf{w}$，这个可以通过叉乘来获得：
$$\mathbf{w}=\mathbf{u}\times {\mathbf{v}}_{\perp }\text{\hspace{1em}(08)}$$注意叉乘的顺序，因为我们使用的是右手坐标系统，按照右手定则你可以发现这个新的向量 $\mathbf{w}$ 指向 ${\mathbf{v}}_{\perp }$ 的
逆时针旋转 $\pi /2$ 后的方向，并且和 ${\mathbf{v}}_{\perp }$ 一样也处于正交于 $\mathbf{u}$ 的平面内。因为 $\Vert \mathbf{u}\Vert =1$，我们可以发现:
$$\begin{array}{rl}\Vert \mathbf{w}\Vert & =\Vert \mathbf{u}\times {\mathbf{v}}_{\perp }\Vert \\ & =\Vert \mathbf{u}\Vert \cdot \Vert {\mathbf{v}}_{\perp }\Vert \cdot \sin (\pi /2)\\ & =\Vert {\mathbf{v}}_{\perp }\Vert \end{array}\text{\hspace{1em}(09)}$$其上的 $\pi /2$ 是 $\mathbf{u}$ 和 ${\mathbf{v}}_{\perp }$ 的夹角。也就是说 $\mathbf{w}$ 和 ${\mathbf{v}}_{\perp }$ 的模长是相同的，所以，$\mathbf{w}$ 也位于圆上．有了这个新的向量 $\mathbf{w}$，就相当于我们在平面内有了两个坐标轴．我们现在可以把 ${\mathbf{v}}_{\perp }^{\prime }$ 投影到 $\mathbf{w}$ 和 ${\mathbf{v}}_{\perp }$ 上。将其分解为 ${\mathbf{v}}_v^{\prime }$ 和 ${\mathbf{v}}_w^{\prime }$。使用一点三角学的知识我们就能得到：
$$\begin{array}{rl}{\mathbf{v}}_{\perp }^{\prime }& ={\mathbf{v}}_v^{\prime }+{\mathbf{v}}_w^{\prime }\\ & =\cos (\theta ){\mathbf{v}}_{\perp }+\sin (\theta )\mathbf{w}\\ & =\cos (\theta ){\mathbf{v}}_{\perp }+\sin (\theta )\left(\mathbf{u}\times {\mathbf{v}}_{\perp }\right)\end{array}\text{\hspace{1em}(10)}$$这也就完成了旋转的第二步，我们可以得到这样一个定理：当 ${\mathbf{v}}_{\perp }$ 正交于旋转轴 $\mathbf{u}$ 时，旋转 $\theta$ 角度之后的 ${\mathbf{v}}_{\perp }^{\prime }$ 为：
$${\mathbf{v}}_{\perp }^{\prime }=\cos (\theta ){\mathbf{v}}_{\perp }+\sin (\theta )\left(\mathbf{u}\times {\mathbf{v}}_{\perp }\right)\text{\hspace{1em}(11)}$$

6、公式推导→$\mathbf{v}$的旋转

将上面的两个结果组合就可以获得，也就是根据公式(04)(07)(11),可以得到如下公式：$$\begin{array}{rl}{\mathbf{v}}^{\prime }& ={\mathbf{v}}_{\Vert }^{\prime }+{\mathbf{v}}_{\perp }^{\prime }\\ & ={\mathbf{v}}_{\Vert }+\cos (\theta ){\mathbf{v}}_{\perp }+\sin (\theta )\left(\mathbf{u}\times {\mathbf{v}}_{\perp }\right).\end{array}\text{\hspace{1em}(12)}$$因为叉乘遵守分配律,则：$$\begin{array}{rl}\mathbf{u}\times {\mathbf{v}}_{\perp }& =\mathbf{u}\times \left(\mathbf{v}-{\mathbf{v}}_{\Vert }\right)\\ & =\mathbf{u}\times \mathbf{v}-\mathbf{u}\times {\mathbf{v}}_{\Vert }\\ & =\mathbf{u}\times \mathbf{v}\end{array}\text{\hspace{1em}(13)}$$
因为 $\mathbf{u}$ 平行于 ${\mathbf{v}}_{\Vert }$，所以 $\mathbf{u}\times {\mathbf{v}}_{\Vert }=0$，最后将${\mathbf{v}}_{\Vert }=(\mathbf{u}\cdot \mathbf{v})\mathbf{u}$ 与 ${\mathbf{v}}_{\perp }=\mathbf{v}-(\mathbf{u}\cdot \mathbf{v})\mathbf{u}$ 带入到 (12) 式，可以得到：
$$\begin{array}{rl}{\mathbf{v}}^{\prime }& =(\mathbf{u}\cdot \mathbf{v})\mathbf{u}+\cos (\theta )(\mathbf{v}-(\mathbf{u}\cdot \mathbf{v})\mathbf{u})+\sin (\theta )(\mathbf{u}\times \mathbf{v})\\ & =\cos (\theta )\mathbf{v}+(1-\cos (\theta ))(\mathbf{u}\cdot \mathbf{v})\mathbf{u}+\sin (\theta )(\mathbf{u}\times \mathbf{v})\end{array}\text{\hspace{1em}(14)}$$
这样我们就得到了一般形式的旋转公式→3D 空间中任意一个 $\mathbf{v}$ 沿着单位向量 $\mathbf{u}$ 旋转 $\theta$ 角度之后的 ${\mathbf{v}}^{\prime }$ 为：
$${\mathbf{v}}^{\prime }=\cos (\theta )\mathbf{v}+(1-\cos (\theta ))(\mathbf{u}\cdot \mathbf{v})\mathbf{u}+\sin (\theta )(\mathbf{u}\times \mathbf{v})\text{\hspace{1em}(15)}$$不过呢，在 slam 中上面这个这个公式通常使用矩阵的形式表示，这里令 ${\mathbf{v}}^{\prime }=\mathbf{Rv}$，然后等式两边都除以 $\mathbf{v}$，那么并且其上 $\mathbf{u}$ 的叉乘，写成 ${\mathrm{u}}^{\wedge }$ 的形式，那么可以得到
$$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{u}{\mathbf{u}}^{\mathrm{T}}+\sin \theta {\mathbf{u}}^{\wedge }.\text{\hspace{1em}(16)}$$把上式的 $\mathbf{u}$ 换成 $\mathbf{u}$ 便可以得到(01)式，这样就完成了推导。
 

四、旋转矩阵 →旋转向量

根据上面的推导，获得了： $$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }.\text{\hspace{1em}(01)}$$如果要根据旋转矩阵求得旋转向量，也就是要求单位方向向量 $\mathbf{n}$，以及围绕该方向向量旋转的角度 $\theta$，对于角度 $\theta$ 还是很好求的，对矩阵的两边都求迹：
$$\begin{array}{rl}\mathrm{tr}(\mathbf{R})& =\cos \theta \mathrm{tr}(\mathbf{I})+(1-\cos \theta )\mathrm{tr}\left(\mathbf{n}{\mathbf{n}}^T\right)+\sin \theta \mathrm{tr}\left({\mathbf{n}}^{\wedge }\right)\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta .\end{array}\text{\hspace{1em}(17)}$$$$\theta =\arccos \left(\frac{\mathrm{tr}(\mathbf{R})-1}{2}\right)\text{\hspace{1em}(18)}$$那么剩下的就是要求旋转轴 $\mathbf{n}$ (单位方向向量)，需要注意的是 $旋转轴上的向量在旋转之后不发生改变$，也就是成立如下公式等式：
$$\mathbf{Rn}=\mathbf{n}=1\mathbf{n}\text{\hspace{1em}(19)}$$上式表达的含义就是矩阵 $\mathbf{R}$ 作用在向量 $\mathbf{n}$上其等价于数字 $1$ 作用在向量 $\mathbf{n}$ 上，该特点表示向量 $\mathbf{n}$ 就是 $\mathbf{R}$ 进行特征分解后，特征值1对应的特征向量，求得该特征向量再进行归一化即可。