视觉SLAM理论到实践系列（二）——三维物体的刚体运动（1）

视觉SLAM理论到实践系列

视觉SLAM理论到实践系列（一）——SLAM概述与预备知识

视觉SLAM理论到实践系列（二）——三维物体的刚体运动（1）

视觉SLAM理论到实践系列（二）——三维物体的刚体运动（2）

下面是《视觉SLAM十四讲》学习笔记的系列记录的总链接，本人发表这个系列的文章链接均收录于此

视觉SLAM理论到实践系列文章链接

---

---

前言

高翔博士的《视觉SLAM14讲》学习笔记的系列记录

---

视觉SLAM理论到实践系列（二）——三维物体的刚体运动（1）

点与坐标系

2D的情况：用两个坐标加旋转角表达

3D 的情况？

• 向量的运算可由坐标运算表达

• 加法和减法

• 内积

  

• 外积

  

注意

这样就把$a\times b$写成了矩阵与向量的乘法$a^{\wedge }b$，把它变成了线性运算。这个符号将在后文经常用到，请记住它，
并且此符号是一个一一映射，意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然：
$$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],$$

这个符号本质上是叉乘

反对称矩阵的性质

参考：

矩阵运算_反对称矩阵性质_axb = [a]xb

1.反对称矩阵的基本性质
$$[\mathbf{a}{]}^{\wedge }=-[\mathbf{a}{]}^{\wedge T}$$

2.叉乘的基本性质，叉乘顺序互换，叉乘结果大小不变，方向相反
$$[\mathbf{a}{]}^{\wedge }\mathbf{b}=-[\mathbf{b}{]}^{\wedge }\mathbf{a}$$

3.叉乘的基本性质，向量与自己叉乘等于零向量
$$[a{]}^{\wedge }a=0$$

4.叉乘的基本性质，向量叉乘的结果同时垂直于叉乘的两个向量
$$b^T[a{]}^{\wedge }\mathbf{b}=0$$

5.混合积公式，标量三重积，
$$\begin{array}{rl}& \mathbf{a}\cdot (\mathbf{b}\times \mathbf{c})=\mathbf{b}\cdot (\mathbf{c}\times \mathbf{a})=\mathbf{c}\cdot (\mathbf{a}\times \mathbf{b})\\ & \\ & {\mathbf{a}}^T[\mathbf{b}{]}^{\wedge }\mathbf{c}={\mathbf{b}}^T[\mathbf{c}{]}^{\wedge }\mathbf{a}={\mathbf{c}}^T[\mathbf{a}{]}^{\wedge }\mathbf{b}\end{array}$$

6.向量三重积，
$$a\times (b\times c)=b(a\cdot c)-c(a\cdot b)$$
写成矩阵相乘形式有
$$[\mathbf{a}{]}^{\wedge }[\mathbf{b}{]}^{\wedge }\mathbf{c}=\mathbf{b}\left({\mathbf{a}}^T\mathbf{c}\right)-\left({\mathbf{a}}^T\mathbf{b}\right)\mathbf{c}=\left(\mathbf{b}{\mathbf{a}}^T\right)\mathbf{c}-\left({\mathbf{a}}^T\mathbf{b}\right)\mathbf{c}=\left(\mathbf{b}{\mathbf{a}}^T-{\mathbf{a}}^T\mathbf{b}{I}_3\right)\mathbf{c}$$
由于对于任意的三维向量 $c$ 都成立，所以有
$$[\mathbf{a}{]}^{\wedge }[\mathbf{b}{]}^{\wedge }=\mathbf{b}{\mathbf{a}}^T-{\mathbf{a}}^T\mathbf{b}{I}_3$$

7.二次幂公式，性质6的一个特例
$$[\mathbf{a}{]}^{\wedge }[\mathbf{a}{]}^{\wedge }=\mathbf{a}{\mathbf{a}}^T-{\mathbf{a}}^T\mathbf{a}{I}_3\Rightarrow [\mathbf{a}{]}^{{\wedge }^2}=\mathbf{a}{\mathbf{a}}^T-\Vert \mathbf{a}{\Vert }_2^2{I}_3$$

8.三次幂公式和特征值，在性质7上两边各左乘一个$[\mathbf{a}{]}^{\wedge }$，可以得到 $[\mathbf{a}{]}^{\wedge }$ 的零化多项式。
$$\begin{array}{rl}& [\mathbf{a}{]}^{{\wedge }^3}=-\Vert \mathbf{a}{\Vert }_2^2[\mathbf{a}{]}^{\wedge }\\ & \\ & {\lambda }^3+\Vert \mathbf{a}{\Vert }_2^2\lambda =0\end{array}$$
因此可以得到 $[\mathbf{a}{]}^{\wedge }$ 的特征值为 ${\lambda }_1=0$，以及 ${\lambda }_{2、3}=i\Vert a{\Vert }^2$，反对称矩阵的特征值有一个是0,另外两个是相等的虚数。特征值0对应的特征向量为 $\mathbf{a}$，因为有 $[\mathbf{a}{]}^{\wedge }\mathbf{a}=0\mathbf{a}$

如果 $\Vert \mathbf{a}\Vert =1$，则有

