三维刚体运动

点和向量，坐标系

首先说说向量是什么，它是线性空间的一个元素，可以想象从原点指向某处的一个箭头，不要把向量和坐标混淆。谈论向量 $\vec a$在一个线性基（ $\vec e_1,\vec e_2,\vec e_3$）的坐标
$\vec a = \left[ \begin{matrix} \vec e_1 & \vec e_2 &\vec e_3 \\ \end{matrix} \right] \left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \\ \tag{1}\end{matrix} \right]$
向量的点乘（内积）可以描述成向量间的投影关系，对于 $\vec a,\vec b \in \vec R^3$,
$\vec a \cdot \vec b = \sum_{i=1}^3{a_i b_i} = |\vec a||\vec b|cos< \vec a , \vec b> \tag{2}$
向量的叉乘（外积）
$\vec a \times \vec b = \left[ \begin{matrix} \vec i & \vec j &\vec k \\ a_1 & a_2 &a_3 \\ b_1 & b_2 &b_3 \\ \end{matrix} \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_1 &0 \\ \end{matrix} \right] \vec b =\vec a^v \cdot \vec b\tag{3}$
| $\vec a× \vec b$|在数值上等于由 $\vec a$和 $\vec b$构成的平行四边形的面积 $|\vec a||\vec b|sin< \vec a , \vec b>$,方向垂直于这两个向量。
(3)式中最后的等号是近似的，我们把 $\vec a^v$当成 $\vec a$生成的一个反对称矩阵，这样就把向量的叉乘变成矩阵与向量的乘积，把运算变成了线性运算

坐标系的欧式变化

在欧式变化中，除了旋转还有平移。考虑到世界坐标系中的向量 $\vec a$，经过一次选择（R）和一次平移 $\vec t$,得到了 $\vec a'$,有：
$\vec a' = R \vec a + \vec t$
我们定义两个坐标系：世界坐标系和相机坐标系，在该定义下，设某个点再世界坐标系中的坐标为 $\vec p_w$，在相机坐标系下为 $\vec p_c$,那么：
$\vec p_c= T_{cw} \vec p_w$
这里 $T_{cw}$表示世界坐标系到相机坐标系的变换。或者反过来的 $T_{wc}$:
$\vec p_w= T_{wc} \vec p_c = T_{wc}^{-1} \vec p_c$
如果把 $\vec p_c$取成零向量，也就是相机坐标系的原点，那么 $\vec p_w$就是相机原点在世界坐标系下的坐标：
$\vec p_w= T_{wc} \vec 0 = \vec t_{wc}$
我们发现这就是 $T_{wc}$的平移部分。
note： $T_{wc}$表示相机坐标系到世界坐标系的变换。旋转部分遵循相机到世界的旋转，平移部分则是相机在世界坐标系下的坐标

四元数

假设某个旋转是绕单位向量 $\vec n=[n_x,n_y,n_z]^T$进行角度 $\theta$的旋转，那么这个旋转的四元数形式就是
$\vec q = [cos\frac{\theta}{2},n_xsin\frac{\theta}{2},n_ysin\frac{\theta}{2},n_zsin\frac{\theta}{2}]^T$