视觉SLAM第三讲

本讲将介绍视觉 SLAM 的基本问题之一：三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角。

点、向量和坐标系

点：空间当中的基本元素，没有长度，没有体积。
向量：可以将向量看作从一个点指向另一个点的箭头，它是空间中的一个实体。向量不同于坐标，只有在指定了三维空间中的某个坐标系后，才能讨论该向量在此坐标系下的坐标，即通过若干个实数来表示这个向量。

向量内积：

$\boldsymbol{a} \cdot \boldsymbol{b}=\boldsymbol{a}^{\mathrm{T}} \boldsymbol{b}=\sum_{i=1}^{3} a_{i} b_{i}=|\boldsymbol{a}||\boldsymbol{b}| \cos \langle\boldsymbol{a}, \boldsymbol{b}\rangle$

向量外积：

在这里插入图片描述

反对称矩阵：

$\boldsymbol{a}^{\wedge}=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\a_{3} & 0 & -a_{1} \\-a_{2} & a_{1} & 0\end{array}\right]$

坐标系间的欧式变换：

通过坐标系欧式变换（旋转+平移）表征刚体运动是一种常见的方法。

在实际场景中，我们通常定义各种坐标系。例如，在机器人学中，每个连杆和关节都有自己的坐标系。如果考虑移动的机器人，通常设定一个固定的世界坐标系，同时相机或机器人自身则是一个移动坐标系。刚体坐标系相对于参考坐标系的变换，即是其局部定位的过程。

旋转矩阵的性质：

正交性

旋转矩阵是一个正交矩阵，这意味着它的列向量（或行向量）是正交的，并且都是单位向量。 $\mathbf{R}^T \mathbf{R} = \mathbf{R} \mathbf{R}^T = \mathbf{I}，\mathbf{R}^{-1} = \mathbf{R}^T$
行列式为1

$\det(\mathbf{R}) = 1$ ，旋转矩阵保持向量的长度（或范数）不变， $||\mathbf{R} \mathbf{v}|| = ||\mathbf{v}||$ （矩阵的范数等于自身的内积平方根）。

变换矩阵与齐次坐标

$\boldsymbol{a}'=\boldsymbol{Ra}+\boldsymbol{t}$

该公式完整地表达了欧氏空间的旋转与平移，不过还存在一个小问题：这里的变换关系不是一个线性关系。假设我们进行了两次变换：

$\boldsymbol{b}=\boldsymbol{R_{1} a}+\boldsymbol{t_{1} }，\boldsymbol{c}=\boldsymbol{R_{2} b}+\boldsymbol{t_{2} }$

那么，从 $\boldsymbol{a}$ 到 $\boldsymbol{c}$ 的变换为：

$\boldsymbol{c}=\boldsymbol{R_{2}(\boldsymbol{R_{1} a}+\boldsymbol{t_{1} })}+\boldsymbol{t_{2} }$

这样的形式在变换多次之后会显得很啰嗦。因此，我们引入齐次坐标和变换矩阵：

$\left[\begin{array}{l}\boldsymbol{a}^{\prime} \\1\end{array}\right]=\left[\begin{array}{ll}\boldsymbol{R} & \boldsymbol{t} \\\mathbf{0}^{\mathrm{T}} & 1\end{array}\right]\left[\begin{array}{l}\boldsymbol{a} \\1\end{array}\right] \triangleq \boldsymbol{T}\left[\begin{array}{l}\boldsymbol{a} \\1\end{array}\right]$

这是一个数学技巧：我们在一个三维向量的末尾添加1，将其变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成线性关系。该式中，矩阵 $\mathbf{T}$ 称为变换矩阵（Transform Matrix）。

在不引起歧义的情况下，以后我们就直接把它写成 $\mathbf{b} =\mathbf{Ta}$ 的样子，默认其中进行了齐次坐标的转换。

旋转向量和四元数

旋转向量

矩阵表示方式至少有以下几个缺点：

旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。同理，变换矩阵用16个量表达了6自由度的变换。那么，是否有更紧凑的表示呢？
旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更困难。

一种紧凑地描述旋转的方式：任意旋转都可以用一个旋转轴和一个旋转角来刻画。

于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。

假设旋转轴为一个单位长度的向量 $\mathbf{n}$ ，角度为 $\mathbf{\theta }$ ，那么向量 $\mathbf{\theta n}$ 也可以描述这个旋转，从旋转向量到旋转矩阵的转换过程由罗德里格斯公式：

$\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}+\sin \theta \boldsymbol{n}^{\wedge}$

反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 $\mathbf{\theta }$ ，取两边的迹，有：

在这里插入图片描述

因此：

$\theta =\arccos (\frac{\mathbf{tr}(R)-1 }{2} )$

关于转轴 $\mathbf{n}$ ，由于旋转轴上的向量在旋转后不发生改变，说明：

$R n = n$

因此，转轴 $\mathbf{n}$ 是矩阵 $\mathbf{R}$ 特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

四元数

四元数的定义

四元数 $q$ 通常表示为 $q = w + x i + y j + z k$ ，其中 $w, x, y, z$ 是实数， $i, j, k$ 是虚单位，满足以下乘法规则：

$i^{2} =j^{2} =k ^{2} =ijk=−1$

四元数可以用向量形式表示为：

$q=(w,\mathbf{v} )$

其中 $w$ 是标量部分， $v = (x, y, z)$ 是向量部分。

四元数表示旋转

四元数可以有效地表示三维空间中的旋转。给定旋转轴 $\mathbf{u}$ 和旋转角度 $θ$ ，我们可以构造一个四元数 $q$ 来表示旋转：