$$\begin{gathered} [\mathbf{a}{]}^{\wedge }[\mathbf{a}{]}^{\wedge }=\mathbf{a}{\mathbf{a}}^T-I \\ [\mathbf{a}{]}^{{\wedge }^3}=-[\mathbf{a}{]}^{\wedge } \end{gathered}$$

9.当a不为零向量时
$$Rank([\mathbf{a}{]}^{\wedge })=2$$

10.零空间

由性质3、性质8和性质9可知，$Rank([\mathbf{a}{]}^{\wedge })=2$， $[\mathbf{a}{]}^{\wedge }$必有一维零空间，且 $\mathbf{a}$ 是其中一个解，于是
$$Null\left([\mathbf{a}{]}^{\wedge }\right)=Span\left(\mathbf{a}\right)$$

11.旋转矩阵的伴随性质，当 $U$ 是一个旋转矩阵，有
$$[\mathbf{Ua}{]}^{\wedge }=\mathbf{U}[\mathbf{a}{]}^{\wedge }{\mathbf{U}}^T$$

12.三阶反对称矩阵可以分解为
$$S=[\mathbf{a}{]}^{\wedge }=k\mathbf{UZ}{\mathbf{U}}^T$$
$S$ 为反对称矩阵，$k$ 为常数，$U$ 为正交矩阵，$Z=[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}]$

旋转矩阵

考虑一次旋转

• 坐标系 $(e_1,e_2,e_3)$ 发生了旋转，变成 $(e_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }})$

• 向量 $a$ 不动，那么它的坐标如何变化？

R 称为旋转矩阵

可以验证：

• R是一个正交矩阵；
• R的行列式为+1。

满足这两个性质的矩阵称为旋转矩阵
S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } SO(n)=\{\boldsymbol{R}\in\mathbb{R}^{n\times n}|\boldsymbol{RR}^T=\boldsymbol{I},\det(\boldsymbol{R})=1\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
$SO(n)$ 是Special Orthogonal Group 特殊正交群

这个集合由维空间的旋转矩阵组成，特别地，$SO(3)$ 就是指三维空间的旋转。通过旋转矩阵，我们可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起。

注意
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_2^T{e}_3^{{}^{\prime }}\\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]\triangleq \mathbf{R}{\mathbf{a}}^{{}^{\prime }}$$
于是，2到1的旋可表达为：
$$a_1=R_{12}{a}_2$$
反之：
$$a_2=R_{21}{a}_1$$
矩阵关系：
$$R_{21}=R_{12}^{-1}=R_{12}^T$$

旋转加平移
$$a^{{}^{\prime }}=Ra+t$$
其中，$t$ 称为平移向量。相比于旋转，平移部分只需把平移向量加到旋转之后的坐标上，非常简单。通过上式，我们用一个旋转矩阵R和一个平移向量t完整地描述了一个欧氏空间的坐标变换关系。实际当中，我们会定义坐标系1、坐标系2，那么向量a在两个坐标系下的坐标为 $a1,a2$ ，它们之间的关系应该是：
$${\mathbf{a}}_1=R_{12}{\mathbf{a}}_2+t_{12}$$

这里的 $R_{12}$是指 把坐标系2的向量变换到坐标系1 中。由于向量乘在这个矩阵的右边，它的下标是 从右读到左 的。这也是SLAM 14讲中的习惯写法。

坐标变换很容易搞混，特别是存在多个坐标系的情况下。同理，如果我们要表达“从1到2的旋转矩阵”时，就写成 $R_{21}$。

关于平移 $t_{12}$,它实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，所以建议读者把它记作“从1到2的向量”。但是反过来的$t_{21}$，即从2指向1的向
量在坐标系2下的坐标，却并不等于$-t_{12}$，而是和两个系的旋转还有关系¹。所以，当初学者问“我的坐标在哪里”这样的问题时，我们需要清楚地说明这句话的含义。这里“我的坐标”实际上指从世界坐标系指向自己坐标系原点的向量，在世界坐标系下取到的坐标。对应到数学符号上，应该是 $t_{WC}$ 的取值。同理，它也不是 $t_{CW}$。

• 两个坐标系间的运动可用R,t完全描述
• 欧拉定理（Euler’s rotation theorem）：刚体在三维空间里的一般运动，可分解为刚体上方某一点的平移，以及绕经过此点的旋转轴的转动。

旋转矩阵左右乘的意义

参考：

旋转矩阵及左右乘的意义，看这一篇就够了

可能大家都听到过，左乘旋转矩阵绕固定坐标系旋转，右乘旋转矩阵绕自身坐标系旋转。

单纯记这句话很容易让人产生误解！

首先，这里的左右乘指的并不是某一向量左乘或者右乘旋转矩阵，而是多个旋转矩阵的组合方式是左乘还是右乘。

其次，这句话还遗漏了一个相当重要的信息，绕固定坐标系旋转讨论的是向量的旋转，绕自身坐标系旋转讨论的是坐标变换！这是完全不一样的两种功能，例如我想研究向量的旋转，我可以也只可以将其视为向量绕固定坐标系的旋转。往下看。

1.旋转矩阵左乘

