博客转自：半闲居士
前言
理解李群与李代数，是理解许多SLAM中关键问题的基础。本讲我们继续介绍李群李代数的相关知识，重点放在李群李代数的微积分上，这对解决姿态估计问题具有重要意义。
回顾
为了描述三维空间里的运动，我们使用(3 imes 3)的旋转矩阵(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 imes3)）。
该式和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}
]
根据该结果得到的(phi = heta a)就是对应的李代数了。到这为止的内容都在上一讲中介绍过，本讲我们就接着往下说。
(SE(3))中的指数映射和对数映射
(mathfrak{se}(3))里的向量经常写成(xi)。它是一个六维向量，通常前三维为平移，后三维为旋转（也就是(mathfrak{so}(3))里的元素）。请注意，有些地方的定义是倒过来的，即前三维为旋转，后三维为平移，在使用的时候一定要小心！例如g2o的李代数和Sohpus的李代数定义就不一样。
记(mathbf{xi}=[mathbf{
ho}, mathbf{phi}]^T)，它们各为三维向量，一共组成了六维，构成一个李代数。我们的记法里(
ho)为平移，( heta)为旋转，与(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)) 的指数映射，右上较特殊一些，是平移量(
ho)的线性表示。其系数(J_{3×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)求了一次导数，所以这个阵也就记成了(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]
]
它对应的李代数(xi =[
ho, heta]^T) 为：
[egin{equation} phi = ln {left( R
ight)^ vee },
ho = {J^{ - 1}}t end{equation}
]
因为已经有了(SO(3))上的计算方式，这里的式子就变得十分简单了。
Baker-Campbell-Hausdorff公式
小萝卜：师兄我还有个问题啊。
师兄：嗯你说。
小萝卜：李群和李代数的对应关系倒是不难啦。但实际用的时候，我经常在李群上做两个矩阵的乘法。例如，做R1⋅R2时，它们对应的李代数发生了什么呢？是不是直接加起来啊？
对，这就是我们要讨论的问题，即：
[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公式给出了这个运算的答案：
[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_r)和(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)较小）为例。我们给一个旋转矩阵(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}
]
定义在李群上的乘增量
另一种方式是将小量乘在李群上，而非加在李代数上。这种方式定义的模型称为扰动模型。由于左右的差异，需要区别这个增量是左乘在旋转阵，还是右乘在旋转阵上的。如果使用左乘，那么导数的含义为：
[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]。
小萝卜：师兄，知识我倒是知道了，您能讲讲⊙怎么念吗？
师兄：诶？……你看它像一个石子掉进井里，就念"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