视觉SLAM中的数学基础 第四篇 李群与李代数（2）
前言
理解李群与李代数，是理解许多SLAM中关键问题的基础。本讲我们继续介绍李群李代数的相关知识，重点放在李群李代数的微积分上，这对解决姿态估计问题具有重要意义。
回顾
为了描述三维空间里的运动，我们使用3$\times $3的旋转矩阵$\mathbf{R}$来描述一个刚体的旋转，并且，用4$\times$4的变换矩阵来描述六自由度的旋转+平移。这两种矩阵在传统的欧氏空间$\mathbb{R}^{3 \times 3}$和$\mathbb{R}^{4 \times 4}$中，不存在加法运算，只有乘法运算，故无法构成线性空间，只能构成群。也就是我们说的李群$SO(3)$和$SE(3)$：
\[\begin{equation}
SO(3) = \{ \mathbf{R} \in \mathbb{R}^{3 \times 3} | \mathbf{R R}^T = \mathbf{I}, det(\mathbf{R})=1 \}
\end{equation}\]
\[\begin{equation}
SE(3) = \left\{ \mathbf{T} = \left[ {\begin{array}{*{20}{c}}
\mathbf{R} & \mathbf{t} \\
{{\mathbf{0}^T}} & 1
\end{array}} \right]
\in \mathbb{R}^{4 \times 4} | \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3\right\}
\end{equation}\]
在李群中，我们使用矩阵来表达一个旋转和平移，这存在冗余的自由度。三维空间的旋转只有三自由度，旋转+平移有六自由度。因此，我们希望寻找一个没有冗余自由度（但是相应的存在奇异性）的表示，也就是李代数$\mathfrak{so}(3)$和$\mathfrak{se}(3)$：
\[\begin{equation}
\mathfrak{so}(3) = \{ \mathbf{\phi} \in \mathbb{R}^{3} \}
\end{equation}\]
\[\begin{equation}
\mathfrak{se}(3) = \{ \mathbf{\xi } \in \mathbb{R}^{6} \}
\end{equation}\]
怎么将李代数和李群元素对应起来呢？我们说，从李代数到李群，通过指数映射；反之，从李群到李代数，则通过对数映射。
$\mathfrak{so}(3)$的一个元素$\mathbf{\phi}=\mathbf{a} \theta $的指数映射为：
\[\begin{equation} exp\left( {{\phi ^ \wedge }} \right) = \cos \theta {\bf{I}} + (1 - \cos \theta ){\bf{a}}{{\bf{a}}^T} + \sin \theta {{\bf{a}}^ \wedge } \end{equation}\]
其中$\wedge$算符把一个三维向量变换为它对应的反对称矩阵（3$\times $3）。
该式和Rodrigues（罗德里格斯）公式是一致的。说明李代数$\mathfrak{so}(3)$里的向量，其方向指向旋转轴，而模长表示转过的角度。因此，平时用的rpy欧拉角，实际上是李代数之一。另一点需要说明的是，指数映射是一个满射而非单射。由于旋转具有周期性，多转360度和不转，表示的是同一个旋转。如果我们把旋转限制在正负180度内，那么李群到李代数就是一一对应的。
反之，对数映射$\ln {\left( \mathbf{R} \right)^ \vee }$将一个旋转矩阵转换为一个李代数。但由于对数的泰勒展开不是很优雅，通常直接按照下式计算李代数向量的大小和方向：
\[ \begin{equation} \begin{array}{l}
\theta = {\cos ^{ - 1}}\frac{{tr\left( \mathbf{R} \right) - 1}}{2} + 2k\pi \\
\mathbf{Ra = a}
\end{array} \end{equation} \]
根据该结果得到的$\mathbf{\phi} = \theta \mathbf{a}$就是对应的李代数了。到这为止的内容都在上一讲中介绍过，本讲我们就接着往下说。
$SE(3)$中的指数映射和对数映射
$\mathfrak{se}(3)$里的向量经常写成$\mathbf{\xi}$。它是一个六维向量，通常前三维为平移，后三维为旋转（也就是$\mathfrak{so}(3)$里的元素）。请注意，有些地方的定义是倒过来的，即前三维为旋转，后三维为平移，在使用的时候一定要小心！例如g2o的李代数和Sohpus的李代数定义就不一样。
记$\mathbf{\xi}=[\mathbf{\rho}, \mathbf{\phi}]^T$，它们各为三维向量，一共组成了六维，构成一个李代数。我们的记法里$\rho$为平移，$\phi$为旋转，与$SO(3)$中一致。
与$SO(3)$类似，$\mathfrak{se}(3)$到$SE(3)$的指数映射为：
\[ \begin{equation} \begin{array}{lll}
\exp \left( {{\xi ^ \wedge }} \right) &=& \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{\xi ^ \wedge }} \right)}^n}} \\
&=& \left[ {\begin{array}{*{20}{c}}
{\exp \left( {{\phi ^ \wedge }} \right)}&{ \mathbf{J\rho} }\\
{{0^T}}&1
\end{array}} \right]
\end{array} \end{equation} \]
请注意该式左上即 $SO(3)$ 的指数映射，右上较特殊一些，是平移量 $\mathbf{\rho}$ 的线性表示。其系数$\mathbf{J}_{3\times 3}$的计算方式如下：
\[\begin{array}{lll}
\mathbf{J} &=& \sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{ \mathbf{\phi} ^ \wedge }} \right)}^n}} \\
&=& \frac{{\sin \theta }}{\theta }I + \left( {1 - \frac{{\sin \theta }}{\theta }} \right) \mathbf{a}{\mathbf{a}^T} + \frac{{1 - \cos \theta }}{\theta }{\mathbf{a}^ \wedge }
\end{array}\]
它与Rodrigues相似但有差别，实际上是对$\phi$求了一次导数，所以这个阵也就记成了$\mathbf{J}$，意为一个雅可比阵。它在后续许多地方会用到，所以现在单独提一下它。
这个矩阵是可逆的，它的逆形式如下：
\[\begin{equation} {\mathbf{J}^{ - 1}} = \frac{\theta }{2}\cot \frac{\theta }{2}I + \left( {1 - \frac{\theta }{2}\cot \frac{\theta }{2}} \right)a{a^T} - \frac{\theta }{2}{a^ \wedge } \end{equation}\]
嗯，现在我们说清了$SE(3)$上的指数映射。那么它的对数映射怎么计算呢？也就是一个$SE(3)$中的矩阵怎么找到对应的向量呢？我们并不需要实际地计算一个矩阵对数，而只需要计算：
对于
\[\mathbf{T} = \left[ {\begin{array}{*{20}{c}}
\mathbf{R} & \mathbf{t}\\
{{0^T}}&1
\end{array}} \right]\]
它对应的李代数 $\mathbf{\xi}=[\mathbf{\rho}, \mathbf{\phi}]^T$ 为：
\[ \begin{equation} \phi = \ln {\left( R \right)^ \vee },\rho = {J^{ - 1}}t \end{equation} \]
因为已经有了$SO(3)$上的计算方式，这里的式子就变得十分简单了。
Baker-Campbell-Hausdorff公式
小萝卜：师兄我还有个问题啊。
师兄：嗯你说。
小萝卜：李群和李代数的对应关系倒是不难啦。但实际用的时候，我经常在李群上做两个矩阵的乘法。例如，做$\mathbf{R_1} \cdot \mathbf{R_2}$时，它们对应的李代数发生了什么呢？是不是直接加起来啊？
对，这就是我们要讨论的问题，即：
\[ \begin{equation} \exp \left( {\phi _1^ \wedge } \right)\exp \left( {\phi _2^ \wedge } \right) = \exp \left( {{{\left( {{\phi _1} + {\phi _2}} \right)}^ \wedge }} \right) \end{equation} \]
成立吗？
如果该式成立的话，李群和李代数的结构就很方便了。因为我们可以直接把李群的乘法变成李代数的加法，而不需要改变任何东西。直观上，两个指数函数相乘，也确实等于把幂加起来，再取指数。唯一不同的是，这里我们不是对一个数求指数，而是求一个反对称矩阵的指数。
但是成不成立呢？
很遗憾，虽然我们非常希望该式成立，但是实质上它是不满足的。Baker-Campbell-Hausdorff公式（https://en.wikipedia.org/wiki/Baker%E2%80%93Campbell%E2%80%93Hausdorff_formula）给出了这个运算的答案：
\[\ln \left( {\exp \left( A \right)\exp \left( B \right)} \right) = \sum\limits_{n = 1}^\infty {\frac{{{{\left( { - 1} \right)}^{n - 1}}}}{n}\sum\limits_{{r_i} + {s_i} > 0,i \in \left[ {1,n} \right]}^{} {\frac{{{{\left( {\sum\limits_{i = 1}^n {\left( {{r_i} + {s_i}} \right)} } \right)}^{ - 1}}}}{{\prod\limits_{i = 1}^n {{r_i}!{s_i}!} }}\left[ {{A^{{r_1}}}{B^{{s_1}}}{A^{{r_2}}}{B^{{s_2}}} \cdots {A^{{r_n}}}{B^{{s_n}}}} \right]} } \]
那么这式子在说什么鬼呢？
为了避免过于冗长的解释，我们就直接看它的近似式好了：
\[\ln \left( {\exp \left( A \right)\exp \left( B \right)} \right) = A + B + \frac{1}{2}\left[ {A,B} \right] + \frac{1}{{12}}\left[ {A,\left[ {A,B} \right]} \right] - \frac{1}{{12}}\left[ {B,\left[ {A,B} \right]} \right] + \cdots \]
这里的方括号指的是李括号，也就是：
\[\left[ {A,B} \right] = AB - BA\]
特别地，在$SO(3)$上，我们可以求它关于某一个变量的一阶近似：
\[ \begin{equation} \ln {\left( {{R_1}{R_2}} \right)^ \vee } \approx \left\{ \begin{array}{l}
{J_l}{\left( {{\phi _2}} \right)^{ - 1}}{\phi _1} + {\phi _2}\\
{J_r}{\left( {{\phi _1}} \right)^{ - 1}}{\phi _2} + {\phi _1}
\end{array} \right. \end{equation} \]
对我们用处最大的就是这个近似式。在 $\phi_1$ 较小时，使用第一个式；在在 $\phi_2$ 较小时，使用第二个式。这里的 $J_l$ 和 $J_r$ 也称为左/右雅可比——从而李代数就分成了左右两种模型（因此，李代数程序库会声明它使用的是左乘模型还是右乘模型）。
那么这俩雅可比是啥类？
事实上，前面已经提到过它了：
\[ \begin{equation} {J_l} = J = \sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{\phi ^ \wedge }} \right)}^n}} ,{J_r} = J\left( { - \phi } \right) \end{equation}\]
在这个近似下，我们只保留了和小量相关的一阶信息，而舍弃了它的高阶项。因此，可以把这个过程类比于保留泰勒展开的一阶项而舍弃高阶项的过程，其中泰勒展开类比于BCH公式。
BCH近似的直观意义
以左乘形式（$\phi_1$较小）为例。我们给一个旋转矩阵 $\mathbf{R}$ 左乘一个微小扰动 $\Delta R = \exp \left( {\Delta {\phi ^ \wedge }} \right)$，得到了$\Delta R \cdot R$。那么，对应到李代数上的变化量为：
\[ \begin{equation} \ln {\left( {\Delta R \cdot R} \right)^ \vee } = \phi + J_l^{ - 1}\left( \phi \right)\Delta \phi \end{equation} \]
反过来说，如果对某个固定的李代数$\phi$增加一个小量$\delta \phi$，对应到李代数上，为：
\[ \begin{equation} \exp {\left( {\phi + \delta \phi } \right)^ \wedge } = \exp {\left( {{J_l}\delta \phi } \right)^ \wedge } \cdot \exp {\left( \phi \right)^ \wedge } \end{equation} \]
相当于左乘一个 $J_l \phi$ 对应的旋转阵。类似地，由于左右对称，有：
\[ \begin{equation} \exp {\left( {\phi + \delta \phi } \right)^ \wedge } = \exp {\left( {{J_l} \delta \phi } \right)^ \wedge } \cdot \exp {\left( \phi \right)^ \wedge } = \exp {\left( \phi \right)^ \wedge }\exp {\left( {{J_r}\delta \phi } \right)^ \wedge } \end{equation} \]
从这两个式子可以清楚地看到，BCH近似为我们指明了，李群与李代数微小量之间的关系——即，在BCH线性近似的意义下，它们只差一个雅可比矩阵。当区别左右乘时，这两个雅可比自变量差一个符号。
应用：微分模型和扰动模型
实际中，我们经常考虑这样一个问题（我们先讨论$SO(3)$上，再讨论$SE(3)$上）：
设一个空间点$P=[x,y,z]$，它经过一次旋转，变成$RP$。问$RP$随着$R$是如何变化的？
由于$SO(3)$上面没有加法，我们无法使用传统导数的定义（因为 $R+\Delta R$ 不再是旋转矩阵）：
\[\frac{{d\left( {RP} \right)}}{{dR}} = \mathop {\lim }\limits_{\Delta R \to 0} \frac{{\left( {R + \Delta R} \right)P - RP}}{{\Delta R}}\]
为了解决这个问题，有两种方式：一是把增量定义在李代数上，对应于微分模型；二是把增量直接乘在$R$上，对应于扰动模型。如果读者熟悉了前面的公式，那这件事情是很轻松的。
-
定义在李代数上的增量
在这个意义下，导数的含义为（最后一步跳过了若干过程，实际需要分开对各分量计算导数）：
\[\begin{array}{lll}
\frac{{d\left( {RP} \right)}}{{d\phi }} &=& \mathop {\lim }\limits_{\delta \phi \to 0} \frac{{\exp {{\left( {\phi + \delta \phi } \right)}^ \wedge }P - \exp {{\left( \phi \right)}^ \wedge }P}}{{\delta \phi }}\\
&=& \mathop {\lim }\limits_{\Delta \phi \to 0} \frac{{(\exp {{\left( {{J_l}\delta \phi } \right)}^ \wedge } - 1)exp{{\left( \phi \right)}^ \wedge }P}}{{\delta \phi }}\\
&=& - {(RP)^ \wedge }{J_l}
\end{array}\]
2. 定义在李群上的乘增量
另一种方式是将小量乘在李群上，而非加在李代数上。这种方式定义的模型称为扰动模型。由于左右的差异，需要区别这个增量是左乘在旋转阵，还是右乘在旋转阵上的。如果使用左乘，那么导数的含义为：
\[\begin{equation} \begin{array}{l}
\frac{{d\left( {RP} \right)}}{{dR}} = \mathop {\lim }\limits_{\delta \phi \to 0} \frac{{\exp {{\left( {\delta \phi } \right)}^ \wedge } \cdot RP - RP}}{{\delta \phi }}\\
= \mathop {\lim }\limits_{\Delta \phi \to 0} \frac{{(\exp {{\left( {\delta \phi } \right)}^ \wedge } - 1)RP}}{{\delta \phi }}\\
= - {\left( {RP} \right)^ \wedge }
\end{array} \end{equation} \]
可见，由于我们把小量乘在李群上（而非加在李代数上），结果中的导数将少一个雅可比矩阵。因此，扰动模型计算更为简洁，在实际上更加常用。
$SE(3)$上扰动模型
$SE(3)$上扰动模型为：考虑一个空间点$P$（必须为齐次坐标，否则维数不对）受到刚体变换 $T$ ，得到 $TP$ 。求 $TP$ 是如何随 $T$ 变化的。
类似于$SO(3)$，我们给$T$左乘一个小量$\Delta T=exp(\delta \xi)^\wedge $，得到：
\[ \begin{equation}\begin{array}{l}
\frac{{d\left( {TP} \right)}}{{dT}} = \mathop {\lim }\limits_{\delta \xi \to 0} \frac{{\exp {{\left( {\delta \xi } \right)}^ \wedge }TP - TP}}{{\delta \xi }}\\
= {(TP)^ \odot }
\end{array}\end{equation} \]
其中$^ \odot$算符把一个4$\times$4的矩阵变换成一个4$\times$6的矩阵，计算方式如下：
\[ \begin{equation} p = \left[ \begin{array}{l}
{x_{3 \times 1}}\\
{w_{1 \times 1}}
\end{array} \right], \quad {p^ \odot } = \left[ {\begin{array}{*{20}{c}}
{w{I_{3 \times 3}}}&{ - {x^ \wedge }_{3 \times 3}}\\
{{0^T}_{3 \times 1}}&{{0^T}_{3 \times 1}}
\end{array}} \right] \end{equation} \]
至此，我们明确了一个被变换之后的点，是如何随变换矩阵而变化的。注意，我们省略了$SE(3)$上BCH近似雅可比的推导（比较复杂且扰动模型里没有该雅可比），如果读者感兴趣可参考[1]。
小萝卜：师兄，知识我倒是知道了，您能讲讲$^ \odot$怎么念吗？
师兄：诶？……你看它像一个石子掉进井里，就念"dong"吧。
小萝卜：所以$SE(3)$对李代数的扰动的导数就是那个点自己的“dong”喽？
师兄：你愿意怎么记就怎么记吧……我不管了。
小结
本节介绍了以下几个关于李群代数的知识：
1. $SE(3)$的结构，指数映射和对数映射；
2. BCH公式与近似公式；
3. 李代数上的微分运算。
其中，介绍微分是介绍李代数的主要目的所在。知道了如何对位姿求导，我们才能深入地讨论姿态估计，直接法VO等话题。下一讲我们将介绍direct VO和semi-direct VO的方法。
[1]. Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2016
