高翔视觉SLAM十四讲学习笔记——第3讲刚体运动

---

学习任务

1. 理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角。
2. 掌握Eigen 库的矩阵、几何模块使用方法。

一、点，向量和坐标系

• 刚体：不光有位置，还有自身的姿态。相机也可以看成三维空间的刚体，位置是指相机在空间中的哪个地方，而姿态则是指相机的朝向。

向量的坐标

：确定一个坐标系，也就是一个线性空间的基，就可以谈论向量a 在这组基下的坐标

• 右手法则：给定x 和y 轴时，z 就可以通过右手（或左手）法则由x × y 定义出来（大部分程序默认右手法则）
  

内外积

• 内积：描述向量间投影关系
  
• 外积：描述三维及以上向量之间的旋转关系
  
  外积的方向垂直于这两个向量，大小为|a||b|sin ⟨a,b⟩，是两个向量张成的四边形的有向面积。
  如上图公式所示，可以把外积转换为a是一个反对称矩阵与向量b的乘积，用a^b来表示。
  

二、坐标系间的欧氏变换

描述两个坐标系之间的旋转关系，再加上平移，统称为坐标系之间的变换关系。在机器人的运动过程中，常见的做法是设定一个惯性坐标系，（或者叫世界坐标系），可以认为它是固定不动的。

• 欧氏变换：相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。

• 一个欧氏变换由一个旋转和一个平移两部分组成。

• 首先来考虑旋转。
  们设某个单位正交基
  (e1，e2，e3) 经过一次旋转，变成了(e1‘，e2’，e3‘)，对于同一个向量a，它在两个坐标系下的坐标为[a1，a2，a3]T 和为[a1’，a2‘，a3’]T。根据坐标的定义：
  
  转换一下：
  
  矩阵R为两组基的内积，成为旋转矩阵，描述了旋转本身。也是一个行列式为1的正交矩阵（即逆为自身转置的矩阵）。
  
  

• 再考虑平移
  1)欧氏空间的坐标变换关系描述：
  a’=Ra+t(R旋转矩阵，t为平移向量)

  三、齐次坐标和变换矩阵♥重点♥

  a’=Ra+t重写为：

  $\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]$=$\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$\left[\begin{array}{c}a\\ 1\end{array}\right]$=T$\left[\begin{array}{c}a\\ 1\end{array}\right]$

注：将三维向量末尾添加1,变成四维向量，称为齐次坐标，矩阵T为变换矩阵

• 齐次坐标：
  x=$[x,y,z,w{]}^T$=$[x/w,y/w,z/w,1{]}^T$
  忽略掉最后一项1,还原为非齐次坐标
  
  即可表示为：
  
  直接把它写成b = Ta 的样子，默认其中是齐次坐标。
  
  以后不区别齐次坐标与普通的坐标的符号，默认我们使用的是符合运算法则的那一种。例如，当我们写Ta 时，使用的是齐次坐标（不然没法计算）。而写Ra 时，使用的是非齐次坐标。如果写在一个等式中，我们就假设齐次坐标到普通坐标的转换，是已经做好了的——因为齐次坐标和非齐次坐标之间的转换事实上非常容易。

四、旋转向量和欧拉角

4.1 旋转向量

• 目的是：希望有一种方式能够紧凑地描述旋转和平移
• 旋转向量：任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量（或轴角，Axis-Angle）。这种表示法只需一个三维向量即可描述旋转。旋转向量就是我们下章准备介绍的李代数。
• 罗德里格斯公式（Rodrigues’s Formula ）：表明旋转向量和旋转矩阵之间的转换
  
  假设有一个旋转轴为n，角度为θ的旋转，显然，它对应的旋转向量为θn。
  
  I为三维单位向量，迹为3，n^是反对称矩阵，迹为0
  
  
  因此，转轴n 是矩阵R 特征值1 对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

4.2 欧拉角

• 定义：使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转
  由于分解方式有许多种，所以欧拉角也存在着不同的定义方法。
  1.先绕X 轴旋转，再绕Y 轴，最后绕Z 轴，就得到了一个XY Z 轴的旋转。同理，可以定义ZY Z、ZY X等等旋转方式。
  2.每次旋转是绕固定轴旋转的，还是绕旋转之后的轴旋转的，这也会给出不一样的定义方式。
  欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。由于它等价于ZY X 轴的旋转，我们就以ZY X 为例。
  假设一个刚体的前方（朝向我们的方向）为X 轴，右侧为Y 轴，上方为Z 轴
  那么，ZY X 转角相当于把任意旋转分解成以下三个轴上的转角：
  
  用[r，p，y]T 这样一个三维的向量描述任意旋转。
• 万向锁问题（Gimbal Lock）：在俯仰角为±90◦ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）因此很少在程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性）。

4.3 四元数

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式

• 四元数（Quaternion）：既是紧凑的，也没有奇异性。是类似于复数的代数。用复数集C 表示复平面上的向量，而复数的乘法则能表示复平面上的旋转：例如，乘上复数i 相当于逆时针把一个复向量旋转90 度。类似的，在表达三维空间旋转时，也有一种类似于复数的代数即四元数
• 一个四元数q 拥有一个实部和三个虚部。
  
  
  乘以i 应该对应着旋转180 度，这样才能保证ij = k 的性质。而i^2 = -1，意味着绕i 轴旋转360 度后，你得到了一个相反的东西。这个东西要旋转两周才会和它原先的样子相等。
  
  s 称为四元数的实部，而v 称为它的虚部。如果一个四元数虚部为0，称之为实四元数。反之，若它的实部为0，称之为虚四元数。

五、四元数到旋转矩阵的转换