我想要一个向量 $p_0$ 先绕 $x$ 轴转 $\alpha$ 度，再绕 $y$ 轴转 $\beta$ 度，最后绕 $z$ 轴转 $\gamma$ 度，求旋转后向量的位置。

可见这是一个旋转矩阵组合的问题，之前讨论过，向量绕某一坐标轴旋转在数学上展现为向量左乘旋转矩阵，我们按照题意，

当然是先让它绕 $x$轴旋转：
$$\overrightarrow{p_1}=M_x(\alpha )\times \overrightarrow{p_0}$$
再绕 $y$ 轴旋转：
$$\overrightarrow{p_2}=M_y(\beta )\times \overrightarrow{p_1}$$

再绕 $z$ 轴旋转，那么 $p_3$ 即为所求向量：
$$\overrightarrow{p_3}=M_z(\gamma )\times \overrightarrow{p_2}$$
连起来就是：

$$\overrightarrow{p_3}=M_z(\gamma )\times M_y(\beta )\times M_x(\alpha )\times \overrightarrow{p_0}$$
总的旋转矩阵
$$R=M_z(\gamma )\times M_y(\beta )\times M_x(\alpha )$$

重点来了，让向量 $p0$ 先后绕 $x$、$y$、$z$ 轴旋转，其总的旋转矩 $R$ 为绕 $x$ 轴旋转的矩阵左乘绕 $y$ 轴旋转的矩阵再左乘绕 $z$ 轴旋转的矩阵。

由此才引入的那句话，左乘绕固定坐标系旋转，现在我们来改一下，应该这么说 ：讨论向量旋转问题时，认为向量绕固定坐标轴旋转，旋转矩阵按旋转次序依次左乘，得到最终的旋转矩阵R。

2.旋转矩阵右乘

右乘讨论的就是坐标变换了，来看问题：已知在 $C$ 坐标系下表示的向量 $p_C$，我想求 $p_C$ 在原始坐标系0下的表示 $p_0$，已知 $C$ 坐标系由原始坐标系0绕轴 $X_0$ 旋转 $\alpha$ 度成为坐标系 $A$，再绕轴 $Y_A$旋转成为坐标系 $B$，再绕轴 $Z_B$旋转成为坐标系 $C$。

再复习一遍，$p$ 点在 $B$ 坐标系下的坐标，乘以旋转矩阵，得到的是在 $A$ 坐标系下的坐标，而 $B$ 坐标系是由 $A$ 坐标系转了 $\alpha$ 角转过去的，可以发现，这是一种“退回”效果，也就是 $p$ 点在旋转后的坐标系下的坐标左乘旋转矩阵后，会变为旋转前坐标系下的坐标，这种效果表明旋转后的坐标系一定是绕旋转前坐标系的某一轴转过去的，对于旋转前的坐标系，可以理解为绕自身坐标系的坐标轴旋转后成了旋转后的坐标系。

换句话说，因为你退回来得到的是前坐标系下的坐标，你转过去也必须是绕前坐标系中的坐标轴转过去的，故我们称之为绕自身坐标系旋转（感觉绕的话就多读两遍）这里一定要懂。

再回过来看，咱不是已知 $p_C$ 求 $p_0$ 吗，那咱们就一步一步往回退，退到坐标系0为止： 首先由坐标系 $C$ 退回到坐标系 $B$，(应该说 $p$ 向量在 $C$ 系中的表示变换为在 $B$ 系中的表示) ：

$$\overrightarrow{p_B}=M_Z(\gamma )\times \overrightarrow{p_C}$$
再由坐标系 $B$ 退回到坐标系 $A$：

$$\overrightarrow{p_A}=M_Y(\beta )\times \overrightarrow{p_B}$$
再由坐标系 $A$ 退回到坐标系0：

$$\overrightarrow{p_0}=M_X(\alpha )\times \overrightarrow{p_A}$$
连起来为：

$$\overrightarrow{p_0}=M_X(\alpha )\times M_Y(\beta )\times M_Z(\gamma )\times \overrightarrow{p_C}$$
总的旋转矩阵 $R$ 为：

$$R=M_X(\alpha )\times M_Y(\beta )\times M_Z(\gamma )$$
之前说过坐标系 $C$ 是由坐标系0经过绕轴 $X_0$ 变为坐标系 $A$，再绕轴 $Y_A$ 变为坐标系 $B$，再绕轴 $Z_B$ 旋转才得到的，为了使其退回去，看看旋转矩阵 $R$ 的写法，是不是绕 $x$ 轴旋转矩阵右乘 $y$ 轴旋转旋转右乘 $z$ 轴旋转矩阵？这就是我们所说的绕自身坐标轴旋转要右乘旋转矩阵。

补充

参考：

二维（三维）坐标系中旋转矩阵详细推导

3维旋转矩阵推导与助记

二维旋转矩阵

$$M(\theta )=\left[\begin{array}{ccc}\cos \theta & & -\sin \theta \\ \sin \theta & & \cos \theta \end{array}\right]=\cos \theta \left[\begin{array}{ccc}1& & 0\\ 0& & 1\end{array}\right]+\sin \theta \left[\begin{array}{ccc}0& & -1\\ 1& & 0\end{array}\right]=\exp \left(\theta \left[\begin{array}{ccc}0& & -1\\ 1& & 0\end{array}\right]\right)$$

