半闲居士视觉SLAM十四讲笔记（3）三维空间刚体运动 - part 1 旋转矩阵

最新推荐文章于 2024-03-07 17:23:22 发布

youngpan1101

最新推荐文章于 2024-03-07 17:23:22 发布

阅读量5.2k

点赞数 3

分类专栏：视觉SLAM十四讲文章标签：旋转矩阵视觉SLAM 刚体运动描述

本文链接：https://blog.csdn.net/youngpan1101/article/details/71086500

版权

视觉SLAM十四讲专栏收录该内容

13 篇文章 16 订阅

订阅专栏

本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接： http://blog.csdn.net/youngpan1101/article/details/71086500
作者：宋洋鹏（youngpan1101）
邮箱： yangpeng_song@163.com

该讲详细资料下载链接【Baidu Yun】【Video】【Code】

若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】

三维空间的刚体运动描述方式

1. 旋转矩阵

点和向量，坐标系
- 点：在几何学上点是 没有大小而只有位置，即点存在于三维空间中的某一个位置。
- 向量：可以形象化地表示为 带箭头的线段。箭头所指：代表向量的方向；线段长度：代表向量的大小。
  （请勿将向量与它的坐标两个概念混淆，只有确定向量所在的坐标系，才能讨论它在该坐标系下的坐标）
- 定义坐标系后，也就是一个线性空间的基 $({\boldsymbol e}_{1}, {\boldsymbol e}_{2}, {\boldsymbol e}_{3})$ ，向量 ${\boldsymbol a}$ 在该坐标系下的坐标为：
  $a = [e 1, e 2, e 3] ⎡ ⎣ ⎢ a 1 a 2 a 3 ⎤ ⎦ ⎥ = a 1 e 1 + a 2 e 2 + a 3 e 3 (3.1)$ ${\boldsymbol a} = \left[ {\boldsymbol e}_{1}, {\boldsymbol e}_{2}, {\boldsymbol e}_{3} \right] \left[ \begin{aligned} a_{1} \\ a_{2} \\ a_{3} \\ \end{aligned} \right] = a_{1}{\boldsymbol e}_{1} + a_{2}{\boldsymbol e}_{2} + a_{3}{\boldsymbol e}_{3} \tag{3.1}$
- 左手坐标系和右手坐标系（更为常见）
- 向量的运算
  - 加减法
  - 内积（inner product）: 可以描述向量间的投影关系
    对于 ${\boldsymbol a}, {\boldsymbol b} \in {\mathbb R}^{3}$ ，内积可以表示为：
    $a \cdot b = a T b = \sum i = 1 3 a i b i = | a | | b | c o s ⟨ a, b ⟩ (3.2)$ ${\boldsymbol a} \cdot {\boldsymbol b} = {\boldsymbol a}^{T} {\boldsymbol b} = \sum_{i=1}^3 a_{i} b_{i} = |{\boldsymbol a}| |{\boldsymbol b}| cos\left \langle {\boldsymbol a} , {\boldsymbol b} \right\rangle \tag{3.2}$
  - 外积，亦称叉乘（cross product）
    - 对于 ${\boldsymbol a}, {\boldsymbol b} \in {\mathbb R}^{3}$ ，外积可以表示为：
      $a \times b = ∣ ∣ ∣ ∣ i a 1 b 1 j a 2 b 2 k a 3 b 3 ∣ ∣ ∣ ∣ = ⎡ ⎣ ⎢ a 2 b 3 - a 3 b 2 a 3 b 1 - a 1 b 3 a 1 b 2 - a 2 b 1 ⎤ ⎦ ⎥ = ⎡ ⎣ ⎢ 0 a 3 - a 2 - a 3 0 a 1 a 2 - a 1 0 ⎤ ⎦ ⎥ b = a \land b (3.3)$ ${\boldsymbol a} \times {\boldsymbol b} = \begin{vmatrix} {\boldsymbol i} & {\boldsymbol j} & {\boldsymbol k} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \\ \end{vmatrix} = \begin{bmatrix} a_{2} b_{3} - a_{3} b_{2} \\ a_{3} b_{1} - a_{1} b_{3} \\ a_{1} b_{2} - a_{2} b_{1} \\ \end{bmatrix} \\ = \begin{bmatrix} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a{2} & a_{1} & 0 \\ \end{bmatrix} {\boldsymbol b} = {\boldsymbol a}^{\land} {\boldsymbol b} \tag{3.3}$
      式中 $\land$ 为反对称符号（可以读作“上尖”）， ${\boldsymbol a}^{\land}$ 表示向量 ${\boldsymbol a}$ 的 反对称矩阵。
    - 外积可以表示向量的旋转：向量 ${\boldsymbol a}$ 到 ${\boldsymbol b}$ 的旋转向量 ${\boldsymbol w}$ 的方向就是 ${\boldsymbol a} \times {\boldsymbol b}$ 的方向
