3.1 旋转矩阵

3.1.1 点、向量和坐标系

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

3.1.2 坐标系中的欧式变换

旋转关系加上平移关系统称为坐标系之间的变换关系。
我们用两个参考系考察机器人运动，一个是固定不动的世界坐标系，一个是依靠机器人自身建立的移动坐标系，我们研究的问题就是如何把一个向量在这两个坐标系中相互转换。
转换思路：需要先得到该点针对机器人坐标系坐标值，再根据机器人位姿转换到世界坐标系中。
欧式变换：同一个向量在各个坐标系下的长度和夹角都不会发生变化的变换。一个欧氏变换由一个旋转和一个平移两部分组成。
设某个单位正交基 $(\bm{e_1}, \bm{e_2}, \bm{e_3})$ 经过一次旋转，变成了 $(\bm{e_1'}, \bm{e_2'}, \bm{e_3'})$ 。对于同一个向量 $\bm{a}$ （注意该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 $[a_1, a_2, a_3]^T$ 和 $[a_1', a_2', a_3']^T$ 。根据坐标的定义，有：

所以
上述的 $\bm{R}$ 被称为旋转矩阵：矩阵由两组基之间的内积组成，刻
画了旋转前后同一个向量的坐标变换关系。
旋转矩阵的性质：
1. 行列式为1
2. 正交阵
3. 对于同一个旋转变化，对应唯一的旋转矩阵
4. 旋转矩阵的充要条件：行列式为 1 的正交矩阵
5. 集合定义： $SO(n) = \left\{\bm{R}\in \R^{n×n} | \bm{RR^T} = \bm{I}, \det(\bm{R}) = 1\right\}$
旋转矩阵为正交阵，它的逆（即转置）描述了一个相反的旋转
考虑平移，设平移向量为 $\bm{t}$ ，旋转矩阵为 $\bm{R}$ ，则

3.1.3 变换矩阵与齐次坐标

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

3.2 Eigen库的使用

参考链接：

3.3 旋转向量和欧拉角

3.3.1 旋转向量

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

3.3.2 欧拉角

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

3.4 四元数

3.4.1 四元数的定义

四元数是扩展的复数，由一个实部和三个虚部构成，它是紧凑的，没有歧义性
虚部满足如下关系式：
四元数还可以表示成如下表达式：

s 称为四元数的实部，而 $\bm{v}$ 称为它的虚部。如果一个四元数虚部为 $\bm{0}$ ，称之为实四元数。反之，若它的实部为 0，称之为虚四元数。
模长为 1 的复数，可以表示复平面上的纯旋转。
用单位四元数表示三维空间中任意一个旋转。设某个旋转是绕单位向量 $n = [n_x, n_y, n_z]^T$ 进行了角度为 $θ$ 的旋转，那么这个旋转的四元数形式为
四元素到旋转向量的转换
在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。
取 $θ$ 为 0，则得到一个没有任何旋转的实四元数。

3.4.2 四元数的运算

设两个四元数为： $\bm{q_a} = s_a + x_ai + y_aj + z_ak$ , $\bm{q_b} = s_b + x_bi + y_bj + z_bk$

加减法：
乘法：括号打开，按照实部和虚部分别整理

用向量和内外积形式表示：

注意四元数乘法是不可交换的，除非 $\bm{v_a}$ 和 $\bm{v_b}$ 在 $\R^3$ 中共线（外积项为零）
共轭：把虚部取成相反数

四元数与其共轭相乘等于模的平方：
模长
1. 定义：各个部系数平方和开根号
2. 四元数乘积的模等于模的乘积，对于单位四元数，则意味着单位四元数乘积仍为单位四元数
逆
1. 定义：
2. 性质
  1. 与逆相乘可交换、与逆相乘为1
  2. 乘积取逆等于交换顺序取逆的乘积
数乘和点乘
1. 数乘
2. 点乘