这篇博客主要介绍一下SLAM中与位姿有关的数学基础知识。
1.向量点积
有两个向量
\[a=\begin{pmatrix} a_{1}\\ a_{2}\\ a_{3} \end{pmatrix},b=\begin{pmatrix} b_{1}\\ b_{2}\\ b_{3} \end{pmatrix}\]那么它们的点乘定义如下
\[a\cdot b=a^{T}\cdot b = \sum_{i=1}^{3}a_{i}b_{i}=\left | a \right |\left | b \right |cos<a,b>\]简单解释一下，两个向量点乘等于一个向量的转置乘以另一个向量。这个其实是回答了之前在推导对极约束公式的时候，方程两边同时乘以\(x_{2}^{T}\)而不是\(x_{2}\)的原因，参考这篇博客。 或者更直白点说，a、b都是列向量，也就是3×1的矩阵，直接相乘从矩阵乘法的角度是无法运算的。因此要转置。
2.向量叉积
两个向量做叉积结果还是一个向量，这个向量垂直于这两个做叉积的向量所组成的平面。
\[a\times b=\begin{vmatrix} i & j & k\\ a_{1} & a_{2} & a_{3}\\ b_{1} & b_{2} & b_{3} \end{vmatrix}=(a_{2}b_{3}-a_{3}b_{2})i+(a_{3}b_{1}-a_{1}b_{3})j+(a_{1}b_{2}-a_{2}b_{1})k\]为了计算方便，将两个向量写成行列式的形式，然后按对角线法则计算行列式的值。i、j、k分量分别即代表x、y、z轴方向的分量。写成列向量的形式如下
\[\begin{pmatrix} 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{pmatrix}=\begin{pmatrix} 0\cdot b_{1}-a_{3}b_{2}+a_{2}b_{3}\\ a_{3}b_{1}-0\cdot b_{2}-a_{1}b_{3}\\ -a{2}b_{1}+a_{1}b_{2}+0\cdot b_{3} \end{pmatrix}=\begin{pmatrix} 0 & -a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2} & a_{1} & 0 \end{pmatrix}\begin{pmatrix} b_{1}\\ b_{2}\\ b_{3} \end{pmatrix}\]再将其写成与b相乘的矩阵形式，提取系数矩阵。这个系数矩阵就称作向量a的反对称矩阵，用a^记。所谓反对称矩阵，是指满足下面这个条件的矩阵。
因为向量叉积的结果是一个向量，因此可以用来表示旋转，结果向量的方向为旋转轴，大小为旋转角。结果可以看作一个旋转向量。
3.欧氏变换
(1)旋转
对于单位正交基\((e_{1},e_{2},e_{3})\)，变成了\((e_{1}^{'},E_{2}^{'},e_{3}^{'})\)。某个向量a没有发生变化，其在两个坐标系下的坐标分别为\((a_{1},a_{2},a_{3}),(a_{1}^{'},a_{2}^{'},a_{3}^{'})\)。则有下面等式成立
\[\begin{pmatrix} e_{1} & e_{2} & e_{3} \end{pmatrix}\begin{pmatrix} a_{1}\\ a_{2}\\ a_{3} \end{pmatrix}=\begin{pmatrix} e_{1}^{'} & e_{2}^{'} & e_{3}^{'} \end{pmatrix}\begin{pmatrix} a_{1}^{'}\\ a_{2}^{'}\\ a_{3}^{'} \end{pmatrix}\]方程两边左乘\((e_{1}^{T},e_{2}^{T},e_{3}^{T})^{T}\)，这样左边就是个单位阵，得到如下结果
\[\begin{pmatrix} a_{1}\\ a_{2}\\ a_{3} \end{pmatrix}=\begin{pmatrix} e_{1}^{T}\\ e_{2}^{T}\\ e_{3}^{T} \end{pmatrix}\begin{pmatrix} e_{1}^{'} & e_{2}^{'} & e_{3}^{'} \end{pmatrix}\begin{pmatrix} a_{1}^{'}\\ a_{2}^{'}\\ a_{3}^{'} \end{pmatrix}=\begin{pmatrix} e_{1}^{T}e_{1}^{'} & e_{1}^{T}e_{2}^{'} & e_{1}^{T}e_{3}^{'}\\ e_{2}^{T}e_{1}^{'} & e_{2}^{T}e_{2}^{'} & e_{1}^{2}e_{3}^{'}\\ e_{3}^{T}e_{1}^{'} & e_{3}^{T}e_{2}^{'} & e_{3}^{T}e_{3}^{'} \end{pmatrix}\begin{pmatrix} a_{1}^{'}\\ a_{2}^{'}\\ a_{3}^{'} \end{pmatrix}\]定义这个系数矩阵为R，也即旋转矩阵。它的各个元素是旋转前后两组基之间的内积，刻画了旋转前后同一向量的坐标变换关系。同时R是一个行列式为1的正交阵。既然是正交阵，所以R的逆等于R的转置。
(2)平移
平移相对简单，直接在旋转后的坐标上增加平移量即可。
\[a^{'}=Ra+t\]4.齐次坐标与变换矩阵
(1)齐次坐标
源自射影几何。对于一个三维向量，添加最后一维，用4个实数表达一个三维向量。在齐次坐标中，某个点的每个分量同乘一个非0常数后，仍为同一个点。但当最后一项不为0时，我们可以把所有坐标同除最后一项，使其为1。这样就得到了该点的唯一坐标。这其实可以看作我们刚刚增加一个维度添1的逆过程。因此忽略最后一项，显然这就是欧氏空间中的坐标。
理解了齐次坐标后，再看相机的小孔成像模型公式推导中的齐次坐标的转换就不会那么突兀了。
首先，P点的相机坐标系下的坐标和像素坐标关系如下
\[\left\{\begin{matrix} u=\frac{f_{x}}{Z}X+c_{x}\\ v=\frac{f_{y}}{Z}Y+c_{y} \end{matrix}\right.\]由于我们是想要寻找(X,Y,Z)和(u,v)的关系，而且最好写成矩阵形式。所以自然地想到，如果要是能表达成p=AP这样的形式就好了。其中p是像素坐标，P是相机坐标系下的坐标。因此不妨试试看。
首先将上面的方程左右两边同乘Z，这样就可以把分数消掉了，看起来也简单些。
\[\left\{\begin{matrix} Zu=f_{x}X+c_{x}Z\\ Zv=f_{y}Y+c_{y}Z \end{matrix}\right.\]然后试着将其补0，构造成对应形式
\[\left\{\begin{matrix} Zu=f_{x}X+0Y+c_{x}Z\\ Zv=0X+f_{y}Y+c_{y}Z \end{matrix}\right.\]写成矩阵形式
\[Z\begin{pmatrix} u\\ v\\ \end{pmatrix}=\begin{pmatrix} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ & & \end{pmatrix}\begin{pmatrix} X\\ Y\\ Z \end{pmatrix}\]很明显，要想让等式成立，必须在像素坐标上补1，也就是变成齐次才行。所以变成齐次后如下
\[Z\begin{pmatrix} u\\ v\\ 1 \end{pmatrix}=\begin{pmatrix} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ & & \end{pmatrix}\begin{pmatrix} X\\ Y\\ Z \end{pmatrix}\]这样，就可以自然地把系数矩阵中空白的部分填上了。
\[Z\begin{pmatrix} u\\ v\\ 1 \end{pmatrix}=\begin{pmatrix} f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0 & 0 & 1 \end{pmatrix}\begin{pmatrix} X\\ Y\\ Z \end{pmatrix}\]中间的系数矩阵便成为内参矩阵K。
(2)变换矩阵
之所以要有变换矩阵，是因为使用R、t分开来表示欧氏变换太过麻烦，变换后的表达式不是线性关系。如有两个变换
\[b=R_{1}a+t_{1},c=R_{2}b+t_{2}\]那么如果是从a到c的变换，则写为
\[c=R_{2}(R_{1}a+t_{1})+t_{2}\]这种形式显然看起来非常不爽。因此需要寻求一种将R、t写在一起的表达形式，那么最好的办法就是写成矩阵。因为矩阵乘法很好地联系了乘法与加法。
对于某个变换
\[a^{'}=Ra+t\]首先尝试把R、t写成矩阵形式，写在一起
\[\begin{pmatrix} a^{'}\\ \end{pmatrix}=\begin{pmatrix} R & t\\ & \end{pmatrix}\begin{pmatrix} a\\ 1 \end{pmatrix}\]这样其实相当于把a变成了齐次坐标，所以a’也可以变成齐次坐标。变成齐次坐标使方程可以写成线性形式。变齐次是加“1”而不是加“0”。虽然从数学上来说加什么都可以使等式成立，但是那样就不代表齐次坐标了。
\[\begin{pmatrix} a^{'}\\ 1 \end{pmatrix}=\begin{pmatrix} R & t\\ & \end{pmatrix}\begin{pmatrix} a\\ 1 \end{pmatrix}\]这样顺理成章的，便能将系数矩阵中空白的部分填满了。
\[\begin{pmatrix} a^{'}\\ 1 \end{pmatrix}=\begin{pmatrix} R & t\\ 0 & 1 \end{pmatrix}\begin{pmatrix} a\\ 1 \end{pmatrix}\]那么，我们就把这个系数矩阵叫做变换矩阵T。这样以后再表示多次变换的时候，表达就是线性的了。
\[\begin{matrix} a^{'}=T_{1}a,b^{'}=T_{2}a^{'}\\ b^{'}=T_{2}(T_{1}a)=T_{2}T_{1}a \end{matrix}\]与旋转矩阵R类似，T逆表示一个反向的变换。
仔细观察变换矩阵T和上面推导的内参矩阵K的构造，会发现有异曲同工之妙，很相似。直接给出结论，内参矩阵K其实就是射影变换中的变换矩阵T。射影变换是最一般的变换，
\[T=\begin{pmatrix} A & t\\ a^{T} & v \end{pmatrix}\]左上角A为可逆矩阵，右上角t为平移，左下角为缩放a。由于采用齐次坐标，当v不为0时，可以对整个矩阵除以v得到一个右下角为1的矩阵。
对照可以发现K满足它的条件。也即左上角部分是可逆的。为什么说K的左上角可逆呢？因为满足可逆条件，也即它是方阵且行列式不等于0。
\[det(A)=\begin{vmatrix} f_{x} & 0\\ 0 & f_{y} \end{vmatrix}=f_{x}f_{y}\]正常情况下，焦距都不可能为0。所以这部分可逆。
从真实世界到相机照片的变换可以看成是一个射影变换。如果相机的焦距为无穷远，这个变换是一个仿射变换。仿射变换的矩阵形式如下
\[T_{A}=\begin{pmatrix} A & t\\ 0^{T} & 1 \end{pmatrix}\]仿射变换只要求A是可逆矩阵而不必是正交矩阵。仿射变换也叫做正交投影。
5.旋转向量
前面在说向量叉积的时候也简单提到过。对于任意一个旋转，其可以用旋转轴+旋转角的方式刻画。向量方向与旋转轴一致，长度等于旋转角(弧度)。旋转轴为n，角度为θ，那么旋转向量即为θn。由旋转向量转到旋转矩阵，利用罗德里格斯公式。
\[R=cos\theta\cdot I+(1-cos\theta)\cdot nn^{T}+sin\theta \cdot n^{\times}\]这里\(n^{\times}\)表示向量n对应的反对称矩阵。
而从旋转矩阵到旋转向量的转换
\[\theta = arccos(\frac{tr(R)-1}{2})\]tr(R)表示R的迹，也就是R的对角线元素之和，在前面说过。而n是R的特征值为1对应的特征向量。解下面这个方程，归一化即可。
\[Rn=n\]6.欧拉角
欧拉角部分在前面已经有很详细的介绍了，这里就不再赘述了。可以参考这篇博客。
7.四元数
四元数同样在这篇博客中也进行了介绍，这里说一下之前没有说的四元数与旋转向量间的转换。
假设某个绕单位向量\(n=(n_{x},n_{y},n_{z})^{T}\)进行了角度为θ的旋转，其对应的四元数为
\[q=(cos\frac{\theta}{2},n_{x}sin\frac{\theta}{2},n_{y}sin\frac{\theta}{2},n_{z}sin\frac{\theta}{2})^{T}\]反之有
\[\begin{matrix} \theta = 2arccosq_{0}\\ (n_{x},n_{y},n_{z})^{T}=[q_{1},q_{2},q_{3}]^{T}/sin\frac{\theta}{2} \end{matrix}\]而四元数与旋转矩阵的转换，同样在这篇博客中已经说过了。
8.四元数转换公式推导
在由旋转向量转四元数的公式中可以发现，旋转了θ角，但在公式中却变成了θ/2角。莫名有种转了一半的感觉。这是为什么呢？这里首先给出结论：因为四元数的共轭作用，每次旋转都是原来的2倍，所以要变成θ/2。好了，结论就是这样，下面是比较详细的推导。如果对推导过程不感兴趣，可以直接跳过就好。
推导过程主要参考了这个网页上的内容。整个推导过程分为以下几个不同部分，最终证明上面提到的结论。
(1)复数与旋转
对于一个平面而言，每一个旋转(关于原点)都可以用一个复数来表示
\[z_{\theta}=cos\theta + i sin\theta\]因为对于每个点(x,y)=x+iy来说，
\[z_{\theta}(x+iy)=(cos\theta + i sin\theta)(x+iy)\]整理得
\[cos\theta \cdot x+cos\theta \cdot iy+isin\theta \cdot x +i^{2}sin\theta\cdot y\]又因为复数的定义，i的平方为-1，带入有
\[(cos\theta \cdot x-sin\theta \cdot y)+i(cos\theta \cdot y+sin\theta \cdot x)\]分别对应x’、y’。也就是说，经过\(z_{\theta}\)的旋转，(x,y)被移到了\((cos\theta \cdot x-sin\theta \cdot y,cos\theta \cdot y+sin\theta \cdot x)\)。写成矩阵形式如下
\[\begin{pmatrix} x^{'}\\ y^{'} \end{pmatrix}=\begin{pmatrix} cos\alpha & -sin\alpha\\ sin\alpha & cos\alpha \end{pmatrix}\begin{pmatrix} x\\ y \end{pmatrix}\]这和我们之前推导的二维旋转公式是一模一样的。
为了更好地表达旋转矩阵R和复数的关系，也可以写成这样
\[R_{\theta}=cos\theta \begin{pmatrix} 1 & 0\\ 0 & 1 \end{pmatrix}+sin\theta \begin{pmatrix} 0 & -1\\ 1 & 0 \end{pmatrix}=cos\theta \cdot 1+sin\theta \cdot i\]于是每个复数都可以写成矩阵形式，a、b为实数
\[a\cdot 1+bi=\begin{pmatrix} a &-b\\ b & a \end{pmatrix}\](2)再看四元数定义 某个四元数q可以写成这样
\[q=a\cdot 1+bi+cj+dk\]其中，
\[1= \begin{pmatrix} 1 & 0\\ 0 & 1 \end{pmatrix},i= \begin{pmatrix} 0 & -1\\ 1 & 0 \end{pmatrix},j= \begin{pmatrix} 0 & -i\\ i & 0 \end{pmatrix},k= \begin{pmatrix} i & 0\\ 0 & -i \end{pmatrix}\]可以把四元数看成是复数的推广，在复数基础上又增加了j、k。将它们带入四元数定义式，可得到矩阵形式的四元数
\[a= \begin{pmatrix} a+id & -b-ic\\ b-ic & a-id \end{pmatrix}\]对于由四元数构成的四元数空间，记作H。
(3)四元数的性质
每一个非零四元数q都有其对应矩阵的逆矩阵\(q^{-1}\)，且有
\[q^{-1}=\frac{a\cdot 1 -bi-cj-dk}{a^{2}+b^{2}+c^{2}+d^{2}}\]每个四元数都有与其共轭的四元数\(\overline{q}\)
\[\overline{q}=a-bi-cj-dk\]且
\[q\cdot \overline{q}=\left |q \right |^{2},\overline{q}=q^{-1}\left |q \right |^{2}\]又有定义
\[\left |q \right |^{2}=det(q)=a^{2}+b^{2}+c^{2}+d^{2}\]即是(a,b,c,d)在四元数空间中到原点的距离。
单位长度的四元数满足\(a^{2}+b^{2}+c^{2}+d^{2}=1\)，对应三维球面S3。由于任意两个单位四元数的乘积仍为单位长度，所以S3对乘法是封闭的。
(4)等距同构
有某个单位长度的复数u。对于任意两个复数v、w，考虑它们的距离在乘u前后的变化。
\[\left |uv-uw \right |=\left |u(v-w) \right |=\left |u \right |\left |v-w \right |=\left |v-w \right |\]乘以u后，距离没有发生变化，乘u这个操作用术语就是“等距同构”。同理在R4(四元数空间)中，乘上任意一个单位长度的四元数也是等距同构。
(5)虚四元数的乘积
四元数对应四维空间，但我们研究的是三维空间旋转。因此将其限定在四元数的一个子空间：虚四元数。虚四元数也即实部为0的四元数。这就类似于在三维空间中，我们要研究一个平面上的旋转，就把研究范围限定在二维平面上一样。
\[p=bi+cj+dk\]虚四元数构成了三维空间，记作\(Ri+Rj+Rk\)。由需四元数的定义可以看到，其对于加法是封闭的，但对于乘法就不封闭了。除非是两个虚四元数彼此垂直，其结果才在\(Ri+Rj+Rk\)中。
(6)虚四元数的共轭
与复数相同，对于一个单位长度的四元数t，写成实部与虚部之和
\[t=cos\theta +usin\theta\]u为\(Ri+Rj+Rk\)中的单位向量。但之前发现，对于一个属于\(Ri+Rj+Rk\)的q来说，qt并不一定在其中。为了解决这个问题，就需要使用t的共轭。\(t^{-1}qt\)一定在\(Ri+Rj+Rk\)中。对于任意四元数r，都有
\[t^{-1}rt=r\]即t的共轭作用把R映射到其自身，也会把\(Ri+Rj+Rk\)映射到自身，t的共轭作用是\(Ri+Rj+Rk\)上的双射。
(7)共轭作用的效果
我们最终要证明的就是给定t=cosθ+usinθ，u为\(Ri+Rj+Rk\)中的单位向量。t的共轭作用将把\(Ri+Rj+Rk\)绕着u所在的轴旋转2θ。
首先证明u是旋转轴。也即u的所有实倍数向量在t的共轭作用下保持不变。
\[\begin{align*} t^{-1}ut &= (cos\theta -usin\theta)u(cos\theta +usin\theta) \\ &= (ucos\theta -u^{2}sin\theta)(cos\theta +usin\theta)\\ &= (ucos\theta +sin\theta)(cos\theta +usin\theta)(because \quad u^{2}=-1)\\ &= ucos^{2}\theta+ u^{2}cos\theta sin\theta +sin\theta cos\theta +usin^{2}\theta \\ &= u(cos^{2}\theta+sin^{2}\theta)+ +sin\theta cos\theta - cos\theta sin\theta\\ &=u \end{align*}\]在\(Ri+Rj+Rk\)中，选取垂直于u的向量v，w=u×v。自然，{u,v,w}就构成了\(Ri+Rj+Rk\)中的一组规范正交基底。为了证明确实旋转了2θ，只需证明下式
\[\begin{matrix} t^{-1}vt=vcos2\theta -w sin2\theta\\ t^{-1}wt=vsin2\theta +wcos2\theta \end{matrix}\]这里只证明第一个式子。
\[\begin{align*} t^{-1}vt &= (cos\theta -usin\theta)v(cos\theta +usin\theta) \\ &= (vcos\theta -uvsin\theta)(cos\theta +usin\theta)\\ &= vcos^{2}\theta+ vucos\theta sin\theta -uvsin\theta cos\theta -uvusin^{2}\theta \\ &= vcos^{2}\theta - 2uvsin\theta cos\theta +u^{2}vsin^{2}\theta (because\quad vu=-uv)\\ &=v(cos^{2}\theta - sin^{2}\theta)-2uvsin\theta cos\theta\\ &=v(cos^{2}\theta - sin^{2}\theta)-wsin\theta cos\theta(because \quad u^{2}=-1,uv=w)\\ &=vcos2\theta-wsin2\theta \end{align*}\]从而得证。对于R3中的每个旋转，给定转轴u及转角α，都有一个单位长度的四元数t=cos(α/2)+usin(α/2)的共轭作用与之对应。
本文作者原创,未经许可不得转载,谢谢配合
