目标：理解slam的框架以及它的理论知识。供以后自己查阅。

第三章：主要讲一些基本的数学知识，包括点，向量，旋转矩阵三大类，高翔老师图文并用的方式讲的非常清晰。

第一部分

点
点需要在某个坐标系下的表达，一般在世界坐标系表达，一般情况是$e_0=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$, $e_1=\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]$,$e_2=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$。在日常表达中，这个这类基向量是省略的。
点存在于三维空间之中，因此在上述向量基空间中，由一个三维向量表示$\overrightarrow{p}=(x,y,z)$。

向量
因为有点的表达，但是点与点之间关系需要数学表达方式，它就也是由向量表示。也是由一个三维向量表示$AB=(a,b,c)$，它只有方向和大小的概念。同时它在不同的坐标系下，一般和点在用一个坐标系下（也可以等同于同一个向量基）。

点可以向量都可以用向量表示。因此基于向量的操作，有以下几类。

点乘（内积）：$$\overrightarrow{a}\cdot \overrightarrow{b}={\overrightarrow{a}}^T\overrightarrow{b}=\sum _{i=1}^3{a}_i{b}_i=|\overrightarrow{a}|\overrightarrow{b}|cos(a,b)$$

叉乘（外积）：$$\overrightarrow{a}\times \overrightarrow{b}=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-b_1{a}_3\\ a_1{b}_2-a_{2}b1\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]\overrightarrow{b}=skew(\overrightarrow{a})\overrightarrow{b}$$

其中$skew(\overrightarrow{a})$为反对称矩阵。

第二部分

三维刚体运动
上述介绍了一些向量和点等等的基本运算。进一步需要知道在空间中，坐标系是怎么变化的。以及怎么样计算不同坐标系之间，同一个点的坐标表达。

高翔老师介绍的怎么样从一个坐标系转化为另一个坐标系。可以看到，它在自己的坐标系下，先进行旋转，使得它和目标坐标系方向一致，轴对应。然后用平移向量，移动到目标坐标系即可。它包括这两个过程，使用表达方式为：旋转，平移。

１）旋转：

向量基：它是一组相互垂直的单位向量组成的。一般坐标系都是一组向量基组成，
假设原始向量基为：$\overrightarrow{e_0}$,$\overrightarrow{e_{\mathrm{１}}}$,$\overrightarrow{e_{\mathrm{２}}}$。目标向量基为：${\overrightarrow{e_0}}^{\prime }$,${\overrightarrow{e_{\mathrm{１}}}}^{\prime }$,${\overrightarrow{e_{\mathrm{２}}}}^{\prime }$。假设一个向量$\overrightarrow{a}$是固定向量，它不随着坐标系变化而移动，因此还是$\overrightarrow{a}$，但是它不同坐标系下，它的坐标发生了变化。可以表达为：
$$\left[\begin{array}{ccc}\overrightarrow{e_0}& \overrightarrow{e_1}& \overrightarrow{e_2}\end{array}\right]\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{ccc}{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_2}}^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_0^{\prime }\\ a_1^{\prime }\\ a_2^{\prime }\end{array}\right](1)$$

其中$\overrightarrow{a}$在$E=\left[\begin{array}{ccc}\overrightarrow{e_0}& \overrightarrow{e_1}& \overrightarrow{e_2}\end{array}\right]$坐标系下的坐标；${\overrightarrow{a}}^{\prime }$在$E^{\prime }=\left[\begin{array}{ccc}{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_2}}^{\prime }\end{array}\right]$ 坐标系下的坐标。

对$1$公式左乘$E$：
１）因为$E$是一组向量基组成的向量，因此$E^{T}E=I$为单位矩阵。因此左边的为$\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]$（因为${\overrightarrow{e_0}}^T\overrightarrow{e_1}$=0）,通过计算容易得到。
２）因为１）属性，得到如下：
$$\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{ccc}{\overrightarrow{e_0}}^T{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_0}}^T{\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_0}}^T{\overrightarrow{e_2}}^{\prime }\\ {\overrightarrow{e_1}}^T{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_1}}^T{\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_1}}^T{\overrightarrow{e_2}}^{\prime }\\ {\overrightarrow{e_2}}^T{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_2}}^T{\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_2}}^T{\overrightarrow{e_2}}^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_0^{\prime }\\ a_1^{\prime }\\ a_2^{\prime }\end{array}\right]=\mathbf{R}\left[\begin{array}{c}{a}_0^{\prime }\\ a_1^{\prime }\\ a_2^{\prime }\end{array}\right](2)$$
因此得到$\mathbf{R}=\left[\begin{array}{ccc}{\overrightarrow{e_0}}^T{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_0}}^T{\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_0}}^T{\overrightarrow{e_2}}^{\prime }\\ {\overrightarrow{e_1}}^T{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_1}}^T{\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_1}}^T{\overrightarrow{e_2}}^{\prime }\\ {\overrightarrow{e_2}}^T{\overrightarrow{e_0}}^{\prime }& {\overrightarrow{e_2}}^T{\overrightarrow{e_1}}^{\prime }& {\overrightarrow{e_2}}^T{\overrightarrow{e_2}}^{\prime }\end{array}\right]$

