slam第3讲三维空间刚体运动

学习目标

1. 三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数、欧拉角
2. Eigen库的矩阵、几何模块的使用方法

旋转矩阵

三维空间点的描述

假设在一个线性空间中，找到了空间中的一组基$(e_1,e_2,e_3)$，那么任意向量$\vec{a}$在这组基下就有一个坐标
$$a=\left[\begin{array}{ccc}{e}_1& e_2& e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3$$
这里$(a_1,a_2,a_3{)}^T$称为a在此基下的坐标，坐标的具体取值，一是和向量本身有关，二是和坐标系(基)的选取有关

向量的点积

$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos<a,b>$$
其中<a,b>指向量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& b3\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$$外积的结果视一个向量，它的方向垂直于这两个向量，大小为$|a||b|sin<a,b>$，是两个向量张成的四边形有向面积。

欧式变换

欧式变换解决的问题

相机视野中的某个向量$p$，它在相机坐标系下的坐标为$p_c$，而从世界坐标系下看，它的坐标为$p_w$,那么，这两个坐标之间是如何转换的呢？这时，就需要先得到该点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中。我们用一个矩阵$T$来描述。

欧式变换由旋转和平移组成。首先考虑旋转，设单位正交基$（e_1,e_2,e_3）$经过旋转变成$(e_1^{\prime },e_2^{\prime },e_3^{\prime })$。设向量$a$在两个坐标系下的坐标为$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$。根据坐标的定义有：
$$\left[\begin{array}{ccc}{e}_1& e_2& e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^{\prime }& e_2^{\prime }& e_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
将上式左右两边同时左乘$\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$，则左边系数变为单位阵，则有：
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]=R{a}^{\prime }$$
矩阵$R$即为旋转矩阵，也被称为方向余弦矩阵，它是一个行列式为1的正交矩阵，反之也成立，可以将$n$维旋转矩阵的集合定义如下：
$$SO(n)=\left\{R\in R^{n\times n}|R{R}^T=I,det(R)=1\right\}$$
$SO(n)$是特殊正交群，$SO(3)$就是指三维空间的旋转。通过旋转矩阵，我们可以直接讨论两个坐标系之间的旋转变换，而不用再从基开始谈起。
由于旋转矩阵为正交矩阵，它的逆(转置)描述了一个相反的旋转，按照上面的定义方式有：
$$a^{\prime }=R^{-1}a=R^{T}a$$
$R^T$刻画了一个相反的旋转
考虑到欧式变换中的平移$t$，则有
$$a^{\prime }=Ra+t$$
上面的描述还存在一个问题就是当变换存在多次时描述会变得很繁琐，因此我们引入齐次坐标重写矩阵：
$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$$
这是一个数学技巧：我们在三维向量得末尾添加1，将其变成了四维向量，称为齐次坐标，矩阵$T$称为变换矩阵。
关于变换矩阵$T$，它具有比较特别得结构，左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1。这种矩阵又称为特殊欧式群：
$$SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|R\in SO(3),t\in R^3\right\}$$
与$SO(3)$一样，求解该矩阵得逆表示一个反向得变换：
$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$
综上我们介绍了向量及其坐标表示，并介绍了向量间得运算；然后坐标系之间得运动由欧式变换描述，它由平移和旋转组成。旋转可以由旋转矩阵$S0(3)$描述，而平移直接由一个$R^3$向量描述。最后，如果将平移和旋转放在一个矩阵中，就形成了变换矩阵$SE(3)$

旋转向量和欧拉角

旋转向量

当用变换矩阵描述一个6自由度得三维刚体运动时，存在以下两个缺点：
1.$S0(3)$得旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余得。同理，变换矩阵用16个量表达了6个自由度得变换。那么，是否有更紧凑得表示呢？
2. 旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变得更困难。
定义一个旋转向量，该旋转向量由一个旋转轴和一个旋转角来描述，假设旋转轴为一个单位长度得向量$n$,角度为$\theta$，，那么向量$\theta n$来描述这个旋转。则旋转向量到旋转矩阵得转换有：
$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n$^
符号^是向量到反对称矩阵得转换符。反之也可以从一个旋转矩阵到旋转向量得转换。
$\theta =arccos\frac{tr(R)-1}{2}$

欧拉角

用“偏航-俯仰-滚转”(yaw-pitch-roll)3个角度来描述一个旋转，假设一个刚体的前方(朝向我们的方向)为X轴，右侧为Y轴，上方为Z轴，那么，ZYX转角相当于把任意旋转分解成以下3个轴上的转角：

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

四元数

一个四元数$q$拥有一个实部和三个虚部：
$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中,i,j,k为四元数的三个虚部。这三个虚部满足以下关系式：
$$\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$
如果把i，j，k看成三个坐标轴，那么它们与自己的乘法和复数一样，相互之间的乘法和外积一样，有时也可以用一个标量和一个向量来表达四元数：
$$a=[s,v{]}^T,s=q_0\in R,v=[q_1,q_2,q_3{]}^T\in R^3$$
这里，s称为四元数的实部，而v称为它的虚部。如果一个四元数的虚部为0，则称为实四元数，反之，若它的实部为0，则称为虚四元数。

四元数的运算

1. 加法和减法
   四元数$q_a,q_b$的加减运算为：
   $$q_a+q_b=[s_a\pm s_b,v_a\pm v_b{]}^T$$

2. 乘法
   乘法是将$q_a$的每一项和$q_b$的每一项进行相乘，最后相加
   $$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$$

3. 模长
   $$||q_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$

4. 共轭
   四元数的共轭是把虚部取成相反数：
   $$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a{]}^T$$

5. 逆
   一个四元数的逆为：
   $$q^{-1}=q^{\ast }/||q|{|}^2$$

6. 数乘

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

相似变换

相似变换比欧式变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为：
$$T_s=\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]$$

放射变换

$$T_A=\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]$$

摄影变换

$$T_p=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$$
综上常见的变换性质比较：