三维刚体运动

点和向量，座标系

首先说说向量是什么，它是线性空间的一个元素，可以想象从原点指向某处的一个箭头，不要把向量和座标混淆。谈论向量$\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$