视觉SLAM第三讲

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

点、向量和坐标系

• 点：空间当中的基本元素，没有长度，没有体积。

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

向量内积：

$\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^{\mathrm{T}}\mathbf{b}={\sum }_{i=1}^3{a}_i{b}_i=|\mathbf{a}||\mathbf{b}|\cos ⟨\mathbf{a},\mathbf{b}⟩$

向量外积：

反对称矩阵：

${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$

坐标系间的欧式变换：

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

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

旋转矩阵的性质：

1. 正交性

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

2. 行列式为1

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

变换矩阵与齐次坐标

${\mathbf{a}}^{\prime }=\mathbf{Ra}+\mathbf{t}$

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

$\mathbf{b}={\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1，\mathbf{c}={\mathbf{R}}_2\mathbf{b}+{\mathbf{t}}_2$

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

$\mathbf{c}={\mathbf{R}}_2({\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1)+{\mathbf{t}}_2$

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

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

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

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

旋转向量和四元数

旋转向量

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

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

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

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

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

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

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

因此：

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

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

$Rn=n$

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

四元数

四元数的定义

四元数$q$通常表示为$q=w+xi+yj+zk$，其中$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}$和旋转角度$\theta$，我们可以构造一个四元数$q$来表示旋转：

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

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

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

$q=(\cos (\frac{\theta }{2}),u_x\sin (\frac{\theta }{2}),u_y\sin (\frac{\theta }{2}),u_z\sin (\frac{\theta }{2}))$

四元数旋转向量

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

1. 构造旋转四元数：

   $q=(\cos (\frac{\theta }{2}),u_x\sin (\frac{\theta }{2}),u_y\sin (\frac{\theta }{2}),u_z\sin (\frac{\theta }{2}))$

2. 构造向量四元数（纯四元数）：

   $p=(0,p_x,p_y,p_z)$

3. 计算旋转后的向量：
   使用四元数乘法进行旋转：

   $p^{\prime }=qp{q}^{-1}$

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

习题p6:

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

• 从解析解和数值解分析

  解析解

  解析解（Analytical Solution）指的是通过数学公式或代数操作直接求出精确解。对于线性方程组$Ax=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$是正定的，则可以分解为$L{L}^T$，其中$L$是下三角矩阵。
     • 优点：比计算矩阵逆更稳定、更高效，适用于大规模或病态矩阵。
     • 缺点：仍然是$O(n^3)$复杂度，对于非常大的矩阵可能不适用。

  比较与选择

  • 解析解：

    • 优点：如果可行，能提供精确解。
    • 缺点：仅适用于小规模问题，且计算复杂度高，数值不稳定。

  • 数值解：

    • 优点：适用于大规模问题和稀疏矩阵，计算速度快，方法稳定。
    • 缺点：提供的是近似解，可能存在数值误差，收敛性依赖于问题特性。

  在实际应用中，选择解析解还是数值解通常取决于问题的规模、矩阵的特性和计算资源。对于小规模的线性方程组或特定类型的矩阵（如对称正定矩阵），解析解是一个可行的选择。而对于大规模的线性方程组或稀疏矩阵，数值解通常是更好的选择。

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

定义

• 解析解

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

• 数值解

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

区别

1. 形式：

   • 解析解：以数学公式或解析表达式的形式给出。
   • 数值解：以数值结果或近似值的形式给出。

2. 精度：

   • 解析解：精确解，假设没有计算误差。
   • 数值解：近似解，精度取决于算法、步长、计算机精度等因素。

3. 求解过程：

   • 解析解：通常通过符号运算、代数技巧或解析函数求得。
   • 数值解：通常通过迭代算法、数值方法或计算机程序求得。

4. 适用范围：

   • 解析解：适用于方程或问题有明确的解析表达式，并且能通过代数操作求解的情况。
   • 数值解：适用于复杂方程或问题解析求解困难或无解析解的情况。

优缺点

解析解：

• 优点：
  • 提供精确解。
  • 便于理解问题的内在结构和性质。
• 缺点：
  • 复杂或高阶问题可能难以求得解析解。
  • 某些方程或问题根本没有解析解。

数值解：

• 优点：
  • 能处理复杂问题和无解析解的问题。
  • 利用计算机可以高效地求解大规模问题。
• 缺点：
  • 解是近似的，精度依赖于算法和计算资源。
  • 可能会有数值误差和稳定性问题。
• 提供精确解。
  • 便于理解问题的内在结构和性质。
• 缺点：
  • 复杂或高阶问题可能难以求得解析解。
  • 某些方程或问题根本没有解析解。

数值解：

• 优点：
  • 能处理复杂问题和无解析解的问题。
  • 利用计算机可以高效地求解大规模问题。
• 缺点：
  • 解是近似的，精度依赖于算法和计算资源。
  • 可能会有数值误差和稳定性问题。