$q=\mathrm{cos} ( \frac{\theta}{2} )+\mathrm{sin} (\frac{\theta}{2})(\mathbf{u} ⋅\mathbf{i} )$

其中 $\mathbf{u}$ 是单位向量，表示旋转轴， $\mathbf{u}=(u_{x},u_{y},u_{z} )$ ，且 $\mathbf{i}=(i,j,k )$ 。

更具体地，四元数 $q$ 的分量为：

$q=(\mathrm{cos} (\frac{\theta}{2}),u_{x} \mathrm{sin} (\frac{\theta}{2}),u_{y} \mathrm{sin} (\frac{\theta}{2}),u_{z} \mathrm{sin} (\frac{\theta}{2}))$

四元数旋转向量

如果我们要旋转一个三维向量 $p$ ，使用四元数的方法如下：

构造旋转四元数：

$q=(\mathrm{cos} (\frac{\theta}{2}),u_{x} \mathrm{sin} (\frac{\theta}{2}),u_{y} \mathrm{sin} (\frac{\theta}{2}),u_{z} \mathrm{sin} (\frac{\theta}{2}))$
构造向量四元数（纯四元数）：

$p=(0,p_{x},p_{y},p_{z} )$
计算旋转后的向量：
使用四元数乘法进行旋转：

$p' =qpq^{-1}$

其中， $q^{-1}$ 等于 $q$ 的共轭（ $q^{-1}=\bar{q} / \parallel q \parallel ^{2}$ ，共轭也就是实部不变，虚数相反）。

习题p6:

一般线性方程 $A x = b$ 有哪几种做法？

从解析解和数值解分析

解析解

解析解（Analytical Solution）指的是通过数学公式或代数操作直接求出精确解。对于线性方程组 $A x = b$ ，常用的方法有：
1. 高斯消元法：
  通过行变换将矩阵 $A$ 化为上三角矩阵，然后通过回代求解 $x$ 。
  - 优点：可以得到精确解。
  - 缺点：计算复杂度较高，适用于较小规模的线性方程组。
2. 矩阵逆：
  如果矩阵 $A$ 是可逆的，则解可以表示为 $x=A^{-1} b$ 。
  - 优点：直接使用矩阵运算得到精确解。
  - 缺点：计算矩阵逆的过程复杂度高（ $O(n^{3})$ ），并且数值不稳定；对于大规模矩阵和病态矩阵不适用。
3. 克拉默法则：
  如果 $A$ 是方阵且行列式不为零，则每个未知数可以用行列式表示。
  - 优点：概念简单。
  - 缺点：仅适用于小规模方程组，计算行列式复杂度高（ $O (n!)$ ）。
数值解

数值解（Numerical Solution）指的是通过数值方法近似求解方程组。常用的数值解方法有：
1. 迭代法：
  - 雅可比迭代法（Jacobi Method）：依次更新每个未知数。
  - 高斯-赛德尔迭代法（Gauss-Seidel Method）：每次迭代使用最新的解。
  - 超松弛迭代法（SOR Method）：对高斯-赛德尔方法进行加速。
  - 优点：适用于大型稀疏矩阵。
  - 缺点：收敛性依赖于矩阵的特性，可能需要很多迭代次数。
2. 直接法：
  - LU 分解：将矩阵 $A$ 分解为下三角矩阵 $L$ 和上三角矩阵 $U$ ，然后解两个三角方程组。
  - QR 分解：将矩阵 $A$ 分解为正交矩阵 $Q$ 和上三角矩阵 $R$ ，然后解三角方程组。
  - Cholesky 分解：如果矩阵 $A$ 是正定的，则可以分解为 $LL^{T}$ ，其中 $L$ 是下三角矩阵。
  - 优点：比计算矩阵逆更稳定、更高效，适用于大规模或病态矩阵。
  - 缺点：仍然是 $O(n^{3})$ 复杂度，对于非常大的矩阵可能不适用。
比较与选择
- 解析解：
  - 优点：如果可行，能提供精确解。
  - 缺点：仅适用于小规模问题，且计算复杂度高，数值不稳定。
- 数值解：
  - 优点：适用于大规模问题和稀疏矩阵，计算速度快，方法稳定。
  - 缺点：提供的是近似解，可能存在数值误差，收敛性依赖于问题特性。
在实际应用中，选择解析解还是数值解通常取决于问题的规模、矩阵的特性和计算资源。对于小规模的线性方程组或特定类型的矩阵（如对称正定矩阵），解析解是一个可行的选择。而对于大规模的线性方程组或稀疏矩阵，数值解通常是更好的选择。

数值解和解析解是解决数学方程或问题的两种不同方法。它们各自有其优缺点和适用范围。下面详细介绍它们的定义、区别、优缺点。

定义

解析解

解析解是通过代数操作或解析函数得到的精确解。它通常用数学表达式来表示，并且能提供问题的精确解。
数值解

数值解是通过数值方法和计算机算法近似求得的解。它提供了问题的近似解，通常以数值形式表示。

区别

形式：
- 解析解：以数学公式或解析表达式的形式给出。
- 数值解：以数值结果或近似值的形式给出。
精度：
- 解析解：精确解，假设没有计算误差。
- 数值解：近似解，精度取决于算法、步长、计算机精度等因素。
求解过程：
- 解析解：通常通过符号运算、代数技巧或解析函数求得。
- 数值解：通常通过迭代算法、数值方法或计算机程序求得。
适用范围：
- 解析解：适用于方程或问题有明确的解析表达式，并且能通过代数操作求解的情况。
- 数值解：适用于复杂方程或问题解析求解困难或无解析解的情况。