视觉SLAM十四讲学习2 旋转矩阵，轴角，欧拉角，四元数

前言

视觉SLAM十四讲这本书我应该已经看过四五遍了。前两遍看起来比较吃力，因为SLAM涉及到了比较广泛的数学理论，包括线性代数、概率论、随机过程、优化理论等。

因此我先将SLAM相关的一些数学基础在数值计算专栏里总结下来。据说高博曾经说过：数学学不好，Demo跑到老~笑:）。数学能力过关后，再回到十四讲这本书，看起来就有种说不出的爽感，越读越有劲。

本篇将开始进行SLAM十四讲的学习和总结。

坐标，基与旋转矩阵

在一个标准正交基下的向量可以通过坐标表示为：
$$\alpha =[{\vec{e}}_1,{\vec{e}}_2,{\vec{e}}_3][a_1,a_2,a_3{]}^T$$
同一向量在不同基下的坐标可表示为：
$$\begin{gathered} [{\vec{e}}_1,{\vec{e}}_2,{\vec{e}}_3][a_1,a_2,a_3{]}^T=[{\vec{e}}_1^{\prime },{\vec{e}}_2^{\prime },{\vec{e}}_3^{\prime }][a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T \\ [a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T=[{\vec{e}}_1^{\prime },{\vec{e}}_2^{\prime },{\vec{e}}_3^{\prime }{]}^T[{\vec{e}}_1,{\vec{e}}_2,{\vec{e}}_3][a_1,a_2,a_3{]}^T \\ {a}^{\prime }=Ra \end{gathered}$$
上式中的$R$被称为从坐标系${\perp }_a\to {\perp }_a^{\prime }$的旋转矩阵。

从线性代数上而言，$a^{\prime }=Ra$表示坐标$a$通过$R$线性变换为坐标$a^{\prime }$。两个标准正交基的乘积还是标准正交基，因此旋转矩阵$R$是正交矩阵。并且旋转矩阵的行列式必为1，$R^T$表示一个旋转$R$的反旋转。

通常把机器人自身作为可变的移动坐标系，外界环境中存在一个不变的世界坐标系。

旋转矩阵表示的是不同坐标系之间，相应坐标轴的旋转关系。

内积和外积

内积：
$$a\cdot b=a^{T}b=|a||b|cos{\theta }_{ab}$$

外积：
$$a\times 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}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b=a^{\wedge }b$$
在这里使用$a^{\wedge }$表示向量与矩阵的一种映射关系，简化运算。

变换矩阵

两个坐标系之间的旋转和位移可表示为：
$$a^{\prime }=Ra+t$$
称为欧式变换。可以使用齐次坐标简化欧式变换的表现形式：
$$\begin{gathered} \left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right] \\ 简记为：a^{\prime }=Ta \end{gathered}$$
将$T$称为变换矩阵，包含一个旋转矩阵和一个位移向量。可以使用$T^{-1}$表示一个反变换：
$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$

旋转矩阵与变换矩阵的自由度

三维空间中的旋转具有3个自由度，而旋转矩阵有9个元素；欧式变换具有6个自由度，而变换矩阵有16个元素。此外，旋转矩阵的行列式为1。

使用旋转矩阵和变换矩阵不仅计算冗余，还受到行列式约束。而计算冗余和约束都是由旋转矩阵带来的。

旋转向量与旋转角

三维空间中的旋转也可以表示为，向量绕着某一个单位向量旋转一个具体角度：
$$\varphi =\theta n$$
即使用轴角angle-axis，其中$\theta$表示旋转角度，$n$表示单位旋转向量。

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

旋转矩阵到旋转向量：
$$\begin{gathered} tr(R)=3\cos \theta +(1-\cos \theta )tr(n{n}^T) \\ =3\cos \theta +(1-\cos \theta )||n|{|}_2^2 \\ =3\cos \theta +(1-\cos \theta )=1+2\cos \theta \\ \theta =\arccos \frac{tr(R)-1}{2} \\ Rn=n,(R-I)n=0,||n|{|}_2=1 \end{gathered}$$
即通过旋转矩阵的迹求旋转角度，通过旋转矩阵的特征值求单位旋转向量。

轴角的表示也有奇异性，比如转30度与转390度的轴角表示是一样的。

旋转矩阵与轴角对于三维空间的旋转而言，都不够直观。

欧拉角

有一种简单粗暴的旋转表示法，直接把旋转表示为围绕三个坐标轴的三个旋转，即欧拉角：