$$\begin{array}{c}{x}^{\prime }=\cos \theta \cdot x-\sin \theta \cdot y\\ y^{\prime }=\sin \theta \cdot x+\cos \theta \cdot y\\ \Rightarrow \left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]\end{array}$$

因此二维坐标系下逆时针旋转矩阵 $\mathbf{R}$ 为：
$$R=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

对于单位矩阵，绕哪个轴旋转，哪一列不用变，然后将二维旋转矩阵替换对应的4个位置，注意，绕Y的旋转矩阵看起来与另外两个不同，它的 $-\sin \beta$ 是在左下

变换矩阵与齐次坐标

旋转加平移在表达复合情况下有不便之处：

这样的形式在变换多次之后会显得很啰嗦。因此，我们引入齐次坐标和变换矩阵
$$\left[\begin{array}{l}{a}^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{ll}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{l}a\\ 1\end{array}\right]\triangleq \mathbf{T}\left[\begin{array}{l}a\\ 1\end{array}\right]$$
这是一个数学技巧：我们在一个三维向量的末尾添加1，将其变成了四维向量，称为齐次坐标（Homogeneous）。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系。该式中，矩阵T称为变换矩阵(Transform Matrix)

我们暂时用 $\tilde{a}$ 表示 $a$ 的齐次坐标，即
$$\tilde{a}=\left[\begin{array}{l}a\\ 1\end{array}\right]$$
乘任意非零常数时仍表达同一坐标

$$\tilde{a}=\left[\begin{array}{c}a\\ 1\end{array}\right]=k\left[\begin{array}{c}a\\ 1\end{array}\right]$$

那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式：
$$\tilde{b}=T_1\tilde{a},\tilde{c}=T_2\tilde{b}\Rightarrow \tilde{c}=T_2{T}_1\tilde{a}$$
关于变换矩阵T,它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1。这种矩阵的集合又称为特殊欧氏群(Special Euclidean Group):
$$\mathrm{SE}(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in \mathrm{SO}(3),t\in {\mathbb{R}}^3\right\}.$$
与 $SO(3)$ 一样，求解该矩阵的逆表示一个反向的变换：
$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$
同样，我们用 $T_{12}$ 这样的写法来表示从2到1的变换。并且，为了保持符号的简洁，在不引起歧义的情况下，以后不刻意区别齐次坐标与普通坐标的符号，默认使用的是符合运算法则的那一种。

例如，当我们写 $Ta$ 时，使用的是齐次坐标（不然没法计算）。而写 $Ra$ 时，使用的是非齐次坐标。如果写在一个等式中，就假设齐次坐标到普通坐标的转换是已经做好了的一因为齐次坐标和非齐次坐标之间的转换事实上非常容易，而在C++程序中你可以使用运算符重载来完成这个功能，保证在程序中看到的运算是统一的。

旋转向量和欧拉角

旋转向量

除了旋转矩阵/变换矩阵之外，还存在其他的表示方式

旋转矩阵 R 有九个元素，但仅有三个自由度

能否以更少的元素表达旋转？

我们重新回到理论部分。有了旋转矩阵来描述旋转，有了变换矩阵描述一个6自由度的三维
刚体运动，是不是已经足够了呢？矩阵表示方式至少有以下两个缺点：

1. $SO(3)$的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。
   同理，变换矩阵用16个量表达了6自由度的变换。那么，是否有更紧凑的表示呢？
2. 旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此（指自身带有约束，因为变换矩阵不是正交阵）。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变得更困难。

因此，我们希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用一个六维向量表达变换，可行吗？事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。

于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量（或轴角/角轴，Axis-Angle),只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。

考虑某个用 $R$ 表示的旋转。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量 $n$，角度为 $\theta$，那么向量 $\theta n$ 也可以描述这个旋转。于是，我们要问，两种表达方式之间有什么联系吗？事实上推导它们的转换关系并不难。从旋转向量到旋转矩阵的转换过程由罗德里格斯公式(Rodrigues’s Formula)表明²：
$$R=\cos \theta I+(1-\cos \theta ){nn}^{\text{T}}+\sin \theta n^{\wedge }.$$
符号 $\wedge$ 是向量到反对称矩阵的转换符，见 转换符。

反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 $\theta$，取两边的迹，有
$$\begin{array}{rl}tr\left(R\right)& =\cos \theta tr\left(I\right)+\left(1-\cos \theta \right)\mathrm{tr}\left(n{n}^T\right)+\sin \theta \mathrm{tr}\left(n^{\Lambda }\right)\\ & =3\cos \theta +\left(1-\cos \theta \right)\\ & =1+2\cos \theta \end{array}$$
因此
$$\theta =\arccos \frac{\mathrm{tr}(R)-1}{2}.$$
关于转轴，旋转轴上的向量在旋转后不发生改变，说明：
$$Rn=n$$
因此，转轴是矩阵R特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

旋转向量

• 方向为旋转轴，长度为转过的角度
• 称为角轴/轴角（Angle Axis）或旋转向量（Rotation Vector）

欧拉角

欧拉角（Euler Angles）

