视觉SLAM14讲——三维空间刚体运动

1、旋转矩阵

1、点，向量和坐标系

我们生活在三维空间当中，三维空间由三个轴组成，空间中的任意一个位置可以由三个坐标来指定。对于视觉SLAM的研究，我们不仅要确定相机的位置，还有确定相机的位姿，也就是相机的朝向。
用线性代数的知识来说，我们可以在一个线性空间中确定一个向量，并且这个向量在该线性空间内的坐标是确定的。
例如确定向量a的坐标为：

根据线性代数的知识我们可以对向量进行一定的运算，如加减，内积，外积，数乘等。
向量间的内积可以表示为：

内积可以用来描述向量间的投影关系。
向量间外积可以表示为：

两个向量外积的运算结果还是一个向量，这个向量和进行外积运算的两个向量互相垂直。
如上图所示，我们可以将外积运算进行变形，将向量a写成一个反对称矩阵的形式，在这里我们可以将^记为反对称符号。
这样我们就将向量a和向量b的外积改写成矩阵a^和向量b的乘积。

2、坐标间的欧式变换

对于自动驾驶的研究，我们通常会定义不同的坐标系，车辆自身有车身坐标系，车辆上方的相机又有相机坐标系，当车辆对一个点进行建图时，相机视野中的向量p在相机坐标系下的坐标为p_c，而在车身坐标系下的坐标为p_w那么这两个坐标该如何转换，就是我们接下来要探讨的事情。
两个坐标系之间的变换可以由旋转和平移组成，在空间中只会有位置和姿态的不同，我们将这种变化称为欧氏变化。

上图可以看出两个坐标系可以经过一个变化矩阵T来进行转变，变换包括旋转和平移，我们首先考虑旋转（因为一个坐标系平移到另一个坐标系只需要加上一个向量）。
由于向量a并没有随着坐标系的变化而变化，在两个坐标系中向量a都不变，所以我们可以将向量a的坐标写为：

为了描述坐标间的变换，我们将上式改写为：

将中间的矩阵定义为R，该矩阵刻画了同一向量的坐标变换关系，所以我们将矩阵R定义为旋转矩阵。
旋转矩阵有一些特别的性质：旋转矩阵是一个行列式为1的正交矩阵(AA^T=E)，反过来，行列式为1的正交矩阵为旋转矩阵。我们将n维旋转矩阵的集合定义为：

上述我们只是说了欧氏变换中的旋转变换，除了旋转还需要平移，车身坐标下的向量a在经过旋转和一次平移t后得到了a_’，我们可以表示为：

3、变换矩阵和齐次坐标

我们在前面解释了欧氏空间中的旋转和平移，但仍然有一些问题：如果我们需要连续变换两次，例如：R₁,t₁，R₂,t₂

那么从a到c的变化为：

假设这样的变化有很多次，我们的形式就会很繁琐。
我们可以采用一个数学技巧：在一个三维向量末尾添加1，变为四维向量，称为齐次坐标。在这个四维向量中，将旋转和平移写到一个矩阵T中。

这样写后并不会改变原来的计算结果，同时还能将形式简化。用上面的形式来表示两次变化可写为：

可以大大的简化写法，我们将这种矩阵称为特殊欧氏群。

2、旋转向量

在上面我们说了用旋转矩阵来表示旋转，用变换矩阵来表示三维空间刚体运动，旋转矩阵的写法有不足之处。
1、旋转矩阵有9个量，但是一次旋转只有三个自由度(x,y,z)这样的写法有些冗余，变换矩阵同理。
2、旋转矩阵本身有限制，要求是行列式为1的正交矩阵，这样的特性会影响后面的优化问题。
事实上任何旋转我们都可以用一个旋转轴和一个旋转角来表示，所以我们可以找一个向量，该向量方向和旋转轴一致，大小和旋转角一致。我们将这样的向量称为旋转向量。同理，变换矩阵我们可以用一个旋转向量加上一次平移来表示。
旋转向量和旋转矩阵之间可以相互转换，假设旋转轴为一个单位长度的向量n角度为θ，那么向量θn可以用来描述旋转。该旋转向量到旋转矩阵的转化可以用罗德里格斯公式来表示：

式子中的n^含义为将向量n转化为反对称矩阵的形式。
从旋转矩阵到旋转向量的转换为：


转轴n是矩阵R特征值为1的特征向量。

3、四元数

旋转矩阵用来描述旋转有些冗余，旋转向量会有奇异性。对于二维平面，我们放在复平面上考虑，乘上一个复数i相当于逆时针把一个复向量旋转90°，同理，在三维空间中也有类似与复数的代数用来描述旋转——四元数。
为了更好的理解四元数，我们可以将四元数和复数对比学习。
在二维中，我们想将复平面中的向量旋转θ角，可以用e^iθ和复向量相乘。在二维情况下，旋转用单位复数来描述，同理，在三维情况下，旋转可以用单位四元数来描述。

一个四元数q有一个实部和三个虚部，可表示为：

其中i，j，k为四元数的三个虚部，这三个虚部满足以下关系：

仔细来看，其实i，j，k就相当于三个坐标轴，他们自身乘积运算满足复数运算，相互之间的乘积运算和外积运算一样。有时会将四元数用一个标量和一个向量来表示：
q=[s,v]^T,s为四元数的实部，v为四元数虚部，当实部为0时称为虚四元数。
我们可以通过四元数的运算法则最终得到四元数和旋转矩阵的对应关系以及四元数和旋转向量的对应关系，由于过程对于我个人来说有些困难，所以在这里直接给出结论:
四元数和旋转矩阵的转换关系：
设四元数q=q₀+q₁i+q₂j+q₃k
那么对应的旋转矩阵R为：

同理，如果知道了旋转矩阵R，那么四元数的实部和虚部分别为：

四元数和旋转向量的转化关系：
假设某个旋转是绕单位向量n=[n_x,n_y,n_z]^T进行了角度为θ的旋转，那么该旋转的四元数形式为：

反之我们可以从单位四元数中推出旋转向量的旋转轴和旋转角：

四元数中由于本人水平有限，只是给出了大体概念和四元数与旋转矩阵以及旋转向量间的转换关系。