SLAM #3 三维空间刚体运动
为什么Unity和Direct3D是左手系？
旋转矩阵
外积 a $\times$ b = a ^ b
定义反对称矩阵：
$$
a^\land = \left[
\begin{matrix}
0 & -a_3 & a_2 \
a_3 & 0 & -a_1 \
-a_2 & a_1 & 0
\end{matrix}
\right] \tag{3}
$$
旋转矩阵(Rotation Matrix)刻画旋转前后同一个向量的坐标变换关系，各分量为两个坐标系基的内积，又称方向余弦矩阵(Direction Cosine Matrix)
旋转矩阵为行列式为1的正交矩阵行列式-1称瑕旋转
定义n维旋转矩阵的集合：
$$SO(n)=\lbrace {\pmb R} \in {\Bbb R}^{n \times n} | {\pmb{RR}}^T = {\pmb I}, det({\pmb R}) = 1\rbrace$$
SO(n)为特殊正交群(Special Orthogonal Group)
加上平移向量，得到向量a在两个坐标系下坐标的关系为：
$$\pmb {a_1 = R_{12}a_2 + t_{12}} $$
此处$${\pmb R_{12}}$$ 是“把2变换到1”，从右到左读；$\pmb t_{12}$ 则是坐标系1原点指向2原点的向量,是在1中取的坐标
由于该变换关系非线性，引入齐次坐标与变换矩阵$\pmb T$(Transform Matrix)
$$\left[\begin{array}{l}
{\pmb a}^{\prime} \
1
\end{array}\right]=\left[\begin{array}{ll}
{\pmb R} & {\pmb t} \
{\pmb 0}^{T} & 1
\end{array}\right]\left[\begin{array}{l}
{\pmb a} \
1
\end{array}\right] \stackrel{\operatorname{def}}{=} {\pmb T}\left[\begin{array}{l}
{\pmb a} \
1
\end{array}\right]$$
这种左上旋转矩阵，右侧平移向量，左右下为0向量和1的矩阵又称特殊欧式群(Special Euclidean Group):
$$\mathrm{SE}(3)=\left{T=\left[\begin{array}{cc}
\boldsymbol{R} & \boldsymbol{t} \
\mathbf{0}^{\mathrm{T}} & 1
\end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \pmb{t} \in \mathbb{R}^{3}\right}$$
旋转向量和欧拉角
由于前述表示方法的冗余性和约束，引入旋转向量(Axis-Angle)描述旋转
利用罗德里格斯公式(Rodrigues’s Formula)表示旋转向量到旋转矩阵的转换关系，并通过取迹(trace)反向转换:
$$\begin{array}{c}
{\pmb R}=\cos \theta {\pmb I}+(1-\cos \theta) {\pmb {n n}}^T+\sin \theta{\pmb n}^{\wedge} \
\
\theta=\arccos \frac{\operatorname{tr}(\pmb R)-1}{2}
\end{array}$$
注意到旋转轴上的向量在旋转后不发生任何改变: $\pmb{ Rn = n}$,
所以转轴$\pmb n$是矩阵$\pmb R$特征值1对应的特征向量
欧拉角，用来直观描述旋转过程。常用：“偏航-俯仰-滚转”(yaw-pitch-roll，rpy角,等价ZYX轴旋转)。
注意二三次都是以旋转后的Y/X轴
由此可用$\left[{r,p,y}\right] ^ T$来描述任意旋转。
但是，其缺点在于会遇到万向锁问题(Gimbal Lock)
找不到不带奇异性的三维向量描述方式
四元数
四元数(Quaternion)既是紧凑的，也没有奇异性
类比复数，三维旋转可以用单位四元数描述: $ \pmb q = q_0 + q_1 i + q_2 j + q_3 k $
或用一个标量和一个向量来表示：
$$\begin{array}{l}
\pmb q=[s,\pmb v]^{\mathrm{T}}, \quad s=q_{0} \in \mathbb{R}, \quad \pmb v=\left[q_{1}, q_{2}, q_{3}\right]^{\mathrm{T}} \in \mathbb{R}^{3}
\end{array}$$
从运算法则来看，大致与复数运算一致。需要注意由于虚部相乘是做外积，不满足交换律 除非虚部共线，外积为0
为什么14讲里四元数的模要用范数符号表示？
四元数表示旋转
首先将三维空间点用虚四元数描述：
$$\pmb p = \left[{0,x,y,z}\right]^T = \left[{0,\pmb v}\right]^T$$
那么旋转后的点$ \pmb p^\prime $可表示为:
$$ {\pmb{p^\prime = q p q^{-1}}} $$
四元数到旋转矩阵和旋转向量的转换:
$$\begin{array}{l}
\pmb {R=v v}^{T}+s^{2} \pmb I+2 s \pmb v^{\wedge}+\left(\pmb v^{\wedge}\right)^{2} \ \
\left{\begin{array}{l}
\theta=2 \arccos q_{0} \
{\left[n_{x}, n_{y}, n_{z}\right]^{\mathrm{T}}=\left[q_{1}, q_{2}, q_{3}\right]^{\mathrm{T}} / \sin \frac{\theta}{2}}
\end{array}\right.
\end{array}$$
相似、仿射、射影变换
- 相似变换 增加一个自由度(7)允许均匀缩放
$$
\pmb {T}_S =
\left[\begin{array}{ll}
{s \pmb R} & {\pmb t} \
{\pmb 0}^{T} & 1
\end{array}\right]
$$ - 仿射变换 $\pmb A$只用是可逆矩阵而不必正交(12)，所以仿射变换后立方体不再是方的，各个面仍为平行四边形
$$
\pmb {T}_A =
\left[\begin{array}{ll}
{ \pmb A} & {\pmb t} \
{\pmb 0}^{T} & 1
\end{array}\right]
$$ - 射影变换 最一般的变换(3D 15)
$$
\pmb {T}_P =
\left[\begin{array}{ll}
{ \pmb A} & {\pmb t} \
{\pmb a}^{T} & v
\end{array}\right]
$$