• 将旋转分解为三个方向上的转动
• 例，按Z-Y-X顺序转动
• 轴可以是定轴或动轴，顺序亦可不同
• 常见的有：yaw-pitch-roll，东北天
• 不同领域的习惯有所不同

假设一个刚体的前方（朝向我们的方向）为X轴，右侧为Y轴，上方为Z轴，如下图所示。那么，ZYX转角相当于把任意旋转分解成以下3个轴上的转角：

1. 绕物体的Z轴旋转，得到偏航角yaw；
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch；
3. 绕旋转之后的X轴旋转，得到滚转角roll。

欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)：在俯仰角为±90°时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转)。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。

理论上可以证明，只要想用3个实数来表达三维旋转，都会不可避免地碰到奇异性问题³。由于这种原理，欧拉角不适用于插值和迭代，往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性）。

不过，若你想验证自己的算法是否有错，转换成欧拉角能够帮你快速分辨结果是否正确。在某些主体主要为2D运动的场合(例如扫地机、自动驾驶车辆)，我们也可以把旋转分解成三个欧拉角，然后把其中一个（例如偏
航角)拿出来作为定位信息输出。

万向锁（Gimbal Lock）

参考：无伤理解欧拉角中的“万向死锁”现象

• 欧拉角的奇异性问题
• 在特定值时，旋转自由度减1；
• Yaw-pitch-roll顺序下，当pitch为90度时，存在奇异性

先转的轴会带动后转的轴运动，所以你每次旋转都必须基于初始状态，而不能在一个旋转结果上叠加旋转，否则就会出现被死锁的风险

避免死锁的方法：第二次转动的轴的角度不为90度即可

四元数

参考：

四元数的可视化

四元数和三维转动，可互动的探索式视频

更详细的解释，请看https://eater.net/quaternions

四元数与三维旋转 （quaternion.pdf ）

https://github.com/CatOnly/CrashNote/tree/master/LinearAlgebra（[LinearAlgebra](books and docs\CrashNotes-master\LinearAlgebra)）

四元数定义

旋转矩阵用9个量描述3自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。

事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于用两个坐标表示地球表面（如经度和纬度），将必定存在奇异性（纬度为±90°时经度无意义）。

我们用复数集C表示复平面上的向量，而复数的乘法则表示复平面上的旋转：例如，乘上复数i相当于逆时针把一个复向量旋转90°。

类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数(Quaternion)。四元数是Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。

把四元数与复数类比可以帮助你更快地理解四元数。例如，当我们想要将复平面的向量旋转 $\theta$ 角时，可以给这个复向量乘以 $e^{i\theta }$。这是极坐标表示的复数，它也可以写成普通的形式，只要使用欧拉公式即可：
$${\mathrm{e}}^{i\theta }=\cos \theta +i\sin \theta .$$
这正是一个单位长度的复数。所以，在二维情况下，旋转可以由单位复数来描述。
$$z=x+iy=\rho e^{i\theta }$$
乘 $i$ 即转90度，乘 $-i$ 转 -90度

类似地，我们会看到，三维旋转可以由单位四元数来描述。

一个四元数q拥有一个实部和三个虚部
$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中，$i,j,k$为四元数的三个虚部。这三个虚部满足以下关系式：
$$\{\begin{array}{l}{\text{i}}^2=j^2=k^2=-1\\ \text{j}=\text{k},\text{ji}=-\text{k}\\ \text{jk}=\text{i},\text{kj}=-i\\ \text{ki}=j,\text{ik}=-j\end{array}.$$
如果把 $i,j,k$ 看成三个坐标轴，那么它们与自己的乘法和复数一样，相互之间的乘法和外积一样，即类似于叉乘。

有时，也用一个标量和一个向量来表达四元数：
$$q=[s,v{]}^{\mathsf{T}},s=q_0\in \mathbb{R},v=[q_1,q_2,q_3{]}^{\mathsf{T}}\in {\mathbb{R}}^3$$
这里，s称为四元数的实部，而v称为它的虚部。如果一个四元数的虚部为0，则称为实四元数；反之，若它的实部为0，则称为虚四元数。

四元数的运算

四元数和通常的复数一样，可以进行一系列的运算。常见的有四则运算、共轭、求逆、数乘等。

现有两个四元数 $q_a,q_b$，它们的向量表示为 $[s_a,v_a{]}^T,[s_b,v_b{]}^T$,或者原始四元数表示为
$$q_a=s_a+z_a\mathrm{i}+y_a\mathrm{j}+z_a\mathrm{k},q_b=s_b+{\mathrm{x}}_0\mathrm{i}+y_b\mathrm{j}+z_0\mathrm{k}$$
那么，其运算可表示如下。

（1）加法和减法

四元数 $q_a,q_b$ 的加减运算为
$$q_a\pm q_b={\left[s_a\pm s_b,v_a\pm v_b\right]}^T$$

（2）乘法

乘法是把 $q_a$ 的每一项与 $q_b$ 的每项相乘，最后相加，虚部要按照乘法公式进行。整理可得
$$\begin{array}{rl}{q}_a{q}_b=& y_a{y}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +\left(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b\right)i\\ & +\left(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b\right)j\\ & +\left(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b\right)k\end{array}$$
虽然稍为复杂，但形式上是整齐有序的。如果写成向量形式并利用内外积运算，该表达会更加简洁：
$$q_a{q}_b={\left[s_a{s}_b-v_a^{\mathrm{T}}{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b\right]}^{\mathrm{T}}$$
在该乘法定义下，两个实四元数乘积仍是实的，这与复数是一致的。然而，我们注意到由于最后一项外积的存在，四元数乘法通常是不可交换的，除非 $v_a$ 和 $v_b$ 在 ${\mathbb{R}}^3$ 中共线，此时外积项为零。

四元数 $q_1$ 乘以 $q_2$ ，会把 $q_2$ 以 $q_1$ 的大小放缩，然后再施加一种特殊的四维的旋转

（3）模长

四元数的模长定义为
$$\Vert q_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$
可以验证，两个四元数乘积的模即模的乘积。这使得单位四元数相乘后仍是单位四元数。
$$\Vert q_a{q}_b\Vert =\Vert q_a\Vert \Vert q_b\Vert .$$

（4）共轭

四元数的共轭是把虚部取成相反数：
$$q_a^{\ast }=s_a-x_{a}i-y_{k}j-z_{a}k=[s_k,-{\nu }_a{]}^{\top }$$
四元数共轭与其本身相乘，会得到一个实四元数，其实部为模长的平方：
$$q^{\ast }q=q{q}^{\ast }=[s_a^2+v^{\mathrm{T}}v,0{]}^{\top }.$$

（5）逆

一个四元数的逆为
$$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2.}$$
按此定义，四元数和自己的逆的乘积为实四元数1：
$$q{q}^{-1}=q^{-1}q=1.$$
如果 $q$ 为单位四元数，其逆和共轭就是同一个量。同时，乘积的逆具有和矩阵相似的性质：
$$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}.$$

