视觉SLAM十四讲（2）———三维空间刚体运动

视觉SLAM十四讲（2）——三维空间刚体运动

• 旋转矩阵

• • 1：点和向量，坐标系
  • 2：坐标系间的欧式变换
  • 3：变换矩阵与齐次坐标

• 旋转向量和欧拉角

• • 1：旋转向量
  • 2：欧拉角

• 四元数

• • 1：定义
  • 2：四元数的运算
  • 3：用四元数表示旋转
  • 4：四元数到旋转矩阵的转换

• 相似/仿射/射影变换

旋转矩阵

1：点和向量，坐标系

相机可以被视为三维空间中的一个刚性物体；其位置则表示其在空间中的具体位置；而姿态则描述了其朝向。

如果我们确定了一个坐标系，也就是一个线性空间的基(e_1,e_2,e_3)，那么就可以谈论向量a在这组基下的坐标：
a=[e_1,e_2,e_3]\left[ \begin{matrix} a_1\\ a_2 \\ a_3 \end{matrix} \right]=a_1e_1+a_2e_2+a_3e_3
对于a\cdot b\in \mathbb{R}^{3}，内积 可以写成：
a\cdot b=a^{T}b=\sum_{i=1}^{3}a_{i}b_{i}=\left | a \right |\left | b \right |cos<a,b >
内积可以描述向量间的投影关系。
外积 可以写成：
a\times b=\begin{Vmatrix} i &j &k \\ a_{1}&a_{2} &a_{3} \\ b_{1} &b_{2} &b_{3} \end{Vmatrix}=\begin{bmatrix} a_{2}b_{3}-a_{3}b_{2}\\ a_{3}b_{1}-a_{1}b_{3}\\ a_{1}b_{2}-a_{2}b_{1} \end{bmatrix}=\begin{bmatrix} 0 &-a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2}& a_{1} & 0 \end{bmatrix}b\doteq a^{\wedge }b
外积的方向垂直于这两个向量，我们引入了^符号，这样就把外积写成了矩阵与向量的乘法。外积可以表示向量的旋转 ，大拇指的朝向就是旋转向量的方向。

2：坐标系间的欧式变换

坐标系间的变换关系：旋转+平移

以下是对输入文本的改写版本

3：变换矩阵与齐次坐标

