视觉SLAM十四讲：从理论到实践（Chapter3：三维空间刚体运动）

前言

学习笔记，仅供学习，不做商用，如有侵权，联系我删除即可

目标

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

3.1 旋转矩阵

3.1.1 点、向量和坐标系

内积：

外积：

外积的结果是一个向量，方向垂直于这两个向量，大小为|a||b|sin<a,b>，是两个向量张成的四边形的有向面积。a^是反对称矩阵。 

反对称矩阵A满足：。

3.1.2 坐标系间的欧氏变换

刚体运动：两个坐标系之间的运动由一个旋转加上一个平移组成。

欧式变换由旋转和平移组成。

旋转矩阵：行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。

n维旋转矩阵的集合定义如下：

SO(n)是特殊正交群（Special Orthogonal Group）。这个集合由n维空间的旋转矩阵组成，例如，SO(3)就是三维空间的旋转。通过旋转矩阵，可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起。

旋转矩阵的逆（转置）描述了一个相反的旋转。

世界坐标系中的向量，经过一次旋转和一次平移后得到，把旋转平移合到一起，有：

实际当中会定义坐标系1、坐标系2，向量在两个坐标系下的坐标为，那么有，这里的是指把坐标系2的向量变换到坐标系1中，下标是从右读到左的。对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标。

3.1.3 变换矩阵和齐次坐标

齐次坐标：在三维向量的末尾添加1，将其变成四维向量。

对于齐次坐标，可以把旋转和平移写在一个矩阵里，使整个关系变成线性关系，矩阵T为变换矩阵。

矩阵T结构特别：左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1，这种矩阵称为特殊欧式群(Special Euclidean Group)

与SO(3)一样，该矩阵的逆表示一个反向的变换。 

表示从2到1的变换。本书中不刻意区分齐次和非齐次，默认使用的是符合运算法则的坐标。例如Ta就是齐次坐标，Ra就是非齐次坐标，否则不满足矩阵运算法则，无法运算。 

3.2 实践：Eigen

待完善

3.3 旋转向量和欧拉角

3.3.1 旋转向量

矩阵表示方式的缺点：

1. SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。变换矩阵更是如此。
2. 旋转矩阵自身带有约束，且行列式为1，变换矩阵也是如此。因此当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使求解变得更困难。

事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。

3.3.2 欧拉角

旋转矩阵或旋转向量不能直观描述旋转究竟是什么样的。

欧拉角提供了直观的方式：把一个旋转分解成3次绕不同轴的旋转。如果讨论的更细一些，还需要区分每次是绕固定轴旋转还是绕旋转之后的轴旋转。

经典的一种欧拉角：，rpy是一种常用的欧拉角。但大部分领域在使用欧拉角时都有各自的坐标方向和顺序上的习惯。常用的旋转方式：ZYX，逆时针旋转为正，顺时针旋转为负。

• 绕Z轴：偏航角yaw
• 绕旋转之后的Y轴旋转：俯仰角pitch
• 绕旋转之后的X轴旋转：滚转角roll

缺点：存在万向锁问题(Gimbal Lock)：在俯仰角为±90°时，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转）。这被称为奇异性问题。

因此，欧拉角不适用于插值和迭代，SLAM中也很少用欧拉角表达姿态，滤波或优化中也不会用欧拉角表达旋转（因为奇异性）。但是转换成欧拉角可以快速验证结果是否正确，可用于可视化。

3.4 四元数（Quaternion）

3.4.1 四元数的定义

一个四元数有一个实部和三个虚部。本书把实部写在前面：，三个虚部满足以下的关系：

3.4.2 四元数的运算

 

3.4.3 用四元数表示旋转

首先把三维空间点用一个虚四元数表示：

那么旋转后的点表示为：

3.5 相似、仿射、射影变换

1. 刚体变换
2. 相似变换，增加一个自由度，允许物体均匀缩放
3. 仿射变换，A只需要是一个可逆矩阵，不鄙视正交矩阵
4. 射影变换

总结