视觉SLAM中李群李代数与四元数总结

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

旋转的表示方式

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

旋转向量

旋转向量可以非常直观地表示空间中的一个旋转变换：假设空间中有一单位向量$\mathbf{n}$，以$\mathbf{n}$为转轴且旋转方向与$\mathbf{n}$相同，旋转的角度为$\theta$。可以将上述旋转变换记为“角轴”的组合形式：$<\theta ,\mathbf{n}>$，也可以组合成变通向量的形式：$\varphi =\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}$为单位正交阵，其具有如下性质：

1. $\mathbf{R}$的行（列）均为单位向量且两两正交；
2. $\mathrm{d}\mathrm{e}\mathrm{t}(\mathbf{R})=1$；
3. ${\mathbf{R}}^{-1}={\mathbf{R}}^{\mathrm{T}}$

四元数

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

其中$i,j,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}=[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$ 旋转矩阵

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

其中${\mathbf{n}}^{\wedge }$为由$\mathbf{n}$转化而成的反对称矩阵，即
$${\mathbf{n}}^{\wedge }=\left[\begin{array}{ccc}0& -n_3& n_2\\ n_3& 0& -n_1\\ -n_2& n_1& 0\end{array}\right]$$
$\mathbf{n}$的反对称矩阵也可以用$[\mathbf{n}{]}_{\times }$来表示。
反过来，由$\mathbf{R}$转化为$\mathbf{n}$的方法为：
$$\{\begin{array}{c}\theta =\arccos (\frac{\mathrm{t}\mathrm{r}(\mathbf{R})-1}{2})\\ \mathbf{R}\mathbf{n}=\mathbf{n}\end{array}$$
其中$\mathbf{R}\mathbf{n}=\mathbf{n}$表示矩阵$\mathbf{R}$的为1的特征值对应的单位特征向量。

旋转向量 $\leftrightarrow$ 四元数

设旋转向量为$\varphi =\theta \mathbf{n}$，与之等价的四元数为$\mathbf{q}$，则有：
$$\mathbf{q}=\cos \frac{\theta }{2}+\mathbf{n}\sin \frac{\theta }{2}=[\cos \frac{\theta }{2},\mathbf{n}\sin \frac{\theta }{2}{]}^{\mathrm{T}}$$
这种转换与虚数中的欧拉公式十分相似，即${\mathrm{e}}^{j\theta }=\cos \theta +j\sin \theta$，此处可以进行类比记忆。

旋转矩阵 $\leftrightarrow$ 四元数

设四元数为$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$，与之等价的旋转矩阵为$\mathbf{R}$，他们之间的变换有：
$$\mathbf{R}=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$

$$\begin{array}{cccc}{q}_0=\frac{\sqrt{\mathrm{t}\mathrm{r}(\mathbf{R})+1}}{2}& q_1=\frac{m_{23}-m_{32}}{4q_0}& q_2=\frac{m_{31}-m_{13}}{4q_0}& q_3=\frac{m_{12}-m_{21}}{4q_0}\end{array}$$

李群李代数的性质

上面讲到的旋转矩阵$\mathbf{R}$可以构成一个李群SO(3)，即特殊正交群，而旋转向量$\varphi =\theta \mathbf{a}$可以构成一个李代数$\mathfrak{s}\mathfrak{o}(3)$。由于引入发李代数，所以对旋转矩阵的求导就变得非常方便。在SLAM中对旋转矩阵求导或对物体的姿态作优化时会大量用到李代数与李群之间的转换。

指数与对数映射

李群与李代数之间的转换可以使用指数与对数来实现.
指数映射：
$$\mathbf{R}=\exp ({\varphi }^{\wedge })=\exp (\theta {\mathbf{a}}^{\wedge })=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }$$

上面这条公式是不是貌似十分相识？？？没错，它就是上面曾经曰过的罗德里格斯公式。

对数映射：
$$\varphi =\ln (\mathbf{R}{)}^{\vee }={\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(\mathbf{R}-\mathbf{I}{)}^{n+1}\right)}^{\vee }$$
这条公式看起来有点复杂，不过没有关系，反正也用得不太多，因为我们通常不会用上式来计算对数映射，而是用旋转矩阵的迹来计算，请见旋转向量 $\leftrightarrow$ 旋转矩阵一节中有介绍。

BCH公式

