视觉SLAM十四讲第三讲

第三章 三维空间刚体运动

主要目标

1. 理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角。
2. 掌握 Eigen 库的矩阵、几何模块使用方法。
   一、 旋转矩阵
   刚体，它不光有位置，还有自身的姿态。相机也可以看成三维空间的刚体，于是位置是指相机在空间中的哪个地方，而姿态
   则是指相机的朝向。我们要用数学语言来描述它。
   1）点和向量
   点就是空间当中的基本元素，没有长度，没有体积。把两个点连接起来，就构成了向量。向量可以看成从某点指向另一点的一个箭头。
   2）基
   维空间中的某个点的坐标也可以用 R3 来描述。假设在这个线性空间内，我们找到了该空间的一组基(e1, e2, e3)，那么，任意向量 a 在这组基下就有一个坐标：
   
   这里 (a1, a2, a3)T 称为 a 在此基下的坐标。坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。
   3）内积和外积
   对于 a, b ∈ R3，通常意义下的内积可以写成：
   
   其中 ⟨a, b⟩ 指向量 a, b 的夹角。内积也可以描述向量间的投影关系。而外积则是这个样子：
   
   外积的结果是一个向量，它的方向垂直于这两个向量，大小为 |a| |b|sin ⟨a, b⟩，是两个向量张成的四边形的有向面积。
   对于外积运算，我们引入 ∧ 符号，把 a 写成一个矩阵。事实上是一个反对称矩阵（Skew-symmetric matrix），可以将 ∧ 记成一个反对称符号。这样就把外积 a × b 写成了矩阵与向量的乘法a∧b，把它变成了线性运算。
   
   4）坐标系间的欧氏变换
   如下图所示的坐标变换。对于同一个向量 p，它在世界坐标系下的坐标 pw 和在相机坐标系下的坐标 pc是不同的。这个变换关系由变换矩阵 T 来描述。变换矩阵由旋转矩阵和平移向量构成。
   
   欧氏变换由旋转和平移组成。我们首先考虑旋转。设某个单位正交基 (e1, e2, e3) 经过一次旋转变成了 (e′1, e′2, e′3)。那么，对于同一个向量 a（该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 [a1, a2, a3]T 和 [a′1, a′2, a′3]T。因为向量本身没变，根据坐标的定义，有：
   
   为了描述两个坐标之间的关系，我们对上述等式的左右两边同时左乘 [eT1 eT2 eT3 ]T,那么左边的系数就变成了单位矩阵，所以：
   
   5)变换矩阵与齐次坐标
   
   这是一个数学技巧：我们在一个三维向量的末尾添加 1，将其变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成线性关系。该式中，矩阵 T 称为变换矩阵（Transform Matrix）。
   二、实践：Eigen
   输入以下命令进行安装：
   sudo apt-get install libeigen3-dev
   三、旋转向量和欧拉角
   1）旋转向量
   任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。
   假设旋转轴为一个单位长度的向量 n，角度为 θ，那么向量 θn 也可以描述这个旋转。从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigues’s Formula ）表明：
   
   一个旋转矩阵到旋转向量的转换：
   
   2）欧拉角
   而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了 3 个分离的转角，把一个旋转分解成 3 次绕不同轴的旋转。
   具体参考：https://blog.csdn.net/sinolover/article/details/90671784
   三、四元数
   1）四元数的定义
   旋转矩阵用 9 个量描述 3 自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。而四元数是 Hamilton 找到的一种扩展的复数。它既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。
   一个四元数 q 拥有一个实部和三个虚部。
   
   其中 i, j, k 为四元数的三个虚部。这三个虚部满足以下关系式：
   
   2）四元数的运算
   四元数 qa, qb 的加减运算为：
   
   乘法：
   
   模长：
   
   共轭：
   
   逆：
   
   数乘：
   
   3）四元数表示旋转
   首先，把三维空间点用一个虚四元数来描述：
   
   相当于把四元数的 3 个虚部与空间中的 3 个轴相对应。那么，旋转后的点 p′ 即可表示为这样的乘积：
   
   这里的乘法均为四元数乘法，结果也是四元数。最后把 p′ 的虚部取出，即得旋转之后点的坐标。
   4）四元数到其他旋转表示的转换
   四元数到旋转矩阵：
   
   四元数到旋转向量的转换公式：
   
   四、相似、仿射、射影变换
   1、相似变换
   相似变换比欧氏变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为：
   
   2、仿射变换
   仿射变换的矩阵形式如下：
   
   3、射影变换
   
   4、常见变换比较
   
   五、实践：Eigen 几何模块
   • 旋转矩阵（3 × 3）：Eigen::Matrix3d。
   • 旋转向量（3 × 1）：Eigen::AngleAxisd。
   • 欧拉角（3 × 1）：Eigen::Vector3d。
   • 四元数（4 × 1）：Eigen::Quaterniond。
   • 欧氏变换矩阵（4 × 4）：Eigen::Isometry3d。
   • 仿射变换（4 × 4）：Eigen::Affine3d。
   • 射影变换（4 × 4）：Eigen::Projective3d。