SLAM十四讲学习总结（二）：三维空间刚体运动

1. 向量及其坐标：向量是空间中一点指向另一点的箭头。而向量的坐标，是在指定了一个空间坐标系以后与向量对应的若干实数。

2. 向量坐标：
选定一组基$(\overrightarrow{e_1},\overrightarrow{e_2},\overrightarrow{e_3})$,则向量$\vec{a}$在这组基下可表示为一个坐标：
$$\vec{a}=[\overrightarrow{e_1},\overrightarrow{e_2},\overrightarrow{e_3}]\left[\begin{array}{c}a1\\ a2\\ a3\end{array}\right]=a_1\overrightarrow{e_1}+a_2\overrightarrow{e_2}+a_3\overrightarrow{e_3}$$
$(a_1,a_2,a_3{)}^T$为向量$\vec{a}$的坐标。坐标取值与向量本身和基都有关系。

3.内积和外积
内积（点乘）：$$\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^{\mathbf{T}}\cdot \mathbf{b}=|\mathbf{a}||\mathbf{b}|cos<\mathbf{a},\mathbf{b}>=\sum _{i=1}^3{a}_i{b}_j$$
外积（叉乘)：
$$\mathbf{a}\times \mathbf{b}=\Vert \begin{array}{ccc}{\mathbf{e}}_1& {\mathbf{e}}_2& {\mathbf{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]\mathbf{b}=\mathbf{a}\hat{}\mathbf{b}$$
叉乘的大小为$|\mathbf{a}||\mathbf{b}|sin<\mathbf{a},\mathbf{b}>|$,在这里^符号将$\mathbf{a}$写成反对称矩阵，所以叉乘可以看成是矩阵和向量的乘法。

4.坐标系欧式变换
刚体运动：三维空间中两个坐标系之间的转换，一次旋转加一次平移。
欧式变换：同一个向量在各个坐标系下的长度和夹角不会发生改变。

4.1旋转表示
设向量$\mathbf{a}$在第一组单位正交基$({\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3)$下坐标为$[a_1,a_2,a_3{]}^T$,在第二组单位正交基$({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime })$下的坐标为$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$，向量在三维空间中的长度和角度固定，故有如下关系：
$$[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
两边同时乘$\left[\begin{array}{c}{\mathbf{e}}_1^{\mathbf{T}}\\ {\mathbf{e}}_2^{\mathbf{T}}\\ {\mathbf{e}}_3^{\mathbf{T}}\end{array}\right]$,得到：
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{e}}_1^{\mathbf{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^{\mathbf{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^{\mathbf{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^{\mathbf{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^{\mathbf{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^{\mathbf{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^{\mathbf{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^{\mathbf{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^{\mathbf{T}}{\mathbf{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}{ccc}{\mathbf{e}}_1^{\mathbf{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^{\mathbf{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^{\mathbf{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^{\mathbf{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^{\mathbf{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^{\mathbf{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^{\mathbf{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^{\mathbf{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^{\mathbf{T}}{\mathbf{e}}_3^{\prime }\end{array}\right]$定义为矩阵$R$,由两组基的内积组成，被称为旋转矩阵，描述了两个坐标系之间的旋转关系。
由于一组基的各个分量长度都为1，所以该旋转矩阵实际上是各基向量夹角的余弦，所以也称方向余弦矩阵，则坐标系旋转可表示为$\mathbf{a}=\mathbf{R}{\mathbf{a}}^{\prime }$。

旋转矩阵是行列式为1的正交矩阵，行列式为1的正交矩阵都是旋转矩阵。
（注：正交矩阵A即$A^{T}A=A{A}^T=E$,逆矩阵为自身转置)
旋转矩阵的逆或者转置代表一次相反的旋转,则：
${\mathbf{a}}^{\prime }={\mathbf{R}}^{\mathbf{T}}\mathbf{a}={\mathbf{R}}^{-1}\mathbf{a}$

将n维旋转矩阵的集合定义如下,即行列式为1的正交矩阵：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\lbrace{\boldsymbol{R}\in R^{n \times n}}|\boldsymbol{R}\boldsymbol{R^T}=\boldsymbol{I},det(\boldsymbol{R})=1\rbrace SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
这里涉及到了群的概念，称SO(n)为特殊正交群。

4.2平移表示
平移只要将平移向量加到旋转以后的坐标上即可，设平移向量为$t$，它是一个坐标系的原点指向另一个坐标系原点的向量。

综上，用一个旋转矩阵$R$和一个平移向量$t$就可以描述一次欧式变换。
设坐标系1、坐标系2，空间中有一向量$\mathbf{a}$，则坐标系2变换到坐标1可表示成如下形式：${\mathbf{a}}_1={\mathbf{R}}_{12}{\mathbf{a}}_2+{\mathbf{t}}_{12}$,其中，
旋转矩阵指“坐标系2的向量旋转到坐标系1”，在这里下标的变换关系都是从右至左。
平移向量指坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标。注意，${\mathbf{t}}_{12}\ne -{\mathbf{t}}_{21}$，尽管从向量层面来说二者确实相反，但所在坐标系不同故不能直接取反。

5.齐次坐标
在三维向量的末尾添加1变为四维向量，称为齐次坐标。这样就可以使每一次欧式变换都写成线性形式，一次欧式变换通过一个变换矩阵T表示，T的形式为：
$$T=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathbf{T}}& 1\end{array}\right]$$
则
$$\left[\begin{array}{c}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathbf{T}}& 1\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]$$

变换矩阵T左上角为旋转矩阵，右上为平移向量，左下为0向量，右下为1，这种矩阵称为特殊欧氏群。
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 4 × 4 } SE(3)=\lbrace{\boldsymbol{T}=\begin{bmatrix} \boldsymbol{R}&\boldsymbol{t}\\ \boldsymbol{0^T}&1 \end{bmatrix}\in R^{4 \times 4}}|\boldsymbol{R}\in SO(3),\boldsymbol{t}\in R^{4 \times 4}\rbrace SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R4×4}
该矩阵的逆代表一个反向的变换，如下：
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathbf{T}}& -{\mathbf{R}}^{\mathbf{t}}\mathbf{t}\\ 0^{\mathbf{T}}& 1\end{array}\right]$$

6.旋转向量及其与旋转矩阵转换
任意旋转都可以由一个旋转轴和一个旋转角刻画，所以用一个长度为旋转角度，方向与旋转轴一致的旋转向量可以表示旋转。

设旋转角为$\theta$,旋转轴为单位向量$\mathbf{n}$，则旋转可以用$\theta \mathbf{n}$描述。

旋转向量到旋转矩阵的转换——罗德里格斯公式：
$\mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^{\mathbf{T}}+sin\theta \mathbf{n}$^

旋转矩阵到旋转向量的转换——矩阵的迹和特征向量：
$tr(\mathbf{R})=cos\theta tr(\mathbf{I})+(1-cos\theta )tr(\mathbf{n}{\mathbf{n}}^{\mathbf{T}})+sin\theta tr(n$^)$=3cos\theta +(1-cos\theta )=1+2cos\theta ⟶$
$$\theta =arccos\frac{tr(\mathbf{R})-1}{2}$$
旋转轴的向量旋转后不改变，则：
$$\mathbf{R}\mathbf{n}=\mathbf{n}$$
可通过求R的特征向量后归一化来求解旋转向量的方向向量。



7.欧拉角
将旋转分解为3次绕不同轴的旋转，即3个分离的转角，更具体地还需要分为绕3个固定轴还是绕旋转后的轴。

Z、Y、X轴欧拉角——yaw（偏航）-pitch（俯仰）-roll（翻滚）——X轴朝向飞机头正前方——每次都是绕旋转之后的轴接着旋转——$[r,p,y{]}^T$向量表示rpy角

万向锁问题：俯仰角（绕y轴）如果为正负90度时，第一次旋转与第三次旋转使用同一个轴，使得系统丢失一个自由度，所以在工程中很少应用。



8.四元数
三维旋转可以用单位四元数描述，四元数有点像复数，定义如下：
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}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}$$
用标量和向量表达四元数：
$\mathbf{q}=[s,\mathbf{v}{]}^T,s=q_0,\mathbf{v}=[q_1,q_2,q_3{]}^T$
s为实部，v为虚部。

四元数具有加减法、乘法、数乘、求模||q||（乘积的模是模的乘积）、共轭（共轭与本身相乘得到实部为模长平方的实四元数）、逆（${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2,q^{-1}q=q{q}^{-1}=1$,单位四元数共轭和逆相同)。

四元数表示旋转步骤：
设点$p=[x,y,z]$,用四元数表示为$\mathbf{p}=[0,x,y,z{]}^T$,经过四元数$\mathbf{q}$的旋转后有如下结果：
$${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}$$
将p‘的虚部取出就是旋转之后点的坐标。
四元数与旋转矩阵的转换关系：
$\mathbf{R}=\mathbf{v}{\mathbf{v}}^{\mathbf{T}}+s^2\mathbf{I}+2s(\mathbf{v}\times )$+$(\mathbf{v}\times {)}^2$
四元数与旋转向量的转换关系：
$$\{\begin{array}{c}\theta =2arccoss\\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}$$



9.除了欧氏变换以外的其他变换
相似变换
$${\mathbf{T}}_{\mathbf{s}}=\left[\begin{array}{cc}s\mathbf{R}& t\\ 0^{\mathbf{T}}& 1\end{array}\right]$$
旋转部分多了一个缩放因子s，在对向量旋转后可以在x、y、z三个坐标上进行均匀缩放，相似变换不再保持图像面积不变，三维相似变换的集合也叫做相似相似变换群，记作Sim(3)。会改变体积比。
仿射变换
$${\mathbf{T}}_{\mathbf{A}}=\left[\begin{array}{cc}\mathbf{A}& t\\ 0^{\mathbf{T}}& 1\end{array}\right]$$
仿射变换只要求A是可逆矩阵而不必是正交矩阵。会改变平行性和体积比。
射影变换
$${\mathbf{T}}_{\mathbf{P}}=\left[\begin{array}{cc}\mathbf{A}& t\\ {\mathbf{a}}^{\mathbf{T}}& v\end{array}\right]$$
左上角为可逆矩阵A，右上角为平移t，左下角为缩放${\mathbf{a}}^{\mathbf{T}}$。这是最一般的变换，从真实世界到相机照片的变换可以看成一个射影变换。

10.Eigen数据结构
旋转矩阵（3x3）：Eigen::Matrix3d
旋转向量（3x1）：Eigen::AngleAxisd 初始化赋值：AngleAxisd a(PI/4,Vector3d(0,0,1)),显示时转换函数为.matrix()或者.toRotationMatrix()。
欧拉角（3x1）：Eigen::Vector3d 可以通过matrix的.eulerAngles的函数转换得到。
四元数（4x1）：Eigen::Quaterniond,可以通过旋转矩阵或者旋转向量初始化赋值。
欧氏变换矩阵（4x4）：Eigen::Isometry3d,添加旋转的方法.rotate(rotation_vector或者rotation_matrix)，添加平移向量的方法.pretranslate(Vector3d)
仿射变换(4x4)：Eigen::Affine3d
射影变换(4x4)：Eigen::Projective3d

习题：
只有部分的解答：
4.
旋转向量到旋转矩阵的转换——罗德里格斯公式：
$\mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^{\mathbf{T}}+sin\theta \mathbf{n}$^
旋转矩阵到旋转向量的转换——矩阵的迹和特征向量：
$tr(\mathbf{R})=cos\theta tr(\mathbf{I})+(1-cos\theta )tr(\mathbf{n}{\mathbf{n}}^{\mathbf{T}})+sin\theta tr(n$^)$=3cos\theta +(1-cos\theta )=1+2cos\theta ⟶$
$$\theta =arccos\frac{tr(\mathbf{R})-1}{2}$$
旋转轴的向量旋转后不改变，则：
$$\mathbf{R}\mathbf{n}=\mathbf{n}$$
可通过求R的特征向量后归一化来求解旋转向量的方向向量。

四元数与旋转矩阵的转换关系：
$\mathbf{R}=\mathbf{v}{\mathbf{v}}^{\mathbf{T}}+s^2\mathbf{I}+2s(\mathbf{v}\times )$+$(\mathbf{v}\times {)}^2$
四元数与旋转向量的转换关系：
$$\{\begin{array}{c}\theta =2arccoss\\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}$$
5.

Matrix4d matrix44 = Matrix4d::Random();
cout<<"matrix44:"<<endl<<matrix44<<endl;
Matrix3d matrix33_extract = matrix44.block(0,0,3,3);
cout<<"matrix33_extract:"<<endl<<matrix33_extract<<endl;
matrix44.block(0,0,3,3)<<Matrix3d::Identity();
cout<<"exchanged_matrix44:"<<endl<<matrix44<<endl;

6.参考eigenMatrix.cpp中几种方程解法。