视觉SLAM-02 三维空间刚体运动

参考书籍： 视觉SLAM十四讲

点与向量

点A： $$A=[a_1,a_2,a_3].$$

相关运算

$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|\cos <a,b>$$

$$a\times b=\Vert \begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert =\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]==\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b=\hat{a}b$$
其中我们定义
$\hat{a}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$

变换矩阵 （transform matrix）

$$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$$
其中

1. T是变换矩阵，属于特殊欧式群(Special Euclidean Group) SE(3)
   $$SE(3)=\left\{\begin{array}{c}T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\end{array}\right\}$$
2. R是旋转矩阵，属于特殊正交群(Special Orthogonal )SO(n)
   $$SO(n)=\left\{\begin{array}{c}R\in {\mathbb{R}}^{n\times n}|R{R}^T=I,det(R)=1\end{array}\right\}$$$$R\in SO(3)$$
3. t是平移向量

齐次坐标

• 与变换矩阵相对应的就是齐次坐标
  简单理解就是$T\in {\mathbb{R}}^4$ 不能与$A=[a_1,a_2,a_3]$直接相乘
  所以我们需要引入齐次坐标$\tilde{A}=[a_1,a_2,a_3,1]$
  这样就可以进行矩阵运算了
  矩阵计算后得到的齐次坐标的第四个参数应该一直为1

矩阵表示的缺点：

1. $R\in {\mathbb{R}}^{3\times 3}$包含9个元素，但对于一次旋转只有三个自由度，所以旋转矩阵是冗余表达方式，同理，变换矩阵$T\in {\mathbb{R}}^{4\times 4}$用16个元素表示六个自由度的变换，也是冗余的。
2. 旋转矩阵$R\in SO(3)$自身是一个正交矩阵，且$det(R)=1$，所以在计算中，这个约束会使得求解变得困难
   综上我们需要找到一个紧凑得方式描述旋转和平移。

旋转向量

我们知道旋转具有3自由度，变换具有6个自由度。所以很容易的想到用一个三维向量描述旋转，六维向量描述变换。

旋转是由旋转轴和旋转角描述的。
所以旋转向量

1. 方向等于旋转轴
2. 长度等于旋转角

考虑一个向量$n$，方向为旋转轴，长度为单位向量，旋转角为$\theta$，那么向量$\theta n$ 就可以描述这个旋转，
那么旋转矩阵和其关系为：
$$R=\cos \theta \mathrm{I}+(1-\cos \theta )n{n}^T+\sin \theta \hat{n}$$
$\hat{n}$ 应该写为n^不过latex有点小问题。
^是向量到反对称矩阵的转换符。
对上式两端取trace
我们可以得到
$$\theta =\frac{\arccos (tr(R)-1)}{2}$$

关于转轴$n$，旋转操作并不会改变旋转轴：
$$Rn=n$$

欧拉角

使用三个分离的转角，把一个旋转分解成3次绕不同轴的旋转

偏航-俯仰-滚转（yaw-pitch-roll）表示方法

1. 绕物体的Z轴旋转，得到偏航角 yaw
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch
3. 绕旋转之后的X轴旋转，得到滚转角roll

用 $[r,p,y{]}^T$ 这样一个三维向量就可以描述任意旋转，十分直观易于理解

万向锁问题（Gimbal lock）

在俯仰角为$\pm 90^{\circ }$ 时，第一次旋转和第三次旋转将使用同一个轴，使得整个系统丢失了一个自由度，被称作奇异性问题。
理论上可以证明，只要想用三个实数表达三维旋转，都会遇到奇异性问题。所以欧拉角不适用于插值和迭代，往往仅使用于人机交互中。

四元数(Quaternion)

$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$

1. 紧凑且无奇异性
2. 不够直观，运算复杂

$i,j,k$作为四元数的三个虚部，满足：
$$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$
所以我们也可以用一个标量和一个向量来表达四元数:
$$q=[s,v{]}^T,s=q_0\in \mathbb{R},v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$

四元数的运算

1. 加减法

$$q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b{]}^T$$

2. 乘法

$$q_a{q}_b=s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b+(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i+(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)j+(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b)k$$
$$q_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b{]}^T$$
NOTE：由于$v_a\times v_b$的存在，所以乘法不可交换，除非$v_a,v_b\in {\mathbb{R}}^3$共线即$v_a\times v_b=0$

3.模长

$$\Vert q_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$
$$\Vert q_a{q}_b\Vert =\Vert q_a\Vert \Vert q_b\Vert$$

4.共轭

$$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a{]}^T$$
$$q^{\ast }q=q{q}^{\ast }=[s_a^2+v^{T}v,0{]}^T$$

5.逆

$$q^{-1}=q^{\ast }/\Vert q{\Vert }^2$$
$$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}$$

6.数乘

$$kq=[ks,kv{]}^T$$

四元数表示旋转

点A: $a=[x,y,z]\in {\mathbb{R}}^3$
转换为p: $p=[0,x,y,z{]}^T=[0,v{]}^T$
$$p^{\prime }=qp{q}^{-1}$$
$p^{\prime }$的虚部就是旋转后的$A’$，可以发现，这样计算出的结果，实部为0

四元数与其他表示之间的转换

$$q^{+}=\left[\begin{array}{cc}s& -v^T\\ v& sI+\hat{v}\end{array}\right],q^{⨁}=\left[\begin{array}{cc}s& -v^T\\ v& sI-\hat{v}\end{array}\right]$$
$$q^{+},q^{⨁}\in {\mathbb{R}}^{4\times 4}$$
$$\therefore q_1{q}_2=q_1^{+}q2=q_2^{⨁}{q}_1$$
$$p^{\prime }=qp{q}^{-1}=q^{+}{p}^{+}{q}^{-1}=q^{+}{q}^{-1⨁}p$$

其他变换

变换名称	矩阵形式	自由度	不变性质
欧式变换	$\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$	6	长度、夹角、体积
相似变换	$\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]$	7	体积比
仿射变换	$\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]$	12	平行性、体积比
射影变换	$\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$	15	接触平面的相交和相切