１：$\mathbf{R}$它是一个单位正交矩阵，因为$E,E^{\prime }$都是单位正交矩阵，可以自己计算，两两向量相乘，非常容易得到。这类的矩阵也是逆等于它的转置。
２：$\mathbf{R}$的秩为$\mathrm{１}$

这类可以表达为数学形式为：
$$SO(n)=\mathbf{R}\in R^{n\times n}|\mathbf{R}{\mathbf{R}}^T=I,det(\mathbf{R})=1(3)$$

公式$(3)$称这类旋转矩阵为特殊正交群（群的概念第$4$）章将介绍。

因为在空间坐标系下$n=3$。满足以上公式的$3\times 3$为空间坐标系下的旋转矩阵。$n=3$代入上述公式$(3)$得到三维空间的旋转矩阵，也是slam中的基本概念$SO(3)$：
$$SO(3)=\mathbf{R}\in R^{3\times 3}|\mathbf{R}{\mathbf{R}}^T=I,det(\mathbf{R})=1(3)$$

它有如下几种性质：
假设：$a_1={\mathbf{R}}_{12}{a}_2$; $a_2={\mathbf{R}}_{21}{a}_1$,其中${\mathbf{R}}_{12},{\mathbf{R}}_{21}$在两个坐标系的相互转化的旋转矩阵。它们的关系为：
$${\mathbf{R}}_{12}={\mathbf{R}}_{21}^{-1}={\mathbf{R}}_{21}^T(\mathrm{４})$$

公式$4$非常容易得到，因为它是它是一个正交矩阵，通过公式可以得到，以后证明,很简单，直接相乘就可以了，相乘后除了对角线为１，其它都为０。

２）平移：

这个简单，暂且省略。它表达为$t$

３）旋转加平移（后续$n=3$）：

基于上述的先旋转后平移得到公式如下：
$${\overrightarrow{a}}^{\prime }=\mathbf{R}\overrightarrow{a}+t(5)$$

对于多次坐标系转化，上述方式写法，不容易写，因此它们统一到矩阵表达上，采用了齐次坐标的表达方式。如下：
$$\left[\begin{array}{c}{\overrightarrow{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}\overrightarrow{a}\\ 1\end{array}\right]=\mathbf{T}\left[\begin{array}{c}\overrightarrow{a}\\ 1\end{array}\right](6)$$

因此多次坐标系的转化可以表达为：
$$\overrightarrow{b}={\mathbf{T}}_1\overrightarrow{a},\overrightarrow{c}={\mathbf{T}}_2\overrightarrow{b}=>\overrightarrow{c}={\mathbf{T}}_2{\mathbf{T}}_1\overrightarrow{a}(7)$$

这类引入齐次坐标系的旋转平移向量，用统一代数表达为：
$$SE(3)=\left\{\begin{array}{c}\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|\mathbf{R}\in SO(3),T\in R^3\end{array}\right\}(8)$$

公式$8$可以得到$\mathbf{T}$，这类矩阵也被称为特殊欧式群。它的逆变化为：
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^{T}t\\ 0^T& 1\end{array}\right](9)$$

４）角轴

上面介绍的理论知识非常重要。但是它都是基于矩阵的表达形式，它和矩阵表达的意义等价的。下面介绍另一种表达形式（角轴）。
在真实的世界中，向量也可以表示旋转矩阵，它比上面的矩阵表示更为紧凑。如下图，给出了物理意义上的向量表示旋转。

从上图可以看到旋转可以表示为绕着一根轴旋转一定的角度。使用$W=\theta \overrightarrow{n}$表示。其中$\theta$为角度，$\overrightarrow{n}$表示旋转轴。可以见我之前写的blog，(我使用的是$\overrightarrow{u}$表示，这里使用$\overrightarrow{n}$表示，其实意思是一样的)。
https://blog.csdn.net/weixin_43851636/article/details/126004987?spm=1001.2014.3001.5502