（6）数乘

和向量相似，四元数可以与数相乘：
$$\mathbf{k}q=[\mathbf{k}s,\mathbf{k}v{]}^{\mathsf{T}}$$

四元数到角轴：
$$q={\left[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}\right]}^T$$

角轴到四元数：
$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^T={\left[q_1,q_2,q_3\right]}^T/\sin \frac{\theta }{2}\end{array}$$

用四元数表示旋转

我们可以用四元数表达对一个点的旋转。假设有一个空间三维点 $p=[x,y,z]\in {\mathbb{R}}^3$ ，以及一个由单位四元数 $q$ 指定的旋转。三维点 $p$ 经过旋转之后变为 $p^{\prime }$ 。如果使用矩阵描述，那么有 $p^{\prime }=Rp$ 。而如果用四元数描述旋转，它们的关系又如何表达呢？

首先，把三维空间点用一个虚四元数来描述：
$$p={\left[0,x,y,z\right]}^T={\left[0,v\right]}^T$$
相当于把四元数的3个虚部与空间中的3个轴相对应。那么，旋转后的点p可表示为这样的乘积：
$$p^{\prime }=qp{q}^{-1}$$
这里的乘法均为四元数乘法，结果也是四元数。最后把 $p^{\prime }$ 的虚部取出，即得旋转之后点的坐标。

并且，可以验证（留作习题），计算结果的实部为0，故为纯虚四元数。

使用

使用四元数 $q$ 对点 $p$ 进行变换，得到四维点 $p^{\prime }$ ，将后三维取出即为坐标
$$p^{\prime }=qp{q}^{-1}$$
虽然数学实质如上所述，但是Eigen已经将四元数的旋转运算重载，平时只需要这么算就行了
$$p^{\prime }=q\ast p$$

补充

（1）四元数好用难理解，欧拉角难用好理解

（2）旋转左乘和右乘是有区别的，绝大多数人习惯左乘，不要给自己找事儿

（3）四种变换之间的转换

• 对同一旋转/姿态的不同描述方式是等价的
• 描述之间可以相互转化，但这不是重点

（4）关于位姿与旋转

有时候，我们用这些描述旋转的量来描述一个刚体的姿态，有时候我们又用旋转量来描述刚体在一段时间内的变化，要弄清楚这个很不容易

四元数到其他旋转的表示

任意单位四元数描述了一个旋转，该旋转也可用旋转矩阵或旋转向量描述。现在来考察四元数与旋转向量、旋转矩阵之间的转换关系。在此之前，我们要说，四元数乘法也可以写成一种矩阵的乘法。设 $q=[s,v{]}^{\mathsf{T}}$ 那么，定义如下的符号 $+$ 和 $\oplus$ 为

$$q^{+}=\left[\begin{array}{cc}s& -v^{\mathrm{T}}\\ v& sI+v^{\wedge }\end{array}\right],q^{⨁}=\left[\begin{array}{cc}s& -v^{\mathrm{T}}\\ v& sI-v^{\wedge }\end{array}\right]$$

这两个符号将四元数映射成为一个4×4的矩阵。于是四元数乘法可以写成矩阵的形式：

$$q_1^{+}{q}_2=\left[\begin{array}{cc}{s}_1& -v_1^T\\ v_1& s_{1}I+v_1^{\wedge }\end{array}\right]\left[\begin{array}{c}{s}_2\\ v_2\end{array}\right]=\left[\begin{array}{c}-v_1^T{v}_2+s_1{s}_2\\ s_1{v}_2+s_2{v}_1+v_1^{\wedge }{v}_2\end{array}\right]=q_1{q}_2$$

