视觉 SLAM 十四讲 —— 第三讲三维空间刚体运动

GondorFu

已于 2022-07-15 10:51:39 修改

阅读量714

点赞数

分类专栏：三维重建文章标签：计算机视觉

于 2020-09-14 09:41:17 首次发布

本文链接：https://blog.csdn.net/a40850273/article/details/108572924

版权

三维重建专栏收录该内容

10 篇文章 1 订阅

订阅专栏

觉 SLAM 十四讲 —— 第三讲三维空间刚体运动

反对称矩阵

矩阵转置 = 矩阵的所有元素取反，用右上角 ^ 表示

通过反对称矩阵能够将两向量的外积运算转换成第一个向量的反对称矩阵表示与第二个向量矩阵乘法的形式。

坐标系间的欧氏变换与旋转矩阵

欧氏变换将空间中刚体的运动描述为一次旋转和一次平移的叠加，

${a}'=\textbf{R}^{-1}a + \textbf{t}$

其中旋转由旋转矩阵 R 来描述，而平移则由一个向量 t 来描述。其中旋转矩阵 R 的集合可定义如下

$SO(n) = \begin{Bmatrix} {\textbf{R}\in \mathbb{R}^{n\times n} |\textbf{R}\textbf{R}^T=\textbf{I}, det(\textbf{R}))=1} \end{Bmatrix}$

SO(n) 是特殊正交群（Special Orthogonal Group）的意思，其满足正交且行列式为一，即为酉矩阵。因此旋转矩阵 R 的转置就是旋转变换的逆变换。

变换矩阵和齐次坐标系

使用欧氏变换来表述空间中刚体的运动存在一个小问题，就是由于欧氏变换不是一个线性变换，如果依次进行了多次变换那么最终的形式将变得很复杂。这也是为何要引入齐次坐标系的原因。

齐次坐标通过在三维向量的末尾添加1，将其变成四维向量从而将旋转和平移放到一个矩阵中实现，从而将非线性的欧氏变换变为线性的。

其中矩阵 T 称为变换矩阵。同时由于齐次坐标用 4 个实数描述了一个三维向量，这造成多个不同的齐次坐标表示了现实中的同一个向量。即只要齐次坐标的最后一维不为零，那么都可以通过将齐次坐标同除以最后一维从而得到一个点唯一的坐标表示。最后上述变换矩阵 T 的集合可定义如下

SE(3) 被称为特殊欧氏群（Special Euclidean Group）。其逆矩阵可以通过一下方式直接求取

$\textbf{T}^{-1}=\begin{bmatrix} \textbf{R}^T & -\textbf{R}^T\textbf{t} \\ \textbf{0}^T & 1 \end{bmatrix}$

旋转向量和欧拉角

但上面介绍的旋转矩阵和变换矩阵至少存在以下两个问题：

1、旋转矩阵使用了9个量来描述自由度为3的旋转，而变换矩阵使用了16个量来描述自由度为6的旋转加平移，因此描述上存在一定的冗余；

2、不管是旋转矩阵还是变换矩阵都必须满足某些约束，这些约束不利于之后运动的求解。

因此旋转向量使用一个向量来描述旋转，其中向量的方向与旋转轴一致，大小描述了旋转的角度。

旋转向量到旋转矩阵的变换：由罗德里格斯公式来描述

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

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

同时，注意到旋转轴上的向量绕旋转轴旋转不会发生改变，即 $\textbf{R}\textbf{n} = \textbf{n}$ ，因此旋转轴 n 是旋转矩阵 R 特征值为 1 的特征向量。

不过无论是旋转矩阵，旋转向量，它们虽然能够描述旋转，但是对我们人类并不直观。欧拉角则提供了一种非常直观的方式来描述旋转——三个分离的转角。但是由于依次旋转的顺序和旋转轴是否固定，欧拉角有多种形式。这里介绍比较常用的一种 ZYX 欧拉角。

ZYX 欧拉角，首先选定朝向我们的方向为 X，向右为 Y，上方为 Z；其将任意旋转分解为以下 3 个轴上的转角：

1、绕物体的 Z 轴旋转，得到偏航（方向）角yaw

2、绕旋转之后的 Y 轴旋转，得到俯仰角 pitch

3、绕旋转之后的 X 轴旋转，得到滚转角 roll

此时就可以使用 [r,p,y]^T 这样一个三维向量来描述任意旋转。