它计算罗德里公式：
$$\mathbf{R}=Icos\theta +(1-cos\theta )\overrightarrow{n}{\overrightarrow{n}}^T+\overrightarrow{n}\times sin\theta (10)$$

它的逆变化，从$\mathbf{R}$到旋转角，和角轴$\theta ,\overrightarrow{n}$为（那篇blog后续补上这个推导）：
角度：
$$\theta =arccos(\frac{tr(\mathbf{R})-1}{2})(11)$$
轴：
$$\mathbf{R}\overrightarrow{n}=\overrightarrow{n}(12)$$

上述的表达，可以看到旋转，可以按照某种顺序旋转。如果按照$Z-Y-X$的顺序旋转物体。
１）绕着$Z$轴旋转，得到的是偏航角yaw。
２）绕着$Y$轴旋转，得到是俯仰角pitch。
３）绕着$x$轴旋转，得到是翻滚角roll。
它符合人的视角，但是在计算不好。还有一些缺陷。比如旋转顺序，还有万向锁的问题等等，用的不多，所以简略记录。

四元素

上述的欧拉角具有的缺点，四元素不具有，它具有广泛性，且紧凑性，在工程中常常遇到。它也是可以表达旋转矩阵。

四元素定义：一种扩展的复数，
（二维的）可以表达平面旋转。
表达为：
$$\begin{gathered} \mathbf{q}=q_0+q_1\overrightarrow{i}+q_2\overrightarrow{j}+q_3\overrightarrow{k}(13) \\ \{\begin{array}{l}{\overrightarrow{i}}^2={\overrightarrow{j}}^2={\overrightarrow{k}}^2=-1\\ \overrightarrow{i}\overrightarrow{j}=\overrightarrow{k},\overrightarrow{j}\overrightarrow{i}=-\overrightarrow{k}\\ \overrightarrow{j}\overrightarrow{k}=\overrightarrow{i},\overrightarrow{k}\overrightarrow{j}=-\overrightarrow{i}\\ \overrightarrow{k}\overrightarrow{i}=\overrightarrow{j},\overrightarrow{i}\overrightarrow{k}=-\overrightarrow{j}\end{array} \end{gathered}$$
它表达形式也可以为：
$$\begin{gathered} \mathbf{q}=[s,\overrightarrow{v}],s=q_0\in R,\overrightarrow{v}=[q_1,q_2,q_3{]}^T\in R^3(14) \\ \{\begin{array}{l}{\overrightarrow{i}}^2={\overrightarrow{j}}^2={\overrightarrow{k}}^2=-1\\ \overrightarrow{i}\overrightarrow{j}=\overrightarrow{k},\overrightarrow{j}\overrightarrow{i}=-\overrightarrow{k}\\ \overrightarrow{j}\overrightarrow{k}=\overrightarrow{i},\overrightarrow{k}\overrightarrow{j}=-\overrightarrow{i}\\ \overrightarrow{k}\overrightarrow{i}=\overrightarrow{j},\overrightarrow{i}\overrightarrow{k}=-\overrightarrow{j}\end{array} \end{gathered}$$

它的虚部的向量基和相机坐标系中的右手坐标系一样。
这些四元素的基本运算如下，它是基本的运算规则：

这些可以手动运算一下验证，这些都是基本的代数运算规则。
基于四元素的定义，它怎么表示三维旋转？
答：
直观上可以得到从角轴到四元素（形式转换）：
$$\mathbf{q}=\left[\begin{array}{cccc}cos\frac{\theta }{2}& n_{x}sin\frac{\theta }{2}& n_{y}sin\frac{\theta }{2}& n_{z}sin\frac{\theta }{2}\end{array}\right]$$
四元素到角轴：

$$\{\begin{array}{l}\theta =2arccos{q}_0\\ {\left[\begin{array}{ccc}{n}_x& n_y& n_z\end{array}\right]}^T={\left[\begin{array}{ccc}{q}_1& q_2& q_3\end{array}\right]}^T/sin\frac{\theta }{2}\end{array}$$
以上是和欧拉角进行互算，但是基于物理意义上，怎么利用四元素对空间的一个点进行旋转？
假设一个顶点$p=\left[\begin{array}{cccc}0& x& y& z\end{array}\right]$ 经过$\mathbf{q}$旋转后得到$p^{\prime }$，这类关系怎么用代数表达？
旋转后得到如下：$p^{\prime }=\mathbf{q}p{\mathbf{q}}^{-1}$
它是先转到四维空间（第一维不为０），然后再转回三维空间。
实践部分，后面补充。