$$({\theta }_x,{\theta }_y,{\theta }_z)$$
无人机系统中常用的欧拉角是yaw-pitch-roll，可记为偏航-俯仰-滚转角，如下图所示。

欧拉角对于旋转而言非常直观，但是有著名的万向锁问题，导致丢失一个自由度，在计算中容易出现奇异问题。

四元数

类似于复数，四元数由一个实部与三个虚部构成：
$$\begin{gathered} q=q_0+q_{1}i+q_{2}j+q_{3}k \\ \{\begin{array}{l}ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\\ i^2=-1,j^2=-1,k^2=-1\end{array} \end{gathered}$$
可以使用向量形式来简化四元数的表示：
$$q=(q_0,q_1,q_2,q_3)=(s,\vec{v})$$

四元数的基本运算：
$$\begin{gathered} q_a\pm q_b=(s_a\pm s_b{\vec{v}}_a\pm {\vec{v}}_b) \\ {q}_a{q}_b=(s_a{s}_b-{\vec{v}}_a\cdot {\vec{v}}_b,s_a{\vec{v}}_b+s_b{\vec{v}}_a+{\vec{v}}_a\times {\vec{v}}_b) \\ {q}^{\ast }=(s,-\vec{v}),q{q}^{\ast }=(s^2+\vec{v}\cdot \vec{v},\vec{0}) \\ ||q||=||(s,\vec{v})|{|}_2,||q_a{q}_b||=||q_a|||q_b|| \\ {q}^{-1}=\frac{q^{\ast }}{||q|{|}^2},q{q}^{-1}=1,(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1} \end{gathered}$$

单位四元数表示旋转

单位四元数可以表示旋转。单位四元数实际上具有三个自由度，对应于三维空间中的旋转。

三维空间中，绕单位向量$n$逆时针旋转$\theta$，对应于单位四元数：
$$q=(\cos \frac{\theta }{2},\vec{n}\sin \frac{\theta }{2})$$
对于向量$p=(a,b,c)$而言，其旋转可表示为：
$$\begin{gathered} q_p=(0,\vec{p}) \\ {q}_p^{\prime }=q{q}_p{q}^{\ast }=(0,{\vec{p}}^{\prime }) \end{gathered}$$
在这里证明一下单位四元数旋转计算得到的四元数的实部为0：
$$\begin{gathered} q{q}_p{q}^{\ast }=(\cos \frac{\theta }{2},\vec{n}\sin \frac{\theta }{2})(0,\vec{p})(\cos \frac{\theta }{2},-\vec{n}\sin \frac{\theta }{2}) \\ =(0-(\vec{n}\sin \frac{\theta }{2})\cdot \vec{p},\vec{p}\cos \frac{\theta }{2}+0+\vec{n}\times \vec{p}\sin \frac{\theta }{2})(\cos \frac{\theta }{2},-\vec{n}\sin \frac{\theta }{2}) \\ =(-\vec{n}\cdot \vec{p}\sin \frac{\theta }{2},\vec{p}\cos \frac{\theta }{2}+\vec{n}\times \vec{p}\sin \frac{\theta }{2})(\cos \frac{\theta }{2},-\vec{n}\sin \frac{\theta }{2}) \\ =(-\vec{n}\cdot \vec{p}\sin \frac{\theta }{2}\cos \frac{\theta }{2}-(\vec{p}\cos \frac{\theta }{2}+\vec{n}\times \vec{p}\sin \frac{\theta }{2})\cdot (-\vec{n}\sin \frac{\theta }{2}),\dots \dots ) \\ =(-\vec{n}\cdot \vec{p}\sin \frac{\theta }{2}\cos \frac{\theta }{2}+\vec{p}\cdot \vec{n}\cos \frac{\theta }{2}\sin \frac{\theta }{2}+\vec{n}\times \vec{p}\cdot \vec{n}\sin \frac{\theta }{2}\sin \frac{\theta }{2},\dots \dots ) \\ =(\vec{n}\times \vec{p}\cdot \vec{n}\sin \frac{\theta }{2}\sin \frac{\theta }{2},\dots \dots ) \\ =(0,{\vec{p}}^{\prime }) \end{gathered}$$
最后一步中，$\vec{n}\times \vec{p}\cdot \vec{n}$表示$\vec{n},\vec{p}$的外积与$\vec{n}$做内积。外积结果垂直于$\vec{n},\vec{p}$，因此再与$\vec{n}$做内积时结果为0。

后记

本篇记录了十四讲中最基础的刚体旋转和变换部分，下一篇将记录相机的成像模型。