【视觉SLAM十四讲】三维空间刚体运动

三维空间刚体运动

本文意在明确两个问题：1.如何描述点在不同坐标系间的变换 $\Rightarrow$ 欧式变换

​ 2.如何对欧式变换进行高效的梯度下降 $\Rightarrow$ 李群李代数

欧式变换

旋转的表示和计算

1.旋转矩阵

通常为 3 阶正交矩阵，用于描述从一个坐标系到另一个坐标系的旋转变换（${\mathbf{v}}^{\prime }=\mathbf{R}\cdot \mathbf{v}$）

常见旋转矩阵

• 绕 x 轴：${\mathbf{R}}_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$
• 绕 y 轴：${\mathbf{R}}_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$
• 绕 z 轴：${\mathbf{R}}_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$

2.旋转向量

由表示旋转轴的单位向量和旋转角度组成的 3 维向量，例如 $(\frac{\pi }{6},0,0)$ 表示绕x轴逆时针旋转 $30^{\circ }$

相较于旋转矩阵更为紧凑，可以使用罗德里格斯公式将旋转向量转换为旋转矩阵

罗德里格斯公式

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

推导（没有平板，只能手写了）

当然，对于旋转矩阵转换到旋转向量也有如下公式
$$\theta =\arccos \frac{tr(R)-1}{2}Rn=n$$

3.欧拉角

直观的将旋转展示为绕三个不同轴的三次旋转。常用顺序为 Z-X-Y ，或 yaw-pitch-roll

需要注意的是，欧拉角仅适用于演示和理解而非计算（这是因为欧拉角存在无法解决的万向锁问题，这里不展开讲解，有兴趣的可以移步至笔者另一篇文章：传送门）

4.四元数

旋转矩阵用了 9 个量来描述 3 自由度的旋转，具有冗余性；虽然欧拉角和旋转向量都是紧凑的，但具有奇异性。所以我们需要引入一个类似于复数的代数：四元数。

通常定义四元数为 $\mathbf{q}=(s+q_{x}i+q_{y}j+q_{z}k)或(\mathbf{q}=[s,v])$ ，在 SLAM 中是表示姿态的首选

若有旋转轴的单位向量为 $\mathbf{u}=(u_x,u_y,u_z)$ ，旋转角度为 $\theta$ ，则对应的单位四元数可定义为
$$\mathbf{q}=\cos \le (\frac{\theta }{2})+\sin (\frac{\theta }{2})(u_{x}i+u_{y}j+u_{z}k)$$

1. 虚部满足规律：

   $\{\begin{array}{ll}{i}^2=j^2=k^2=-1& \\ \begin{array}{r}ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}& \text{(非交换性)}\end{array}$

2. 四元数性质：

   $s^2+q_x^2+q_y^2+q_z^2=1$ （单位四元数）

   ${\mathbf{q}}^{\ast }=s-q_{x}i-q_{y}j-q_{z}k$ （共轭四元数）

   ​ $q\cdot {\mathbf{q}}^{\ast }={\mathbf{q}}^{\ast }\cdot \mathbf{q}=||\mathbf{q}|{|}^2$

   ${\mathbf{q}}^{-1}=\frac{{\mathbf{q}}^{\ast }}{||\mathbf{q}|{|}^2}$ （逆四元数）

3. 四元数运算：

   ${\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b]$

   ${\mathbf{q}}_{\mathbf{a}}\cdot {\mathbf{q}}_{\mathbf{b}}=s_a{s}_b+x_a{x}_{b}i+y_a{y}_{b}j+z_a{z}_{b}k$

4. 四元数的应用：

   1.若两个旋转分别为 ${\mathbf{q}}_1$（先转） 和 ${\mathbf{q}}_2$（后转），则复合结果为 ${\mathbf{q}}^{\prime }={\mathbf{q}}_2{\mathbf{q}}_1$

   2.用四元数 $\mathbf{q}$ 旋转三维向量 $\mathbf{v}=(x,y,z)$ 时，可将向量视为 $\mathbf{v}=0+xi+yj+zk$

   ​ 选择后向量对应的四元数可表示为 ${\mathbf{v}}^{\prime }=\mathbf{q}v{\mathbf{q}}^{-1}$

刚体运动的统一表述

刚体运动可解释为旋转和平移的总和 $\to$ ${\mathbf{p}}^{\prime }=\mathbf{R}\mathbf{p}+\mathbf{t}$
由于平移是线性变换，但无法用矩阵乘法表示，所以为了简化运算，需要引入齐次坐标和变换矩阵

齐次坐标

为了统一使用矩阵乘法表示旋转和平移，需要引入引入额外维度以实现矩阵的可加性，比如平移：$[x,y,z]+[t1,t2,t3]=[x+t1,y+t2,z+t3]$无法通过矩阵乘法实现。故引入齐次矩阵

$$\left[\begin{array}{cccc}1& 0& 0& t1\\ 0& 1& 0& t2\\ 0& 0& 1& t3\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{c}x+t1\\ y+t2\\ z+t3\\ 1\end{array}\right]$$

变换矩阵

既然现在我们可以用矩阵乘来表示平移了，何不更近一步？将旋转和平移用一个矩阵来表示
$$\left[\begin{array}{c}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]$$
如果要表示变换的叠加，则如下
$$\tilde{\mathbf{b}}={\mathbf{T}}_1\tilde{\mathbf{a}},\tilde{\mathbf{c}}={\mathbf{T}}_2\tilde{\mathbf{b}}\Rightarrow \tilde{\mathbf{c}}={\mathbf{T}}_2{\mathbf{T}}_1\tilde{\mathbf{a}}$$