齐次矩阵转化为欧拉角坐标系_旋转矩阵、变换矩阵、旋转向量、欧拉角、四元数...

视觉SLAM十四讲(三)——三维空间刚体运动(上)

三维空间刚体运动的描述方法有：旋转矩阵、变换矩阵、旋转向量、欧拉角和四元数，接下来将逐一介绍它们

一、旋转矩阵

点、向量、坐标系

* 点——存在于三维空间之中，点和点组成向量，点本身由原点指向它的向量所描述

* 向量——带指向性的箭头，可以进行加法减法等运算，定义坐标系后，向量可以由$ R^3 $当中的三个数表示， 如何理解这句话呢。如下图所示：

在代数中，我们用一组基底和向量 \(a\) 在每个坐标轴上的投影来表示一个向量，对于 \(a\)，通过某种线性组合，可以表示为\(a = a_xe_1+a_ye_2+a_ze_3\)

而上面那句话的意思是在矩阵运算中，\(a\) 可以表示为 \(\left[ \begin{matrix} a_x \\ a_y \\ a_z \end{matrix} \right]\)，因为\((e_1,e_2,e_3)\left[ \begin{matrix} a_x \\ a_y \\ a_z \end{matrix} \right] = a_xe_1+a_ye_2+a_ze_3\)

* 坐标系——三个正交的轴，构成线性空间的一组基，分为左手系和右手系

* 向量的运算可以由坐标运算来表达：加减法，内积，外积

问题的出现——一个最简单的情况，机器人从某一点直线运动到另一点，假设机器人是质点，并且和目标点处于同一平面上，分别以机器人和目标点建立坐标系，在移动过程中机器人的坐标系位置一直在变，要计算与目标点的距离，就需要描述坐标系之间如何变化

* 进而——如何计算同一个向量在不同坐标系里的坐标

* 如果刚才的机器人不是直线运动，而会有拐弯，这时坐标系就会旋转，因此描述整个运动过程就是三个轴的旋转和原点间的平移，这就是所谓的欧式变换，保证同一个向量在各个坐标系下的长度和夹角都不会发生变化，通过旋转和平移两部分组成

问题解决

* 平移是一个向量

* 旋转

设某坐标系(用三个方向上的单位向量表示) \((e_1,e_2,e_3)\) 发生了一次旋转，变成了\((e_1^{'},e_2^{'},e_3^{'})\)

对于某个固定的向量 \(a\)(向量不随坐标系旋转)，它的坐标怎么变化，其中 \(\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right]\) 是 \(a\) 在第一个坐标系中的坐标，\(\left[ \begin{matrix} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{matrix} \right]\) 是 \(a\) 在另一个坐标系中的坐标，如图，P为向量 \(a\)

坐标关系$$ [e_1,e_2,e_3]\left[ \begin{ma