同理亦可证：

$$q_1{q}_2=q_1^{+}{q}_2=q_2^{⨁}{q}_1.$$

然后，考虑使用四元数对空间点进行旋转的问题。根据前面的说法，有

$$\begin{array}{rl}{p}^{\prime }& =qp{q}^{-1}=q^{+}{p}^{+}{q}^{-1}\\ & =q^{+}{q}^{-1^{⨁}}p.\end{array}$$

代入两个符号对应的矩阵，得

$$\begin{array}{rl}{q}^{+}{\left(q^{-1}\right)}^{⨁}& =\left[\begin{array}{cc}s& -v^T\\ v& sI+v^{\wedge }\end{array}\right]\left[\begin{array}{cc}s& v^T\\ -v& sI+v^{\wedge }\end{array}\right]\\ & =\left[\begin{array}{cc}{s}^2+v^{T}v& s{v}^T-s{v}^T+v^T{v}^{\wedge }\\ s{v}^T-s{v}^T-v^{\wedge }v& v{v}^T+s^{2}I+2s{v}^{\wedge }+{\left(v^{\wedge }\right)}^2\end{array}\right]\\ & =\left[\begin{array}{cc}1& \text{0}\\ {\text{0}}^T& v{v}^T+s^{2}I+2s{v}^{\wedge }+{\left(v^{\wedge }\right)}^2\end{array}\right]\end{array}$$

因为 $p$ 和 $p^{\prime }$ 都是虚四元数，所以事实上该矩阵的右下角即给出了从四元数到旋转矩阵的变换关系：

$$R=v{v}^{\mathsf{T}}+s^{2}I+2s{v}^{\wedge }+{\left(v^{\wedge }\right)}^2$$

为了得到四元数到旋转向量的转换公式，对上式两侧求迹，得

$$\begin{array}{rl}tr\left(R\right)& =\mathrm{tr}(v{v}^{\mathsf{T}}+3s^2+2s\cdot 0+\mathrm{tr}((v^{\wedge }{)}^2)\\ & =v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2)\\ & =(1-s^2)+3s^2-2(1-s^2)\\ & =4s^2-1.\end{array}$$

又由式 $\theta =\arccos \frac{\mathrm{tr}(R)-1}{2}$ 可得

$$\begin{array}{c}\theta =\arccos \frac{\mathrm{tr}(R-1)}{2}\\ =\arccos (2s^2-1),\end{array}$$

即

$$\cos \theta =2s^2-1=2{\cos }^2\frac{\theta }{2}-1$$

所以：

$$\theta =2\arccos s$$

至于旋转轴，如果在式

$${\text{q}}^{+}{\left({\text{q}}^{-1}\right)}^{⨁}=\left[\begin{array}{cc}s& -v^T\\ v& sI+v^{\wedge }\end{array}\right]\left[\begin{array}{cc}s& v^T\\ -v& sI+v^{\wedge }\end{array}\right]=\left[\begin{array}{cc}1& \text{0}\\ {\text{0}}^{\tau }& v{v}^T+s^{2}I+2s{v}^{\wedge }+{\left(v^{\wedge }\right)}^2\end{array}\right]$$

中用 $q$ 的虚部代替 $p$ ，易知 $q$ 的虚部组成的向量在旋转时是不动的，即构成旋转轴。于是只要将它除掉它的模长，即得。总而言之，四元数到旋转向量的转换公式如下：

$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_x\right]}^{\text{T}}={\left[q_1,q_2,q_3\right]}^{\text{T}}/\sin \frac{\theta }{2}\end{array}$$

至于如何从其他方式转换到四元数，只须把上述步骤倒过来处理即可。在实际编程中，程序库通常会为我们准备好各种形式之间的转换。无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。我们应该在实际中选择最方便的形式，而不必拘泥于某种特定的形式。

四元数和轴角的关系

假设某个旋转运动的旋转轴为单位向量 $ \boldsymbol{n}$，绕该轴旋转的角度为 $ \theta$，那么它对应的单位四元数为：
$$\mathbf{q}=\left[\begin{array}{c}\cos \frac{\theta }{2}\\ \mathbf{n}\sin \frac{\theta }{2}\end{array}\right]$$

当旋转一段微小的时间，即旋转角度趋于零时，容易有：
$$\Delta \mathbf{q}=\left[\begin{array}{c}\cos \frac{\delta \theta }{2}\\ \mathbf{n}\sin \frac{\delta \theta }{2}\end{array}\right]\approx \left[\begin{array}{c}1\\ \mathbf{n}\frac{\delta \theta }{2}\end{array}\right]=\left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]$$
其中 $\delta \theta$ 的方向表示旋转轴，模长表示旋转角度。

四元数导数

参考：

SLAM基础——四元数（含四元数对时间的导数）

角速度有：
$$\mathbf{\omega }=\underset{\Delta t\to 0}{\lim }\frac{\delta \theta }{\Delta t}$$
四元数的时间导数为：
$$\begin{array}{rl}\dot{\mathbf{q}}& \triangleq \underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \Delta \mathbf{q}-\mathbf{q}}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \left(\left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\delta \theta }\end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right]\right)}{\Delta t}\\ & =\mathbf{q}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{\omega }\end{array}\right]\end{array}$$