当欧拉角有一个重大的缺点——存在万向锁问题（Gimbal Lock）：在俯仰角为 $\pm90 ^ \circ$ 时，第一次旋转与第三次旋转将使用同一个轴，是的系统失去了一个自由度（奇异性问题）。

而且从理论可以证明，只要使用 3 个实数来表达三维旋转时，都会不可避免地碰到奇异性的问题（旋转向量在转角超过2pi时会产生周期性）。因此，欧拉角不适于插值、迭代、滤波和优化中，而往往只用于人机交互中，转换成欧拉角能够快速分辨结果是否正确。

四元数

欧拉角中已经提及：不存在不带奇异性的旋转三维向量描述方式。比如用二维坐标描述球面，纬度为 $\pm90 ^ \circ$ 时经度没有意义。三维旋转也是一个三维流形，因此不能用 3 维向量进行描述。四元数在用一个四维向量来描述旋转，可以证明它是紧凑的，也没有奇异性。

$\textbf{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, ik=-j \end{matrix}\right.$

四元数运算

通过一下两个矩阵记发四元数乘法能够表示为如下矩阵乘法

用四元数表示旋转

空间中的三维点 $\textbf{p}=[x,y,z]^T$ ，可以将坐标值作为四元数虚部，而将实部置零得到三维点对应的虚四元数。

之后旋转之后的三维点可以按照下式求得

求得的 ${\textbf{p}}'$ 实部为零，其 3 个虚部分量就是旋转后 3D 点的坐标。

四元数与旋转向量的转换

旋转向量由 $\textbf{n} = \begin{bmatrix} n_x & n_y & n_z \end{bmatrix} ^ T$ 和旋转角度 $\theta$ 表示，则对应四元数为

$\textbf{q} = \begin{bmatrix} cos \frac{\theta}{2} & n_xsin \frac{\theta}{2} & n_ysin \frac{\theta}{2} & n_zsin \frac{\theta}{2} \end{bmatrix}$ ；

反之四元数 $\textbf{q}=\begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}$ ，用旋转向量表示为

$\left\{\begin{matrix} \theta = 2arccosq_0\\ \begin{bmatrix} n_x & n_y & n_z \end{bmatrix} ^T =\begin{bmatrix} q_1 & q_2 & q_3 \end{bmatrix} ^ T / sin\frac{\theta}{2} \end{matrix}\right.$

从以上式子可以发现， $\theta$ 加上 $2 \pi$ ，我们得到了一个相同的旋转角，但此时对应的四元数变成了 $-\textbf{q}$ 。因此任意一个旋转都可以由互为相反数的四元数表示。

四元数与旋转矩阵的转换

相似、仿射、射影变换

相似变换 $\textbf{T}_S = \begin{bmatrix} s\textbf{R} & \textbf{t}\\ \textbf{0}^T & 1 \end{bmatrix}$ 允许对物体进行均匀缩放（一个立方体经过相似变换仍然时立方体），其比欧拉变换多了一个自由度

仿射变换 $\textbf{T}_A = \begin{bmatrix} \textbf{A} & \textbf{t}\\ \textbf{0}^T & 1 \end{bmatrix}$ ，其只要求 $\textbf{A}$ 是一个可逆矩阵，此时一个立方体经过仿射变换之后将可能不再是方的，但各个面仍然是平行四边形

射影变换 $\textbf{T}_P = \begin{bmatrix} \textbf{A} & \textbf{t}\\ \textbf{a}^T & v \end{bmatrix}$ ，其描述了从真实世界到相机照片的变换，方形经过射影变换，由于近大远小的关系，甚至可能不再是平行四边形了。

变换名称	矩阵形式	自由度	不变性质
欧氏变换	$\textbf{T}=\begin{bmatrix} \textbf{R} & \textbf{t} \\ \textbf{0}^T & 1 \end{bmatrix}$	6	长度、夹角、体积
相似变换	$\textbf{T}_S = \begin{bmatrix} s\textbf{R} & \textbf{t}\\ \textbf{0}^T & 1 \end{bmatrix}$	7	体积比
仿射变换	$\textbf{T}_A = \begin{bmatrix} \textbf{A} & \textbf{t}\\ \textbf{0}^T & 1 \end{bmatrix}$	12	平行性、体积比
射影变换	$\textbf{T}_P = \begin{bmatrix} \textbf{A} & \textbf{t}\\ \textbf{a}^T & v \end{bmatrix}$	15	接触平面的相交与相切