前言

在上一个Part，我们知道了坐标转换的一些基础只是，其实三维空间的刚体运动与其也有紧密的联系，可以理解为刚体运动为持续的把运动坐标系中的坐标变换到固定坐标系中，此外再加上旋转这一过程。

世界坐标系与里程计坐标系

世界坐标系：世界坐标系（world坐标系）一般指固定坐标系，在系统运行过程其全局坐标不发生改变。在建图时，经常会认为起点为世界坐标系原点，而本体初始旋转角度为0。里程计坐标系：里程计坐标系一般指运动坐标系，在建图过程一般指传感器坐标系，视觉SLAM中指的就是相机坐标系（camera坐标系）。

与坐标系变换的区别与联系。坐标系变换中，向量A空间位置不动，已知A在C1坐标系（视角）下的坐标，求在C2坐标系（视角）下的坐标。简单的说，向量A不变，描述向量A的参考系变了，即坐标系在运动。而刚体运动的描述中，参考坐标系不动，刚体坐标系运动。

在描述刚体运动时，我们一般会求得下一个时刻，相对于上一个时刻坐标系下的平移与旋转，如上图右图所示。这相当于我们知道了c2在c1坐标系下的坐标（平移）与相对于c1的旋转（局部坐标）。这个时候，利用c1在w下的坐标，将c2的平移与旋转转换到世界坐标系下，就得到了c2时刻的全局坐标。然后对于c2，得到下一时刻c3相对于c2的平移与旋转（局部坐标），将其转换到w坐标系下，不断进行下去，就得到了以运动坐标系表征的本地在世界坐标系下的刚体运动。

欧式变换

三维空间刚体的运动，简单来说由平移与旋转构成。平移指两个点之间的位移，三维空间中其自由度为三。旋转则是绕着三个轴各种旋转的角度，自由度也为三。平移加旋转可构成一次变换，这个变换称为欧式变换。假设c1原点在w坐标系下坐标为P1，旋转为R1，c2原点在c1坐标系下坐标为△P，旋转为△R，则c2在w坐标系下的欧式变换P2与R2为：

P2 = R1 * △P + P1;R2 = R1 * △R;

欧式变换为一个平移+一个旋转，为了简洁，可以使用一个矩阵表示

那么上述变换，就可以写为：

可以看到，只要求得当前时刻，在上一坐标系下的位移及旋转，就可以得到当前时刻的全局欧式变换。不断累加下去，就可以得到所有时刻的欧式变换，连起来就是传感器在三维空间的刚体运动。在加上上一节的外参变换，就可以得到本体在空间中的刚体运动了。

旋转矩阵和四元素

三维空间里表述旋转的计算方式常见的有两种：矩阵(Matrix)和四元数（Quaternion），为了防止矩阵方式存在万向节死锁（Gimbal Lock）问题，通常采用四元数来计算旋转。但在自动驾驶领域里很少这么干，因为相机是固定在车子上，只有垂直于地面的轴（一般是Z轴）才会发生360度的旋转，根本无法引发万向节问题，总不至于用户坚持在翻车的阶段仍旧保持自动驾驶这个诡异的需求。所以在自动驾驶目标检测或车道线检测里的代码里通常就是矩阵形式，SLAM因为还会用在AR和其它领域，相机不是相对固定的，所以会采用四元数。关于万向节死锁问题，大家可以参考：https://zhuanlan.zhihu.com/p/344050856

旋转矩阵

旋转矩阵是一个3x3的正交矩阵，用来描述一个刚体绕着某个轴旋转的情况。旋转矩阵的每一列代表了一个坐标系中的向量，因此可以通过旋转矩阵将一个向量从一个坐标系转换到另一个坐标系。

四元数

关于四元数是一个比较复杂的问题，此处给出一些参考资料给读者学习。

如何通俗地解释欧拉角？之后为何要引入四元数？彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数

下图是二者的主要对比啦。