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

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/youngpan1101/article/details/71086500

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

---

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

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

---

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

1. 旋转矩阵

1. 点和向量，坐标系

   • 点：在几何学上点是 没有大小而只有位置，即点存在于三维空间中的某一个位置。
   • 向量： 可以形象化地表示为 带箭头的线段。箭头所指：代表向量的方向；线段长度：代表向量的大小。
     （请勿将 向量 与它的 坐标 两个概念混淆，只有确定向量所在的坐标系，才能讨论它在该坐标系下的坐标）
   • 定义坐标系后，也就是一个线性空间的 基 $({\boldsymbol e}_{1}, {\boldsymbol e}_{2}, {\boldsymbol e}_{3})$，向量 ${\boldsymbol a}$ 在该坐标系下的坐标为：
     $${\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}$，内积可以表示为：
       $${\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}$，外积可以表示为：
         $${\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}$ 的方向
         

2. 坐标系间的欧氏变换

   对于同一个向量 ${\boldsymbol p}$，它在世界坐标系下的坐标 ${\boldsymbol p}_{w}$ 和在相机坐标系下的坐标 ${\boldsymbol p}_{c}$ 是不同的，它们的变换关系由两坐标系间的变换矩阵 ${\boldsymbol T}$ 来描述。（如下图所示，下图来自视觉SLAM十四讲 图3-2）
   

   • 欧氏变换 = 旋转 + 平移
   • 旋转
     设某个单位正交基 $({\boldsymbol e}_{1}, {\boldsymbol e}_{2}, {\boldsymbol e}_{3})$ 经过一次旋转变成 $({\boldsymbol e}_{1}', {\boldsymbol e}_{2}', {\boldsymbol e}_{3}')$。对于同一个向量 ${\boldsymbol a}$（该向量不会因坐标系的旋转而发生运动），它在两个坐标系下的坐标分别为 $\left[ a_{1}, a_{2}, a_{3} \right]^{T}$ 和 $\left[ a_{1}', a_{2}', a_{3}' \right]^{T}$，两坐标点满足：
     $$\left[ {\boldsymbol e}_{1}, {\boldsymbol e}_{2}, {\boldsymbol e}_{3} \right] \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \\ \end{bmatrix} = \left[ {\boldsymbol e}_{1}', {\boldsymbol e}_{2}', {\boldsymbol e}_{3}' \right] \begin{bmatrix} a_{1}' \\ a_{2}' \\ a_{3}' \\ \end{bmatrix} \tag{3.4}$$
     式 (3.4) 左右两边同时左乘 $\begin{bmatrix} {\boldsymbol e}_{1}^{T} \\ {\boldsymbol e}_{2}^{T} \\ {\boldsymbol e}_{3}^{T} \\ \end{bmatrix}$，得
     $$\begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \\ \end{bmatrix} = \begin{bmatrix} {\boldsymbol e}_{1}^{T} {\boldsymbol e}_{1}' & {\boldsymbol e}_{1}^{T} {\boldsymbol e}_{2}' & {\boldsymbol e}_{1}^{T} {\boldsymbol e}_{3}' \\ {\boldsymbol e}_{2}^{T} {\boldsymbol e}_{1}' & {\boldsymbol e}_{2}^{T} {\boldsymbol e}_{2}' & {\boldsymbol e}_{2}^{T} {\boldsymbol e}_{3}' \\ {\boldsymbol e}_{3}^{T} {\boldsymbol e}_{1}' & {\boldsymbol e}_{3}^{T} {\boldsymbol e}_{2}' & {\boldsymbol e}_{3}^{T} {\boldsymbol e}_{3}' \\ \end{bmatrix} \begin{bmatrix} a_{1}' \\ a_{2}' \\ a_{3}' \\ \end{bmatrix} = {\boldsymbol R} a' \tag{3.5}$$
     式 (3.5) 中的旋转矩阵 ${\boldsymbol R}$ 的特殊性质：
     
     • 旋转矩阵是行列式为 1 的正交矩阵
     • 旋转矩阵的逆为自身转置，逆矩阵 ${\boldsymbol R}^{T}$ 表示一个相反的旋转
     • 旋转矩阵的集合定义为：
       $$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}'$ :
     $${\boldsymbol a}' = {\boldsymbol R} {\boldsymbol a} + {\boldsymbol t} \tag{3.7}$$
     所以，两个坐标系的刚体运动可以由 ${\boldsymbol R}$ 和 ${\boldsymbol t}$ 完全描述。

3. 变换矩阵与齐次坐标

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

   $$\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）:
   $$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)$ 一样，该矩阵的逆表示一个反向的变换：
   $${\boldsymbol T}^{-1} = \begin{bmatrix} {\boldsymbol R} & -{\boldsymbol R}^{T} {\boldsymbol t} \\ 0^{T} & 1 \\ \end{bmatrix} \tag{3.10}$$