【视觉SLAM:二、三维空间刚体运动】

刚体运动描述的是三维空间中刚体的旋转和平移。SLAM、机器人学、计算机视觉等领域中，经常需要用数学工具表达刚体的旋转与位姿变换。以下从旋转矩阵、旋转向量和欧拉角、四元数、相似、仿射与射影变换，以及可视化演示五个方面详细介绍三维刚体运动的相关内容。

旋转矩阵

旋转矩阵是一种常用的刚体旋转表示方法。它在数学上是一个正交矩阵，维度为3×3，表示坐标系的旋转变换。

欧式变换是刚体运动中最基本的变换，它包括旋转和平移。
数学表示为： p ′ = R p + t \mathbf{p}^{\prime}=\mathbf{Rp}+\mathbf{t} p′=Rp+t

其中：
- R ∈ R 3 × 3 \mathbf{R}\in\mathbb{R}^{3\times3} R∈R3×3：旋转矩阵，表示坐标系的旋转。
- t ∈ R 3 \mathbf{t}\in\mathbb{R}^3 t∈R3：平移向量，表示坐标系的平移。
变换矩阵与齐次坐标
- 为了更方便地描述旋转和平移，可以使用齐次坐标 和变换矩阵： T = [ R t 0 ⊤ 1 ] , p ′ = T ⋅ p homogeneous \mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{t}\\\mathbf{0}^\top&1\end{bmatrix},\quad\mathbf{p}^{\prime}=\mathbf{T}\cdot\mathbf{p}_{\text{homogeneous}} T=[R0⊤t1],p′=T⋅phomogeneous
  其中：
  - T ∈ S E ( 3 ) \mathbf{T}\in SE(3) T∈SE(3)：变换矩阵，包含旋转和平移。
  - 齐次坐标 p homogeneous = [ x , y , z , 1 ] ⊤ . \mathbf{p}_{\text{homogeneous}}=[x,y,z,1]^{\top}. phomogeneous=[x,y,z,1]⊤.
- 变换矩阵的优点：
  - 统一了旋转和平移的表示。
  - 通过矩阵乘法可以连续组合多个变换。

定义：旋转向量𝜔是旋转轴𝑛和旋转角度𝜃的组合，用于表示三维空间中的旋转： ω = θ n , ∥ n ∥ = 1 \omega=\theta\mathbf{n},\quad\|\mathbf{n}\|=1 ω=θn,∥n∥=1
作用：旋转向量是一种紧凑的旋转表示方法（仅用 3 个参数），可以方便地转换为旋转矩阵或四元数。
从旋转向量到旋转矩阵：
- 通过 Rodrigues 公式将旋转向量𝜔转化为旋转矩阵： R = I + sin ⁡ θ n ^ + ( 1 − cos ⁡ θ ) n ^ 2 \mathbf{R}=\mathbf{I}+\sin\theta\mathbf{\hat{n}}+(1-\cos\theta)\mathbf{\hat{n}}^2 R=I+sinθn^+(1−cosθ)n^2
  其中， n ^ \mathbf{\hat{n}} n^是旋转轴的反对称矩阵。

定义：欧拉角是通过三个角度表示三维旋转，通常是绕固定坐标轴（如 XYZ）的旋转：
- 绕𝑍轴旋转角度 𝛼（Yaw）。
- 绕𝑌轴旋转角度𝛽（Pitch）。
- 绕𝑋轴旋转角度𝛾（Roll）。
优点：直观易理解，适合描述旋转的物理意义。
缺点：存在奇异性（万向锁问题 ），即在某些角度下无法唯一表示旋转。

四元数是一种代数结构，用于表示和计算三维旋转。相比旋转矩阵和欧拉角，四元数具有更好的数值稳定性和计算效率。

四元数由一个实数部分和一个三维向量部分组成： q = [ w , x , y , z ] , w ∈ R , v = [ x , y , z ] ∈ R 3 \mathbf{q}=[w,x,y,z],\quad w\in\mathbb{R},\quad\mathbf{v}=[x,y,z]\in\mathbb{R}^3 q=[w,x,y,z],w∈R,v=[x,y,z]∈R3
或记为： q = w + v \mathbf{q}=w+\mathbf{v} q=w+v

加法：按分量相加。
乘法：定义特殊的乘法规则，非交换： q 1 ⋅ q 2 = ( w 1 w 2 − v 1 ⋅ v 2 ) + ( w 1 v 2 + w 2 v 1 + v 1 × v 2 ) \mathbf{q}_1\cdot\mathbf{q}_2=(w_1w_2-\mathbf{v}_1\cdot\mathbf{v}_2)+(w_1\mathbf{v}_2+w_2\mathbf{v}_1+\mathbf{v}_1\times\mathbf{v}_2) q1⋅q2=(w1w2−v1⋅v2)+(w1v2+w2v1+v1×v2)

旋转角度𝜃和旋转轴𝑛对应的四元数表示为： q = [ cos ⁡ θ 2 , sin ⁡ θ 2 n ] \mathbf{q}=\left[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\mathbf{n}\right] q=[cos2θ,sin2θn]
旋转公式： p ′ = q ⋅ p ⋅ q − 1 \mathbf{p}^{\prime}=\mathbf{q}\cdot\mathbf{p}\cdot\mathbf{q}^{-1} p′=q⋅p⋅q−1
其中，𝑝是点的四元数表示。

四元数→旋转矩阵： R = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 z w 2 x z + 2 y w 2 x y + 2 z w 1 − 2 x 2 − 2 z 2 2 y z − 2 x w 2 x z − 2 y w 2 y z + 2 x w 1 − 2 x 2 − 2 y 2 ] \mathbf{R}=\begin{bmatrix}1-2y^2-2z^2&2xy-2zw&2xz+2yw\\2xy+2zw&1-2x^2-2z^2&2yz-2xw\\2xz-2yw&2yz+2xw&1-2x^2-2y^2\end{bmatrix} R= 1−2y2−2z22xy+2zw2xz−2yw2xy−2zw1−2x2−2z22yz+2xw2xz+2yw2yz−2xw1−2x2−2y2
四元数→欧拉角：通过数学推导计算出对应角度。

相似变换是刚体运动加上一个全局缩放因子𝑠： p ′ = s R p + t \mathbf{p}^{\prime}=s\mathbf{Rp}+\mathbf{t} p′=sRp+t

仿射变换不仅包含旋转和平移，还可以引入剪切、缩放等非刚体变换：
p ′ = A p + t \mathbf{p}^{\prime}=\mathbf{Ap}+\mathbf{t} p′=Ap+t
其中， A ∈ R 3 × 3 \mathbf{A}\in\mathbb{R}^{3\times3} A∈R3×3。

射影变换允许非线性变换，例如透视投影： p ′ = H p homogeneous \mathbf{p}^{\prime}=\mathbf{H}\mathbf{p}_{\text{homogeneous}} p′=Hphomogeneous
其中， H ∈ R 4 × 4 \mathbf{H}\in\mathbb{R}^{4\times4} H∈R4×4 是投影矩阵。

三维空间中的刚体运动描述了刚体在空间中的旋转和平移。我们通过旋转矩阵、旋转向量和欧拉角、四元数等方式表示旋转，通过变换矩阵统一旋转和平移的表示。进一步，仿射变换和射影变换拓展了变换的范围，可用于更复杂的场景分析与建模。