MATLAB李群,VIO学习之一——视觉SLAM中李群李代数与四元数总结

在SLAM中经常会用到李群李代数与四元数来表示旋转变换，这些数学公式往往需要推导来推导去，分分钟搞到头都大了。但在SLAM中往往用到其中那么几个固定的性质，所以是没有必要对这些数学基础作过多深入的研究，只需要记住其中一些常用的公式及性质即可。因此，本人在这里对这些数学基础作一个简单的总结，以便日后在工程中使用。

旋转的表示方式

SLAM中，往往会使用三种方式来表达空间中的旋转变换，分别为：旋转向量、旋转矩阵、四元数。

旋转向量

旋转向量可以非常直观地表示空间中的一个旋转变换：假设空间中有一单位向量$\mathbf{n}$，以$\mathbf{n}$为转轴且旋转方向与$\mathbf{n}$相同，旋转的角度为$\theta$。可以将上述旋转变换记为“角轴”的组合形式：$$，也可以组合成变通向量的形式：$\mathbf{\phi}=\theta \mathbf{n}$.

旋转矩阵

旋转矩阵$\mathbf{R}{AB}$可以表示一个物体由姿态$A$旋转变换到姿态$B$的过程，若将$A$与$B$视为一个坐标系，则$\mathbf{R}{BA}$ 可以表示坐标系$A$在坐标系$B$中的姿态。设$W$为世界坐标系，物体由姿态$\mathbf{R}{WA}$转换到姿态$\mathbf{R}{WB}$可以用$\mathbf{R}{AB}$来表示，即：$\mathbf{R}{WB} = \mathbf{R}{WA} \mathbf{R}{AB}$. 如果坐标关系非常明确，则可以直接将下标去掉，直接记为$\mathbf{R}$. 其中$\mathbf{R}$为单位正交阵，其具有如下性质：

$\mathbf{R}$的行(列)均为单位向量且两两正交；

$\mathrm{det}(\mathbf{R})=1$；

$\mathbf{R}^{-1} = \mathbf{R}^\mathrm{T}$

四元数

四元数可以定义为: $$ \mathbf{q} = w+xi+yj+zk $$

其中$i,j,k$ 分别为四元数的三个虚部，这三个虚部满足如下性质： $$ \left{\begin{matrix} i^2 + j^2 + k^2 = 1\ ij=k, ji=-k \ jk=i, kj=-i \ ki=j, ik=-j \end{matrix}\right. $$

四元数还可以表示为 $$ \mathbf{q} = [w,x,y,z]^\mathrm{T} $$

或

$$ \mathbf{q} = [s,\mathbf{v}], \mathbf{v} = [x,y,z]^\mathrm{T} \in \mathbb{R}^3 $$

我们可以使用单位四元数来表示空间任意的旋转变换，其中符合条件$w^2+x^2+y^2+z^2=1$的四元数为单位四元数。

三种方式之间转换

上述三种数学工具都可以表示空间中的旋转，他们其实是等价的，可以进行相互转换。

旋转向量 $\leftrightarrow$ 旋转矩阵

设旋转向量为$\mathbf{\phi}=\theta \mathbf{n}$，与之等价的旋转矩阵为$\mathbf{R}$，两者之间的转换可以使用罗德里格斯公式来实现，即 $$ \mathbf{R} = \cos \theta \mathbf{I} + (1 - \cos \t