或者有

设初始旋转为 $\text{q}=[s,\mathbf{v}]$，然后，发生了角轴为 $\omega ,\theta$ 的旋转（右乘，对应四元数记作 $\Delta \text{q}$），那么 $\text{q}$ 相对该旋转的导数为：
$$\begin{array}{rl}\underset{\theta \to 0}{\lim }\frac{\mathbf{q}\otimes \Delta \mathbf{q}-\mathbf{q}}{\theta }& =\underset{\theta \to 0}{\lim }\frac{{\left[s\cos \frac{\theta }{2}-{\mathbf{v}}^T\omega \sin \frac{\theta }{2},s\omega \sin \frac{\theta }{2}+\cos \frac{\theta }{2}\mathbf{v}+\mathbf{v}\times \omega \sin \frac{\theta }{2}\right]}^{\top }-\mathbf{q}}{\theta }\\ & =\underset{\theta \to 0}{\lim }\frac{{\left[s\left(\cos \frac{\theta }{2}-1\right)-{\mathbf{v}}^T\omega \sin \frac{\theta }{2},s\omega \sin \frac{\theta }{2}+\left(\cos \frac{\theta }{2}-1\right)\mathbf{v}+\mathbf{v}\times \mathbf{\omega }\sin \frac{\theta }{2}\right]}^{\top }}{\theta }\\ & ={\left[-\frac{1}{2}{\mathbf{v}}^T\mathbf{\omega },\frac{1}{2}s\mathbf{\omega }+\frac{1}{2}\mathbf{v}\times \mathbf{\omega }\right]}^{\top }\\ & =\mathbf{q}\otimes {\left[0,\frac{1}{2}\mathbf{\omega }\right]}^{\top }\end{array}$$

*相似、仿射、射影变换

除了欧氏变换，3D空间还存在其他几种变换方式，只不过欧氏变换是最简单的。它们一部分和测量几何有关，因为在之后的讲解中可能会提到，所以先罗列出来。欧氏变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。其他几种变换则会改变它的外形。它们都拥有类似的矩阵表示。

（1）相似变换

相似变换比欧氏变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为
$$T_S=\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]$$
注意，旋转部分多了一个缩放因子 $s$ ,表示我们在对向量旋转之后，可以在 $x,y,z$ 之三个坐标上进行均匀缩放。由于含有缩放，相似变换不再保持图形的面积不变。

你可以想象一个边长为1的立方体通过相似变换后，变成边长为10的样子（但仍然是立方体）。三维相似变换的集合也叫作相似变换群，记作 $Sim(3)$。

（2）仿射变换

仿射变换的矩阵形式如下：
$$T_A=\left[\begin{array}{cc}A& t\\ 0^{\mathsf{T}}& 1\end{array}\right]$$
与欧氏变换不同的是，仿射变换只要求A是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。

（3）射影变换

射影变换是最一般的变换，它的矩阵形式为
$$T_P=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$$
它的左上角为可逆矩阵 $A$ ，右上角为平移 $t$，左下角为缩放 $a^T$。由于采用了齐次坐标，当 $v\ne 0$ 时，我们可以对整个矩阵除以 $v$ 得到一个右下角为1的矩阵；否则得到右下角为0的矩阵。因此，2D的射影变换一共有8个自由度，3D则共有15个自由度。

射影变换是现在讲过的变换中，形式最为一般的。从真实世界到相机照片的变换可以看成一个射影变换。读者可以想象一个原本方形的地板砖，在照片中是什么样子：首先，它不再是方形的。由于近大远小的关系，它甚至不是平行四边形，而是一个不规则的四边形。

下表总结了目前讲到的几种变换的性质。注意在“不变性质”中，从上到下是有包含关系的。例如，欧氏变换除了保体积，也具有保平行、相交等性质。

变换名称	矩阵形式	自由度	不变性质
欧式变换	$\left[\begin{array}{cc}R& t\\ 0^{\text{T}}& 1\end{array}\right]$	6	长度、夹角体积
相似变换	$\left[\begin{array}{cc}sR& t\\ 0^{\mathsf{T}}& 1\end{array}\right]$	7	体积比
仿射变换	$\left[\begin{array}{cc}A& t\\ 0^{\text{T}}& 1\end{array}\right]$	12	平行性、体积比
射影变换	$\left[\begin{array}{cc}A& t\\ a^{\mathsf{T}}& v\end{array}\right]$	15	接触平面的相交和相切

从真实世界到相机照片的变换是一个射影变换。

如果相机的焦距为无穷远，那么这个变换为仿射变换。不过，在详细讲述相机模型之前，我们只要对它们有个大致的印象即可。

本章注释

---

1. 尽管从向量层面来看，它们确实是反向的关系，但这两个向量的坐标值并不是相反数 ↩︎

2. 感兴趣的读者请参见Rodrigues,事实上第4讲会从李代数层面给出一个证明 ↩︎

3. 旋转向量也有奇异性，发生在转角0超过$2\pi$​而产生周期性时。 ↩︎