三维物体刚体运动

3.1 旋转矩阵

3.1.1 点、向量和坐标系

在SLAM中，我们考虑的刚体，比如相机，不仅仅有位置，还有自身的姿态，位置指的是处于空间中的哪个地方，姿态则是指相机的朝向。我们需要用数学的语言来描述它。

我们从最基本的内容：点和向量开始。点是空间中最基本的元素，两个点相连就构成了向量。当我们指定三维空间中的某个坐标系时，我们就可以讨论点和向量在该坐标系下的坐标。

用线性代数的知识来讲，假设我们在一个线性空间内，我们找到了该空间下的一组基$ (e_1,e_2,e_3) $ ，那么任意的向量$a$ 在这组基下就有一个坐标：

$$ a = [e_1,e_2,e_3]\left[ \begin{matrix} a_1\ a_2 \ a_3\end{matrix} \right] = a_1e_1 + a_2e_2 + a_3e_3 $$

这里的$[a_1,a_2,a_3]^T$ 成为$a$ 在此基下的坐标。坐标的具体取值，不仅和向量本身有关，还和坐标系的选取有关。

根据线性代数的知识，我们可以讨论向量与向量，向量与数之间的运算。比如向量之间的内积可以表示为：

$$ a\cdot b = a^Tb = \sum_{i=1}^{3}a_ib_i = |a||b|cos<a,b> $$

内积可以描述为向量之间的投影关系：

$$a\times b = \left| \begin{array}{cccc} e_1 & e_2 & e_3 \ a_1 & a_2 & a_3 \ b_1 & b_2 & b_3 \end{array} \right| = \left[ \begin{matrix} a_2b_3-a_3b_2\ a_3b_1-a_1b_3 \ a_1b_2-a_2b_1\end{matrix} \right] = \left[ \begin{matrix} 0 & -a_3 & a_2 \ a_3 & 0 & -a_1 \ -a_2 & a_2 & 0\end{matrix} \right]b = \widehat{a}b$$

3.12 坐标系间的欧氏变换

我们常常在实际场景中定义各种各样的坐标系，在某一相机视野中，某个向量p在相机坐标系下的坐标为$p$ ，在世界坐标系下的坐标为$p_w$ ,我们可以通过一种变换来从相机坐标得到世界坐标，后面会讲到，这种变换可以写成一个矩阵$T$。

两个坐标系之间的运动由一个旋转和平移组合曾，这种运动叫做刚体运动，在数学中我们用欧氏变换来表达。

比如，向量$a$在坐标系1和坐标系2下的坐标分别为$a_1$和$a_2$,那么他们之间的关系可以表示为：

$$a_1 = R_{12}a_2+t_{12}$$

其中，R表示旋转矩阵，t表示平移向量。

旋转矩阵有一些特殊的性质，事实上他是一个行列式为1的正交矩阵，同样，行列式为1的正交矩阵可以当作旋转矩阵使用。n维旋转矩阵可以定义为以下集合：

$$SO(n) = {R\in R^{nn}|RR^T=I,det(R)=1}$$

3.13 变换矩阵与齐次坐标

上一部分完整地表述了欧氏空间的旋转和变换，但是这一变换不是完美的线性关系，即如果存在多次变换，那么变换的形式将会变得复杂。因此我们引入齐次坐标和变换矩阵来重写：

$$\left[ \begin{matrix} a_1\ 1 \end{matrix} \right] = \left[ \begin{matrix} R &t \ 0^T & 1 \end{matrix} \right]\left[ \begin{matrix} a_2\ 1 \end{matrix} \right] ==T\left[ \begin{matrix} a_2\ 1 \end{matrix} \right]$$

矩阵$T$就被称为是变换矩阵，那么依靠变换矩阵和齐次坐标，两个变换的叠加就会有很好的形式：

$$b=T_1a,c=T_2b \rightarrow c=T_2T_1a$$

变换矩阵$T$也有比较特别的结构，左上角为旋转矩阵，右上角为平移向量，左下角为0向量，右下角为1.这种矩阵又被成为特殊欧氏群：

$$SE(3) = { Y=\left[ \begin{matrix} R &t \ 0^T & 1 \end{matrix} \right] \in R^{44}|R\in SO(3),t\in R^3 }$$

该矩阵的逆表示一个反向的变换：

$$T^{-1} = \left[ \begin{matrix} R^T &-R^Tt \ 0^T & 1 \end{matrix} \right]$$