视觉SLAM十四讲学习笔记——第三章 三维空间刚体运动

3.1 旋转矩阵

3.1.1 点、向量和坐标系

1. 一个空间点的位置可以由三个坐标指定。对于刚体而言，不仅有位置，还应该有姿态。相机也可以看成三维空间的刚体，位置是指相机在空间中的哪个地方，姿态是指相机的朝向。
2. 点和向量构成空间中最基本的元素，我们规定坐标系中的点我们可用以原点为起点、该点为终点的向量描述。
3. 向量和坐标是两个概念；向量是在空间中客观存在的定值，在一个给定的坐标系中，其坐标值才有意义。
4. 三维空间中的某个向量的坐标可以用 ${\mathbb{R}}^3$ 当中的三个数来描述。某个点的坐标也可以用 ${\mathbb{R}}^3$来描述。
5. 描述方法：当确定了一个坐标系后，即可以确定一个线性空间的基 (${\mathbf{e}}_1$, ${\mathbf{e}}_2$, ${\mathbf{e}}_3$)。例如，向量$\mathbf{a}$在上述坐标系中的形式为：
   
6. 坐标系通常由三个正交的坐标轴组成。
7. 给定 $\mathbf{x}$ 和 $\mathbf{y}$ 轴时， $\mathbf{z}$ 就可以通过右手（或左手）法则由 $\mathbf{x}\times \mathbf{y}$ 定义出来。
8. 根据定义方式的不同，坐标系又分为左手系和右手系。左手系的第三个轴与右手系相反。通常用右手系。
9. 向量的重要计算：
   1. 内积：描述了向量间的投影关系
   2. 外积：外积只对三维向量存在定义，外积的方向垂直于这两个向量，大小为 $\left|\mathbf{a}\right|\left|\mathbf{a}\right|sin⟨\mathbf{a},\mathbf{b}⟩$，是两个向量张成的四边形的有向面积。
      
      引入了 ^ 符号，把 $\mathbf{a}$ 写成一个矩阵，^ 是反对称符号，$\mathbf{a}$^ 是反对称矩阵。

3.1.2 坐标系中的欧式变换

1. 旋转关系加上平移关系统称为坐标系之间的变换关系。
2. 我们用两个参考系考察机器人运动，一个是固定不动的世界坐标系，一个是依靠机器人自身建立的移动坐标系，我们研究的问题就是如何把一个向量在这两个坐标系中相互转换。
3. 转换思路：需要先得到该点针对机器人坐标系坐标值，再根据机器人位姿转换到世界坐标系中。
   