我们在一个三维向量的后部赋予1值，并将其转换为四维向量来构成齐次坐标。\n\n\begin{bmatrix} a^{'}\\ 1 \end{bmatrix}=\begin{bmatrix} R & t\\ 0& 1 \end{bmatrix}\begin{bmatrix} a\\ 1 \end{bmatrix}\doteq T\begin{bmatrix} a\\ 1 \end{bmatrix}矩阵T被称为变换矩阵，在此定义下求其逆矩阵将执行反向变换。\n\n在齐次坐标系统中，在不考虑缩放的情况下（即当最后一项不为零时），可以通过将所有分量除以最后一项使其归一化，并得到该点唯一的坐标表示。\n\n关于变换矩阵T而言，在特殊欧氏群SE(3)中具有特殊的结构：\nSE(3)=\begin{Bmatrix} T=\begin{bmatrix} R & t\\ 0& 1 \end{bmatrix}\in \mathbb{R}^{4\times 4}|R\in SO(3),t\in \mathbb{R}^{3}) \end{Bmatrix}

旋转向量和欧拉角

1：旋转向量

在三维空间中旋转变换通常由九个参数来描述其运动状态；而六自由度的空间刚体变换则需要十六个参数来描述其运动状态。为了便于后续分析和计算需求，我们期望能够找到一种更为简洁明了的表现形式。

在坐标系中进行任意的旋转变换时,都可以用一个明确的转轴及其相应的转动角度来进行描述.由此可知,在这种情况下我们能够找到这样一个向量:其方向与转轴一致且长度等于转动角度.这种向量被称为旋转向量,它是一种通过大小和方向来表征空间中某一点绕某条直线作等距变换的方式.因此这种方法只需要三个参数就能完整地描述一次三维空间中的旋转变换.

同样地针对该变换矩阵... ，能够表示为一个旋转参数和一个平移参数的组合。此时其维度正好为6

旋转向量与旋转矩阵之间如何相互转换？假设给定一个旋转轴n及其对应的角度\theta的旋转变换，则其对应的旋转向量表示为\theta n。根据罗德里格斯公式可知：
R = \cos\theta I + (1-\cos\theta)nn^{\top} + \sin\theta n^{\wedge}
其中符号\wedge代表向量到反对称矩阵的转换操作符。反之，在已知特定角度\theta的情况下：
\theta = \arccos\left( \frac{\text{tr}(R) - 1}{2} \right)
对于确定旋转轴的问题，则可从以下条件出发：
由于位于旋转轴上的向量在变换前后保持不变：
Rn = n
因此，在这种情况下，转轴n即为矩阵R对应特征值1的特征向量。通过求解此方程并将其归一化处理即可得到完整的旋转轴信息。

2：欧拉角

考虑到旋转矩阵和旋转向量描述的旋转不易理解, 欧拉角则通过一种直观的方式提供了这种描述——它使用三个分离的角度, 即'偏航-俯仰-滚转'(yaw-pitch-roll), 来表示这个过程, 等价于绕着Z-Y-X轴依次进行这三个角度的操作。

1. 物体在Z轴上进行旋转变动以获得偏航角yaw。
2. Y轴被物体在经过一次绕其他坐标轴后的旋转变动所获得。
3. X轴被物体在经过一次绕其他坐标轴后的旋转变动所获得。

此时，可以用[r,p,y]^{T}这样的三维向量描述任意旋转。

欧拉角的主要缺陷在于会遇到着名的万向锁现象。当仰角达到±90度时，在这种情况下前两次旋转将在同一轴线上（即x轴与z轴重合）进行操作,从而导致系统失去一个自由度

四元数

1：定义

使用9个参数来表示3自由度的旋转，在这种情况下存在冗余；尽管如此，在某些情况下欧拉角和旋转向量仍然保持紧凑性，并且具备一定的局限性。

四元数被称为由Hamilton发现的一种对复数进行扩展的数学结构，在其数学结构上是对复数的一种扩展。该理论体系具有高效表达能力并能够有效避免了奇异点问题。然而，在实际应用中其运算相对繁琐，并不直观。

一个四元数q拥有一个实部，三个虚部：
q=q_0+q_1i+q_2j+q_3k
这三个虚部满足以下关系：
\left\{\begin{matrix} i^{2}=j^{2}=k^{2}=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ij=-j \end{matrix}\right.
我们能用单位四元数 表示三维空间中任意一个旋转。
假设某个旋转是绕单位向量n=[n_x,x_y,n_z]^{T}进行了角度为 \theta的旋转，那么这个旋转的四元数形式为：
q=\begin{bmatrix} cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2} \end{bmatrix}^{T}
把四元数的三个虚部与空间中的3个轴相对应 。

另外一种方法是用一个实数和一个向量来表示四元数：
q=[s,v],s=q_{0}\in \mathbb{R},v=[q_{1},q_{2},q_{3}]^{T}\in \mathbb{R}^{3}

另外一种方法是用一个实数和一个向量来表示四元数：
q=[s,v],s=q_0\in\mathbb{R}, v=\begin{bmatrix} q_1 \\ q_2 \\ q_3 \end{bmatrix}\in\mathbb{R}^3

反过来也可以通过单位四元数推导出对应的旋转轴与夹角：
\left\{\begin{matrix} \theta = 2 \arccos q_0\\ [n_x, n_y, n_z]^T = [q_1, q_2, q_3]^T / \sin\frac{\theta}{2} \end{matrix}\right.

这个式子给人一种部分旋转的印象。同样地，在处理θ+2π的情况时也会出现相同的旋转效果，不过此时对应的四元数值变为其相反数。因此，在旋转变换中任意一次旋转都可通过一对互为相反数的四元数组合来实现

2：四元数的运算

加减法/乘法/共轭/模长/逆/数乘与点乘

3：用四元数表示旋转

给定一个位于三维空间中的点p=[x,y,z]（其中x,y,z\in\mathbb{R}），并设有一个由轴向量n与旋转角度\theta所定义的旋转变换操作。该三维点经过旋转变换后得到新的位置p'。

如果使用矩阵描述 ，那么有p^{'}=Rp。

若采用四元数来描述，则将三维空间中的点通过虚四元数表示为p = [0, x, y, z] = [0, \mathbf{v}]（其中虚部对应于三个坐标轴）。接着以四元数q = [\cos(\theta/2), n\sin(\theta/2)]来表示该旋转变换（其中n为旋转轴单位向量）。则变换后的点p' = qpq^{-1}即为此旋转变换的结果。

4：四元数到旋转矩阵的转换

旋转可以用单位四元数/旋转矩阵/旋转向量描述。
四元数到旋转矩阵的转换 ：
设四元数q=q_0+q_1i+q_2j+q_3k，对应旋转矩阵R为：R=\begin{bmatrix} 1-2q_{2}^{2}-2q_{3}^{2} & 2q_{1}q_{2}-2q_{0}q_{3} & 2q_{1}q_{3}+2q_{0}q_{2}\\ 2q_{1}q_{2}+2q_{0}q_{3}&1-2q_{1}^{2}-2q_{3}^{2} & 2q_{2}q_{3}-2q_{0}q_{1}\\ 2q_{1}q_{3}-2q_{0}q_{2}& 2q_{2}q_{3}+2q_{0}q_{1} & 1-2q_{1}^{2}-2q_{2}^{2} \end{bmatrix}

该方法将旋转矩阵转换为四元数表示。
设该旋转矩阵R=\begin{Bmatrix} m_{i,j} \end{Bmatrix} ，其中元素m_i j分别代表第i行第j列的位置。
其相应的四元数q则由以下公式确定：
计算出的各分量分别为 q_0 = \frac{\sqrt{\textit,tr(R) + 1}}}{2}，
q_1 = \frac{{m}_{23}-{m}_{32}}}{4 q_0}，
q_2 = \frac{{m}_{31}-{m}_{13}}}{4 q_0}，
q_3 = \frac{{m}_{12}-{m}_{21}}}{4 q_0}。

相似/仿射/射影变换

在三维空间中不仅存在欧氏变换之外还有其它若干种几何转换方式。欧氏变换通过维持向量的模长及其间的夹角特点等同于对一个刚体进行无变形的移动或旋转操作从而使其形态保持不变这一行为与其它几何转换相比则会引起物体形状的变化。
这些几何转换都可以用矩阵形式来表示