坐标系间的欧氏变换
对于同一个向量 ${\boldsymbol p}$ ，它在世界坐标系下的坐标 ${\boldsymbol p}_{w}$ 和在相机坐标系下的坐标 ${\boldsymbol p}_{c}$ 是不同的，它们的变换关系由两坐标系间的变换矩阵 ${\boldsymbol T}$ 来描述。（如下图所示，下图来自视觉SLAM十四讲图3-2）
- 欧氏变换 = 旋转 + 平移
- 旋转
  设某个单位正交基 (e1,e2,e3) 经过一次旋转变成 (e′1,e′2,e′3) 。对于同一个向量 a （该向量不会因坐标系的旋转而发生运动），它在两个坐标系下的坐标分别为 [a1,a2,a3]T 和 [a′1,a′2,a′3]T ，两坐标点满足：
  
  [e1,e2,e3]⎡⎣⎢a1a2a3⎤⎦⎥=[e′1,e′2,e′3]⎡⎣⎢a′1a′2a′3⎤⎦⎥(3.4)
  
  式 (3.4) 左右两边同时左乘 ⎡⎣⎢⎢eT1eT2eT3⎤⎦⎥⎥ ，得
  
  ⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3⎤⎦⎥⎥⎡⎣⎢a′1a′2a′3⎤⎦⎥=Ra′(3.5)
  
  式 (3.5) 中的旋转矩阵 R 的特殊性质：
  - 旋转矩阵是行列式为 1 的正交矩阵
  - 旋转矩阵的逆为自身转置，逆矩阵 ${\boldsymbol R}^{T}$ 表示一个相反的旋转
  - 旋转矩阵的集合定义为：
    $S O (n) = {R \in R n \times n | R R T = I, d e t (R) = 1} (3.6)$ $SO(n) = \left \{ {\boldsymbol R} \in {\mathbb R}^{n \times n} | {\boldsymbol R} {\boldsymbol R}^{T} = {\boldsymbol I}, det({\boldsymbol R}) = 1 \right \} \tag{3.6}$
    $SO(n)$ 是特殊正交群（Special Orthogonal Group）， $SO(3)$ 就是由三维空间的旋转矩阵组成。
- 平移
  向量 ${\boldsymbol a}$ 经过一次旋转 ${\boldsymbol R}$ 和一次平移 ${\boldsymbol t}$ 后，得到 ${\boldsymbol a}'$ :
  $a' = R a + t (3.7)$ ${\boldsymbol a}' = {\boldsymbol R} {\boldsymbol a} + {\boldsymbol t} \tag{3.7}$
  所以，两个坐标系的刚体运动可以由 ${\boldsymbol R}$ 和 ${\boldsymbol t}$ 完全描述。
变换矩阵与齐次坐标

欧氏变换多次之后的变换矩阵表示过于复杂，引入齐次坐标和变换矩阵重写式 (3.7):

$[a' 1] = [R 0 T t 1] [a 1] = T [a 1] (3.8)$ $\begin{bmatrix} {\boldsymbol a'} \\ 1 \\ \end{bmatrix} = \begin{bmatrix} {\boldsymbol R} & {\boldsymbol t} \\ 0^{T} & 1 \\ \end{bmatrix} \begin{bmatrix} {\boldsymbol a} \\ 1 \\ \end{bmatrix} = {\boldsymbol T} \begin{bmatrix} {\boldsymbol a} \\ 1 \\ \end{bmatrix} \tag{3.8}$
式 (3.8) 中的 ${\boldsymbol T}$ 称为变换矩阵，这种矩阵又称为特殊欧式群（Special Euclidean Group）:
$S E (3) = {T = [R 0 T t 1] \in R 4 \times 4 | R \in S O (3), t \in R 3} (3.9)$ $SE(3) = \left \{ {\boldsymbol T} = \begin{bmatrix} {\boldsymbol R} & {\boldsymbol t} \\ 0^{T} & 1 \\ \end{bmatrix} \in {\mathbb R}^{4 \times 4} | {\boldsymbol R} \in SO(3), {\boldsymbol t} \in {\mathbb R}^{3} \right \} \tag{3.9}$
与 $SO(3)$ 一样，该矩阵的逆表示一个反向的变换：
$T - 1 = [R 0 T - R T t 1] (3.10)$ ${\boldsymbol T}^{-1} = \begin{bmatrix} {\boldsymbol R} & -{\boldsymbol R}^{T} {\boldsymbol t} \\ 0^{T} & 1 \\ \end{bmatrix} \tag{3.10}$