4. 欧式变换：同一个向量在各个坐标系下的长度和夹角都不会发生变化的变换。一个欧氏变换由一个旋转和一个平移两部分组成。
5. 设某个单位正交基 $({\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3)$ 经过一次旋转，变成了 $({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime })$ 。对于同一个向量 $\mathbf{a}$（注意该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 $[a_1,a_2,a_3{]}^T$ 和 $[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$。根据坐标的定义，有：
   
   所以
   
6. 上述的$\mathbf{R}$被称为旋转矩阵：矩阵由两组基之间的内积组成，刻
   画了旋转前后同一个向量的坐标变换关系。
7. 旋转矩阵的性质：
   1. 行列式为1
   2. 正交阵
   3. 对于同一个旋转变化，对应唯一的旋转矩阵
   4. 旋转矩阵的充要条件：行列式为 1 的正交矩阵
   5. 集合定义：$SO(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}|\mathbf{R}{\mathbf{R}}^{\mathbf{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}$
8. 旋转矩阵为正交阵，它的逆（即转置）描述了一个相反的旋转
                          
9. 考虑平移，设平移向量为 $\mathbf{t}$ ，旋转矩阵为 $\mathbf{R}$ ，则
                        

3.1.3 变换矩阵与齐次坐标

1. 齐次坐标：射影几何里的概念，把一个三维向量的末尾添加 1，变成了四维向量，称为齐次坐标。
2. 变换矩阵 $\mathbf{T}$：4 × 4矩阵，左上角是 3 × 3 旋转矩阵 $\mathbf{R}$ ，下面是3 × 1零向量，右面是1 × 3平移向量 $\mathbf{t}$ ，右下角是1。用 $\tilde{\mathbf{a}}$ 表示 $\mathbf{a}$ 的齐次坐标。
               
3. 在齐次坐标中，某个点 x 的每个分量同乘一个非零常数 k 后， 仍然表示的是同一个点，因此，一个点的具体坐标值不是唯一的。当最后一项不为零时，我们总可以把所有坐标除以最后一项，强制最后一项为 1，从而得到一个点唯一的坐标表示。忽略掉最后一项，这个点的坐标和欧氏空间就是一样。
               
4. 依靠齐次坐标和变换矩阵，两次变换的累加就可以有很好的形式。
               
5. 特殊欧式群：左上角为旋转矩阵，右侧为平移向量，左下角为 0 向量，右下角为 1。
   
6. 变换矩阵的逆，表示反向变换。
               

3.2 Eigen库的使用

参考链接：

1. Eigen官网
2. 民间Eigen教程

3.3 旋转向量和欧拉角

3.3.1 旋转向量

1. 旋转矩阵有9个量，但只有3个自由度；变换矩阵由16个量，但只有6个自由度，表达冗余，浪费空间。
2. 任意旋转都可以用一个旋转轴和一个旋转角来刻画。
3. 旋转向量（轴角，角轴）：方向与旋转轴一致，长度等于旋转角的向量。
4. 旋转向量有3个量和3个自由度，使用一个旋转向量和一个平移向量即可表达一次变换。
5. 旋转向量和旋转向量的转换：
   1. 旋转向量==>旋转矩阵：罗德里格斯公式
      设旋转轴向量（单位向量）$\mathbf{n}$ ，旋转角为 $\theta$ ，则旋转向量为$\theta \mathbf{n}$ ，罗德里格斯公式如下：
             
   2. 旋转矩阵==>旋转向量，公式如下：
             
                    $\{\begin{array}{l}\theta =\arccos (\frac{tr(\mathbf{R})-1}{2})\\ \mathbf{R}\mathbf{n}=\mathbf{n}\end{array}$
      转轴 n 是矩阵 R 特征值 1 对应的特征向量，旋转轴经过旋转之后不变。

3.3.2 欧拉角

1. 欧拉角使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。
2. 由于分解方式有许多种，所以欧拉角存在着不同的定义方法。而且还需要区分每次旋转是绕固定轴旋转的，还是绕旋转之后的轴旋转的。
3. 欧拉角常使用“偏航-俯仰-滚转”（yaw-pitch-roll），等价于 ZY X 轴的旋转。使用 $[r,p,y{]}^T$ 这样一个三维的向量描述任意旋转，注意是先 $y$ 再 $p$ 再 $r$ 。
4. 万向锁：在俯仰角为$\pm 90^{◦}$ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转），也被称为奇异性问题。理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性问题。
5. 由于奇异性问题，欧拉角不适于插值和迭代，往往只用于人机交互中，也很少出现在slam中。
   

3.4 四元数

3.4.1 四元数的定义

1. 四元数是扩展的复数，由一个实部和三个虚部构成，它是紧凑的，没有歧义性
                      
2. 虚部满足如下关系式：
                      
3. 四元数还可以表示成如下表达式：
        
   s 称为四元数的实部，而 $\mathbf{v}$ 称为它的虚部。如果一个四元数虚部为 $0$，称之为实四元数。反之，若它的实部为 0，称之为虚四元数。
4. 模长为 1 的复数，可以表示复平面上的纯旋转。
5. 用单位四元数表示三维空间中任意一个旋转。设某个旋转是绕单位向量 $n=[n_x,n_y,n_z{]}^T$ 进行了角度为 $\theta$ 的旋转，那么这个旋转的四元数形式为
          
6. 四元素到旋转向量的转换
                 
7. 在四元数中， 任意的旋转都可以由两个互为相反数的四元数表示。
8. 取 $\theta$ 为 0，则得到一个没有任何旋转的实四元数。

3.4.2 四元数的运算

设两个四元数为：${\mathbf{q}}_{\mathbf{a}}=s_a+x_{a}i+y_{a}j+z_{a}k$ , ${\mathbf{q}}_{\mathbf{b}}=s_b+x_{b}i+y_{b}j+z_{b}k$

1. 加减法：
                 
2. 乘法：括号打开，按照实部和虚部分别整理
          
   用向量和内外积形式表示：
          
   注意四元数乘法是不可交换的，除非 ${\mathbf{v}}_{\mathbf{a}}$ 和 ${\mathbf{v}}_{\mathbf{b}}$ 在 ${\mathbb{R}}^3$ 中共线（外积项为零）
3. 共轭：把虚部取成相反数
                 
   四元数与其共轭相乘等于模的平方：
                 
4. 模长
   1. 定义：各个部系数平方和开根号
                    
   2. 四元数乘积的模等于模的乘积，对于单位四元数，则意味着单位四元数乘积仍为单位四元数
                    
5. 逆
   1. 定义：
                    
   2. 性质
      1. 与逆相乘可交换、与逆相乘为1
                       
      2. 乘积取逆等于交换顺序取逆的乘积
                       
6. 数乘和点乘
   1. 数乘
                    
   2. 点乘
               

3.4.3 用四元数表示旋转

1. 三维空间的点用虚四元数描述：设 $p=[x,y,z]\in {\mathbb{R}}^3$ ，则
                  
2. 旋转也用四元数表示
                  
3. 则旋转后的向量为：$\mathbf{p}\prime =\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}$ ，旋转后的向量的对应四元数实部仍然为0

3.4.4 四元数到旋转矩阵的转换

1. 四元数==>旋转矩阵：设四元数 $\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$ ，则
   
2. 旋转矩阵==>四元数：设矩阵为 $\mathbf{R}=\{m_{ij}\},i,j\in [1,2,3]$ ，则
   
3. 一个 $\mathbf{R}$ 对应的四元数表示并不惟一。

*3.5 相似、仿射、射影变换

3.5.1 相似变换

1. 相似变换比欧氏变换多了一个自由度，它允许物体进行均匀的缩放，其矩阵如下：
                  
   $s$称为缩放因子，表示在对向量旋转之后，可以在 $x,y,z$ 三个坐标上进行均匀的缩放
2. 由于含有缩放，相似变换不再保持图形的面积不变。

3.5.2 仿射变换

1. 仿射变换也称正交投影，变换矩阵如下
                  
2. 仿射变换只要求 A 是一个可逆矩阵，而不必是正交矩阵。
3. 经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。

3.5.3 射影变换

1. 变换矩阵：
                  
2. 左上角为可逆矩阵 $\mathbf{A}$，右上为平移 $\mathbf{t}$ ，左下缩放 ${\mathbf{a}}^{\mathbf{T}}$。
3. 2D 的射影变换一共有 8 个自由度， 3D 则共有 15 个自由度。