指数函数$\exp (x)$(其中x是标量)具有$\exp (x_1)\exp (x_2)=\exp (x_1+x_2)$的性质，但对于李代数的指数映射是否具有如$\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_1^{\wedge })=\exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })$的性质呢？很不幸，这个性质是不存在的，但可以使用BCH来近似。
BCH公式如下：
$$\ln (\exp ({\varphi }_1^{\wedge }){\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{cc}{\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& {\varphi }_1\in \sigma \\ {\varphi }_1+{\mathbf{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2& {\varphi }_2\in \sigma \end{array}$$

其中$\sigma$表示一个微波量的集合。上式中，第一个式子为左乘近似，第二个式子为右乘近似。现在此对其进行解释如下。
对于第一个式子，假设旋转矩阵为${\mathbf{R}}_2$（其对应的李代数为${\varphi }_2$），对${\mathbf{R}}_2$左乘一个微小的旋转矩阵${\mathbf{R}}_1$（其对应的李代数为${\varphi }_1$），则${\mathbf{R}}_1{\mathbf{R}}_2$所对应的李代数近似为${\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2$，其中${\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1$为左乘了微小旋转矩阵${\mathbf{R}}_1$所导致的李代数增加的部分。

对于第二个式子，假设旋转矩阵为${\mathbf{R}}_1$（其对应的李代数为${\varphi }_1$），对${\mathbf{R}}_1$右乘一个微小的旋转矩阵${\mathbf{R}}_2$（其对应的李代数为${\varphi }_2$），则${\mathbf{R}}_1{\mathbf{R}}_2$所对应的李代数近似为${\mathbf{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1$，其中${\mathbf{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2$为左乘了微小旋转矩阵${\mathbf{R}}_2$所导致的李代数增加的部分。

BCH公式还可以写成如下的形式，其中$\Delta \varphi$为增加的微小旋转：
$$\begin{array}{c}\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp (({\mathbf{J}}_l^{-1}(\varphi )\Delta \varphi +\varphi {)}^{\wedge })\\ \exp ({\varphi }^{\wedge })\exp (\Delta {\varphi }^{\wedge })=\exp ((\varphi +{\mathbf{J}}_r^{-1}(\varphi )\Delta \varphi {)}^{\wedge })\\ \exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp (({\mathbf{J}}_l\Delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp (({\mathbf{J}}_r\Delta \varphi {)}^{\wedge })\end{array}$$

李代数求导

假设我们对一个空间点$\mathbf{p}$进行了旋转，得到了$\mathbf{R}\mathbf{p}$，现在我们要计算旋转之后的点关于旋转矩阵的导数，即：
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial {\mathbf{R}}^{\mathrm{T}}}$$

由于$\mathbf{R}\in \mathrm{S}\mathrm{O}(3)$是不在在加法的，所以为了方便求导，我们可以使用李代数来进行求导，即将上式转化为正式：
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial {\mathbf{R}}^{\mathrm{T}}}\iff \frac{\partial (\exp ({\varphi }^{\wedge })\mathbf{p})}{\partial \varphi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l$$

上式的结果是用左乘BCH来近似得到的，由于${\mathbf{J}}_l$的存在，增加计算的复杂度，所以可以使用扰动模型（左乘）来进行简化。左扰动模型的结果为：
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial {\mathbf{R}}^{\mathrm{T}}}\iff \frac{\partial (\mathbf{R}\mathbf{p})}{\partial \varphi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }$$

上述公式只需直接记忆并使用即可，如果想知道如何推导的，可见参考文献[1]中的p74~p75.

四元数的性质

这里主要是对四无数的运算及若干性质作一简单的总结。具体的四元数的教程请阅读参考文献[2]。

四无数加法

$$\mathbf{p}+\mathbf{q}=[p_w,{\mathbf{p}}_v{]}^{\mathrm{T}}+[q_w,{\mathbf{q}}_v{]}^{\mathrm{T}}=[p_w+q_w,{\mathbf{p}}_v+{\mathbf{q}}_v{]}^{\mathrm{T}}$$

四无数乘法

四元数的加法用符号$\otimes$表示。其运算法则如下：

$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-{\mathbf{p}}_v^{\mathrm{T}}{\mathbf{q}}_v\\ p_w{\mathbf{q}}_v+q_w{\mathbf{p}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]$$

四元数的乘法不满足交换律，即：
$$\mathbf{p}\otimes \mathbf{q}\ne \mathbf{q}\otimes \mathbf{p}$$

但四元数乘法仍然满足结合律，即：
$$(\mathbf{p}\otimes \mathbf{q})\otimes \mathbf{r}=\mathbf{p}\otimes (\mathbf{q}\otimes \mathbf{r})$$

故四元数加法也满足分配律，即：
$$\mathbf{p}\otimes (\mathbf{q}+\mathbf{r})=\mathbf{p}\otimes \mathbf{q}+\mathbf{p}\otimes \mathbf{r}$$
或者是：
$$(\mathbf{p}+\mathbf{q})\otimes \mathbf{r}=\mathbf{p}\otimes \mathbf{r}+\mathbf{q}\otimes \mathbf{r}$$

两个四元数的乘法还可以用矩阵的形式的表示，即：
$${\mathbf{q}}_1\otimes {\mathbf{q}}_2=[\mathbf{q}{]}_L{\mathbf{q}}_2=[\mathbf{q}{]}_R{\mathbf{q}}_1$$

而其中的$[\mathbf{q}{]}_L$ 与 $[\mathbf{q}{]}_R$分别如下：
$$[\mathbf{q}{]}_L=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_x\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right]=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\mathbf{T}}\\ {\mathbf{q}}_v& [{\mathbf{q}}_v{]}_{\times }\end{array}\right]$$

$$[\mathbf{q}{]}_R=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_x\\ q_x& q_w& q_z& -q_y\\ q_y& -q_z& q_w& q_x\\ q_z& q_y& -q_x& q_w\end{array}\right]=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\mathbf{T}}\\ {\mathbf{q}}_v& -[{\mathbf{q}}_v{]}_{\times }\end{array}\right]$$

上面这两条公式在证明或推导公式过程中十分有用，可以重点关注。

参考文献

[1] 《视觉SLAM十四讲——从理论到实践》高翔， 张涛
[2] 《Quaternion kinematics for the error-state Kalman filter》 Joan Sola.