SLAM—三维空间刚体运动

刚体不光有位置，还有自身的姿态。相机也可以看成三维空间的刚体，于是位置是指相机在空间中的哪个地方，而姿态则是指相机的朝向。

内积：点乘，对应坐标积之和。外积：叉乘，可表示成一个反对称矩阵乘以向量。

相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧氏变换。想象你把手机抛到空中，在它落地摔碎之前，只可能有空间位置和姿态的不同，而它自己的长度、各个面的角度等性质不会有任何变化。这样一个欧氏变换由一个旋转和一个平移两部分组成。

旋转矩阵R描述了旋转本身。旋转矩阵可以描述相机的旋转。它是一个行列式为1 的正交矩阵¬。反之，行列式为1 的正交矩阵也是一个旋转矩阵。所以，我们可以把旋转矩阵的集合定义如下：
SO(n) = {R属于 Rn*n|RRT = I; det(R) = 1}

但这里的变换关系不是一个线性关系。假设我们进行了两次变换，会变得比较复杂。因此增加一个自由度，引入齐次坐标的概念。

把一个三维向量的末尾添加1，变成了四维向量，称为齐次坐标。对于这个四维向量就可以把旋转和平移写在一个矩阵（称为变换矩阵T）里面，使得整个关系变成了线性关系。

 

关于变换矩阵 T，它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为 0 向量，右下角为 1。这种矩阵又称为特殊欧氏群（Special Euclidean Group）SE(3)：

 任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量。假设有一个旋转轴为 n，角度为 θ 的旋转，显然，它对应的旋转向量为 θn。由旋转向量到旋转矩阵的过程由罗德里格斯公式给出。

欧拉角：用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。由于它等价于 ZY X 轴的旋转。可以使用 [r; p; y]T 这样一个三维的向量描述任意旋转。
1. 绕物体的 Z 轴旋转，得到偏航角 yaw；
2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
3. 绕旋转之后的 X 轴旋转，得到滚转角 roll。

欧拉角的表示方式存在万向锁问题：在俯仰角为±90◦ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度。由于这种奇异性，欧拉角不适于插值和迭代。
回忆我们以前学习过的复数。我们用复数集 C 表示复平面上的向量，而复数的乘法则能表示复平面上的旋转：例如，乘上复数 i 相当于逆时针把一个复向量旋转 90 度。类似的，在表达三维空间旋转时，也有一种类似于复数的代数： 四元数（Quaternion）。四元数是 一种扩展的复数. 它既是紧凑的，也没有奇异性。缺点：四元数不够直观，其运算稍为复杂一些。一个四元数 q 拥有一个实部和三个虚部。i,j,k为三个虚部。

假设某个旋转是绕单位向量 n = [nx; ny; nz]T进行了角度为 θ 的旋转，那么这个旋转的四元数形式为：
 

在四元数中， 任意的旋转都可以由两个互为相反数的四元数表示。
用四元数表示旋转
假设一个空间三维点 p = [x,y, z] 属于R3，以及一个由轴角 n，θ 指定的旋转。三维点 p 经过旋转之后变成为 p′。如果使用矩阵描述，那么有 p′ = Rp。如果用四元数描述旋转，

首先，把三维空间点用一个虚四元数来描述：p = [0， x， y， z] = [0， v]，然后用四元数 q 表示这个旋转：q = [cos θ/2; nsinθ/2]，那么，旋转后的点 p′ 即可表示为这样的乘积：。

四元数到旋转矩阵的转换

最后，无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。