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