主要目标
1.理解三维空间的刚体运动扫描方式：旋转矩阵、变换矩阵、四元数和欧拉角
2.掌握Eigen库的矩阵、几何模型的使用方法
旋转矩阵
点、向量和坐标系
刚体不光有位置，还有自身的姿态。
相机也可以看成三维空间的刚体，于是位置是指相机在空间中的哪个地方，而姿态则是指相机的朝向。
（相机正处在空间（0，0，0）点处，朝向正前方）
点是空间中的基本元素，没有长度，没有体积。
把两个点连接起来，就构成了向量。向量可以看成从某点指向另一点的一个箭头。
外积的结果是一个向量，它的方向垂直于这两个向量，是两个向量张成的四边形的有向面积。
坐标系间的欧式变换
在机器人学中，会给每一个连杆和关节定义它们的坐标系；
在3D作图时，我们也会定义每一个长方体、圆柱体的坐标系。
如果考虑运动的机器人，那么常见的做法是设定一个惯性坐标系（或者叫世界坐标系），可以认为它是固定不动的。
两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。相机运动就是一个刚体运动。
刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。（只有空间位置和姿态会发生变化）。我们说手机坐标系到世界坐标系之间，相差了一个欧式变换。
对于坐标变换：同一个向量p，它在世界坐标系下的坐标pw和在相机坐标系下的坐标pc是不同的。这个变换关系由变换矩阵T来描述。
欧式变换由旋转和平移组成。我们首先考虑旋转。
旋转向量与欧拉角
旋转向量：
任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。
从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明。
（求迹（trace）即是求矩阵的对角元素之和）
转轴n是矩阵R特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。 （旋转轴经过旋转之后不变）
欧拉角：
欧拉角提供了一种非常直观的方式来描述旋转------它用了3个分离的转角，把一个旋转分解成3 次绕不同轴的旋转。欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）3个角度来描述一个旋转。它等价于ZYX轴的旋转，因此就以ZYX为例。假设一个刚体的前方（朝向我们的方向）为X轴，右侧为Y轴，上方为Z轴。那么，ZXY转角相当于把任意旋转分解成以下3个轴上的转角：
1.绕物体的Z轴旋转，得到偏航角yaw。
2.绕旋转之后的Y轴旋转，得到俯仰角pitch。
3.绕旋转之后的X轴旋转，得到滚转角roll。
其他的欧拉角也是通过这种方式，把旋转分解到3个轴上，得到一个三维的向量，只不过选用的轴及顺序不一样。（大部分领域在使用欧拉角时都有自己的坐标方向和顺序上的习惯，不一定和我们说的相同。）
欧拉角的一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock）：在俯仰角±90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转）。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。
理论上证明，只要想用插值和迭代，往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表达姿态，同样也不会在滤波或优化中使用欧拉角表达旋转（因为具有奇异性）。在某些主为2D运动的场合（例如扫地机、自动驾驶车辆），我们也可以把旋转分解成三个欧拉角，然后把其中一个（例如偏航角）拿出来作为定位信息输出。
四元数：四元数是Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。
在二维情况下，旋转可以由单位复数来描述，类似的，我们会看到，三维旋转可以由单位四元数来描述。
一个四元数q拥有一个实部和三个虚部。如果一个四元数的虚部为0，则称为实四元数，若它的实部为0，则称为虚四元数。
可以用单位四元数表示三维空间中任意一个旋转。
四元数常见的有四则运算、共轭、求逆、数乘等。
运算分为：加法和减法，乘法，模长，共轭，逆，数乘
四元数乘法也可以写成一种矩阵的乘法。
无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。
欧式变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动和旋转，不改变它自身的样子。
三维世界中刚体运动的描述方式：旋转矩阵、旋转向量、欧拉角、四元数。
在SLAM中，除了表示，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决形如”什么样的相机位姿最符合当前观测数据“这样的问题，一种典型的方式是把它构建成一个优化问题，求解最优的R，t，使得误差最小化。
旋转矩阵自身是带有约束的（正交且行列式为1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群-李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。
三维旋转矩阵构成了特殊正交群SO（3），而变换矩阵构成了特殊欧氏群SE（3）。
旋转矩阵、变换矩阵对加法都是不封闭的。换句话来说，对于任意两个旋转矩阵R1,R2，按照矩阵加法的定义，和不再是一个旋转矩阵。
我们可以对任何一个旋转或变换矩阵（在乘法的意义上）求逆。乘法对应旋转或变换的复合，两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个（良好的）运算的集合，我们称之为群。
群是一种集合加上一种运算的代数结构。
群要求这个运算要满足：封闭性、结合律、幺元、逆。（封结幺逆）
旋转矩阵集合和矩阵乘法构成群，同样，变换矩阵和矩阵乘法也构成群（因此才能称它们为旋转矩阵群和变换矩阵群）。
矩阵中常见的群有：
1.一般线性群GL（n）：指n*n的可逆矩阵，它们对矩阵乘法成群。
2.特殊正交群SO（n）：也就是所谓的旋转矩阵群，其中SO（2）和SO（3）最为常见。
3.特殊欧式群SE（n）：也就是前面提到的n维欧氏变换，如SE（2）和SE（3）。
群结构保证了在群上的运算具有良好的性质，群论则是研究群的各种结构和性质的理论。
李群是指具有连续（光滑）性质的群。SO（n）和SE（n）在实数空间上是连续的。我们能直观想象一个刚体能够连续地在空间中运动，所以它们都是李群。
每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确的说，是单位元附近的正切空间。
李代数由一个集合V，一个数域F和一个二元运算[,]组成。