视觉SLAM理论入门——（2）刚体运动

重要概念

1、位置：相机在空间中的哪个地方

2、姿态：相机的朝向

3、向量：向量是空间中的一个东西，并不与实数发生联系，只有在指定坐标系的情况下，向量才有坐标

4、向量积：

（1）内积是个标量

（2）外积是个向量，方向垂直于两个向量组成的平面

其中 ^ 表示是反对称符号，它将一个向量映射成一个反对称矩阵

例如： 映射为 

 

旋转矩阵

机器人的运动是刚体运动，它保证了同一个向量在不同坐标系下长度和角度都不会发生变化

机器人的相机是一个移动坐标系的原点，它位于一个固定不变的世界坐标系中。相机视野中的向量a在不同坐标系下坐标是不相同的，因此需要坐标变换（欧氏变换），欧氏变换由一个旋转和一个平移组成

先考虑旋转

设一个单位正交基（e1，e2，e3）经过一次旋转变成 （e′1 ，e′2 ，e′3 ），同一个向量在两个坐标系中的坐标分为是 [a1，a2，a3 ] T 和 [a′1，a′2，a′3] T 

则

上式同时左乘得到

这里的R就是旋转矩阵，描述旋转本身。旋转矩阵的是一个行列式为1的正交矩阵，反过来，行列式为1的正交矩阵是旋转矩阵

旋转矩阵的集合是特殊正交群，定义如下：

由于旋转矩阵是正交阵，它的逆（也就是转置）描述了一个反向的旋转

考虑平移

一个向量经过一次平移，其结果是原向量+平移向量（记为t）

综上，一个向量a经过一次欧氏变换，得到a′，其数学表达如下：

 

变换矩阵与齐次坐标

上面欧氏变换的数学表达不是线性关系，经过多次变换，式子会变得复杂

引入齐次坐标后，可使得欧氏变换的数学表达变成线性关系：

，其中T称为变换矩阵，包含了欧氏变换的所有信息（旋转和平移）

变换矩阵的集合是特殊欧氏群，定义如下：

它的逆阵描述一次反向的变换

（后面不再区分齐次坐标和非齐次坐标，见到Ta使用齐次坐标运算，见到Ra使用非齐次坐标运算）

 

旋转向量

用一个3*3包含9个量的旋转矩阵描述一次只有三个自由度的旋转，用一个4*4包含16个量的变换矩阵描述一次只有六个自由度（分别沿3个轴旋转和平移）的变换，这样的表达方式是冗余的

任意一个旋转都可以用一个旋转轴和一个旋转角来描述，因此可以用一个三维旋转向量（方向与旋转轴一致，长度等于旋转角）描述旋转；类似地，用一个旋转向量和一个平移向量（共六维）描述一次变换

设有一个旋转轴为n，角度为θ的旋转，对应旋转向量为θn。从旋转向量到旋转矩阵的过程由罗德里格斯公式给出：

从旋转矩阵到旋转向量，需要求解θ，n：

，其中R的迹求解公式为

由于旋转轴上的向量经过旋转后不变，因此又如下式子成立：

，这个式子表明n是R特征值1对应的特征向量。求解方程，再归一化，即可得到n

 

欧拉角

旋转矩阵、旋转向量都能够描述旋转，但是不直观，无法从矩阵和向量中直接看出旋转是怎样的，因此引入欧拉角

欧拉角使用三个分离的转角，将一次旋转分解为三次旋转，直观地描述旋转。旋转的分解方法很多，因此欧拉角也有很多定义方法，最常用的是 偏航-俯仰-滚转（yaw-pitch-roll），它等价于ZYX轴的旋转，其分解过程如下：

1、先绕物体Z轴旋转，得到偏航角yaw

2、再绕旋转后的Y轴旋转，得到俯仰角pitch

3、最后绕旋转后的X轴旋转，得到滚转角roll

欧拉角的缺点是会碰到万向锁问题（奇异性问题），当俯仰角为±90°时，第一次旋转和第三次旋转使用同一个轴，使系统丢失了一个自由度。事实上，通过三个实数描述三维旋转，奇异性问题是不可避免的

 

四元数

平面上的旋转可以使用单位复数描述；类似地，三维空间中的旋转可以使用单位四元数描述（它既是紧凑的、又是非奇异的）

 

四元数定义

一个四元数q包含一个实部和三个虚部

其中，i、j、k是三个虚部，满足

四元数的向量形式

其中，s是实部（标量），v是虚部（三维向量）

 

四元数运算

1、加法：

2、乘法：

3、共轭：

4、模长：

5、逆：

，其中1是四元数

6、数乘：

7、点乘：

 

旋转的四元数表达

假设某个旋转的旋转轴为，转角为θ，那么这个旋转的四元数表达形式为

反过来也可以从单位四元数计算旋转向量：

从旋转的四元数表达式可以看出，当θ = θ + 2π，得到的是一个相同的旋转，此时四元数变成，因此任意旋转可以由两个互为相反数的四元数表示；当θ = 0，得到一个单位实四元数

 

设一个三维空间点经过一个由单位四元数q指定的旋转后变为，则

，其中，，和中的虚部对应是旋转前后的坐标

 

四元数与旋转阵、旋转向量的转换

四元数乘法可以用矩阵表达。设，定义符号如下

，则四元数乘法的矩阵表达如下

，同理

回代前面的式子有

，其中

因此，从四元数到旋转阵的表达式为

由旋转矩阵进一步可以得到旋转向量，其中旋转角推导如下（由罗德里格斯公式）：

，这里用到倍角公式

对于旋转轴，推导思路跟前面类似

旋转轴上的向量经过旋转后不变，因此取=，代入式子

，最后得到Rv = v，解方程，归一化，得到旋转轴v

这里进行归一化，实际上就是虚部每个元素同时除以（由可得，s = ，从三角形关系可以得到向量的虚部的模是）

 

在 旋转的四元数表达 中使用四元数q直接得到旋转向量的方法更为简便！！！