旋转矩阵和变换矩阵

本文基本上是看着《视觉SLAM十四讲》第三章的部分内容写了一遍，加深自己的理解。

---

点和向量，坐标系

以三维空间为例，一个空间点的位置可以用三个坐标来表示，而对于一个刚体而言，除了在空间的位置，还有自身的姿态，合称为“位姿”。

向量是什么?
向量是空间中的一种元素，既有大小，也有方向。有几种表示方式：
代数表示：字母加粗或者字母上加箭头（$\vec a$ 或者 $\textbf a$ ）
几何表示：用一个箭头（线段长度代表向量的大小）
坐标表示：在坐标系中，用坐标可以表示向量，终点坐标-起点坐标

例子：$\textbf a = \begin{bmatrix} \textbf e_1 , \textbf e_2,\textbf e_3\end{bmatrix} \begin{bmatrix} a_1 \\ a_2\\ a_3\end{bmatrix}= a_1 \textbf e_1+a_2\textbf e_2+a_3 \textbf e_3$

坐标系间的欧式变换

坐标系之间变换可以用两个坐标系之间的旋转和平移关系来表示。
设定一个惯性坐标系（世界坐标系），认为是固定不动的。
机器人坐标系（相机坐标系），认为相机固定在机器人上。

现在假设一个向量，在世界坐标系下，有：

$$\textbf a = \begin{bmatrix} \textbf e_1 , \textbf e_2,\textbf e_3\end{bmatrix} \begin{bmatrix} a_1 \\ a_2\\ a_3\end{bmatrix}$$
在相机坐标系中，有：
$$\textbf a= \begin{bmatrix} \textbf e^,_1 , \textbf e^,_2,\textbf e^,_3\end{bmatrix} \begin{bmatrix} a^,_1 \\ a^,_2\\ a^,_3\end{bmatrix}$$

即

$$\begin{bmatrix} \textbf e_1 , \textbf e_2,\textbf e_3\end{bmatrix} \begin{bmatrix} a_1 \\ a_2\\ a_3\end{bmatrix}= \begin{bmatrix} \textbf e^,_1 , \textbf e^,_2,\textbf e^,_3\end{bmatrix} \begin{bmatrix} a^,_1 \\ a^,_2\\ a^,_3\end{bmatrix}$$
等式两边同时左乘 $\begin{bmatrix} e^T_1 \\ e^T_2\\ e^T_3\end{bmatrix}$

得到
$\begin{bmatrix} a_1 \\ a_2\\ a_3\end{bmatrix}= \begin{bmatrix} e^T_1e^,_1 &e^T_1e^,_2&e^T_1e^,_3\\ e^T_2e^,_1&e^T_2e^,_2&e^T_2e^,_3\\ e^T_3e^,_1&e^T_3e^,_2&e^T_3e^,_3 \end{bmatrix} \begin{bmatrix} e^,_1 \\ e^,_2\\ e^,_3\end{bmatrix}= \textbf {R} \textbf a^,$

所以，这个矩阵R就称为旋转矩阵。
$\textbf a=\textbf {R} \textbf a^,$ ，R描述了从相机坐标系旋转到世界坐标系。
如果把平移也考虑上，那么我们可以有这么一个式子：
$\textbf a^,=\textbf {R} \textbf a+\textbf t$
注意，这里的R 描述从世界坐标系旋转一定角度，然后经过一个平移向量t，得到了相机坐标系。

变换矩阵与齐次坐标

但是上述旋转矩阵和平移向量的表达方式有一些问题，比如：

$$\textbf b=\textbf R_1 \textbf a+\textbf t_1 , \textbf c=\textbf R_2 \textbf b+\textbf t_2$$
那么从 a 到 c 的变换就变成了以下式子：
$$\textbf c=\textbf R_2 (\textbf R_1 \textbf a+\textbf t_1)+\textbf t_2$$
这个样子在多次变换之后形式会有点复杂，我们比较嫌弃它，所以，引入齐次坐标和变换矩阵！
$\textbf a^,=\textbf {R} \textbf a+\textbf t$ 这个式子，用齐次坐标和变换矩阵改写如下：

$$\begin{bmatrix} \textbf a^, \\ 1\end{bmatrix}= \begin{bmatrix} \textbf R &\textbf t \\ \textbf 0^T& 1 \end{bmatrix} \begin{bmatrix} \textbf a \\ 1\end{bmatrix}= \textbf T \begin{bmatrix} \textbf a \\ 1 \end{bmatrix}$$

在这个式子中，T 就叫做变换矩阵，然后用$\tilde{\textbf a^,}$来表示$\begin{bmatrix} \textbf a^, \\ 1\end{bmatrix}$，叫做$\textbf a^,$的齐次坐标。
[注]
齐次坐标的概念：
就是在原来的坐标增加一个维度。而且一个向量的齐次坐标是可以有多种表示的。
比如对于（1，1，1）这个点，它的齐次坐标可以是（k,k,k,k），k可以是任意的常数，只要不等于0。
因为一个（非齐次）坐标的齐次坐标有这么一个原则：
齐次坐标的每个分量，乘以一个非零常数后，任然表示同一个点。

所以有了齐次坐标和变换矩阵的概念以后，坐标系的多次变换可以写成这个样子：

$$\tilde{\textbf b} = \textbf T_1 \tilde{\textbf a} , \tilde{\textbf c} = \textbf T_2 \tilde{\textbf b} \Rightarrow \tilde{\textbf c} = \textbf T_2\textbf T_1 \tilde{\textbf a}$$
所以啊，人类懒惰的本质又暴露出来了，默认情况下，我们去掉坐标字幕头上的波浪号了， $\tilde{\textbf a} \rightarrow \textbf a$。
这样也挺好的，带着“帽子”总有点怪怪的，谁知道是什么颜色呢？（哈哈~~）！
不过我们自己心里也要清楚：

• 当出现Ta这种形式的时候，因为写了变换矩阵，所以，这里的a就是齐次坐标；
• 同理，当出现Ra，因为用了旋转矩阵，所以这里的a就是非齐次坐标
• 如果Ta和Ra出现在等号的两边，它们不是严格意义上的等号，我们假设二者之间已经从非齐次转换成了齐次坐标（或者齐次转为非齐次）。