SLAM中的李群与李代数【SLAM】

SLAM中为什么要用李代数？

本人从SLAM问题的角度出发得出的结论目前是这样的：在SLAM问题中，为了表示旋转姿态需要旋转矩阵或者四元数，在后端优化过程中为了求解非线性优化问题需要求雅可比矩阵，即求导；在运动学的描述中也需要求导；这两种情况中一般是对位姿求导，这时就产生了一个矛盾：根据导数的定义可知，对某一个变量求导，定义中需要该变量产生微小增量，为了保证物理意义的正确，我们希望该变量对加法是封闭的，而旋转矩阵或者四元数对加法都不封闭，所以引入李代数。李代数对李括号具有封闭性，李代数和李群中的元素通过指数映射和对数映射一一对应（对于$SO(3)$李群需要规定旋转角在$\pm \pi$之间），SLAM中引入的李代数的李括号刚好都是加法，这样我们就可以用李代数进行求导过程。

BCH线性近似解决了什么问题？

BCH线性近似提供了在SLAM中的李代数上做加法 与 李群上做乘法的对应关系。

指数映射与对数映射

每个李群都有与之对应的李代数，李代数描述了李群的局部性质，即单位元的正切空间。
与$SO(3)$对应的李代数为$\mathfrak{so}(3)$：
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{so}(3) = \{ \phi \in \mathbb{R}^{3}, \mathit{\Phi} = \phi^{\wedge} \in \mathbb{R}^{3\times3} \} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}

SO(3)上的指数映射

$$R=exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$
如果把$\varphi$写成$\theta \mathbf{n}$，经过推导会发现：
$$exp(\theta {\mathbf{n}}^{\wedge })=cos\theta I+(1-cos\theta )\mathbf{n}{\mathbf{n}}^T+sin\theta {\mathbf{n}}^{\wedge }$$
这与罗德里格斯公式如出一辙。
这说明$\mathfrak{so}(3)$的物理意义是和旋转矩阵$R$对应的旋转向量。

SO(3)上的对数映射

$$\varphi =ln(R{)}^{\vee }=\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}\right)$$
一般不用，要求$SO(3)$的对数映射其实就是求对应的旋转向量，直接求$\theta$和特征值为1的特征向量即可。

BCH线性近似

BCH公式提供了两个李代数指数映射乘积的完整形式：
$$ln(exp(A)exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\dots$$
二元运算符$[,]$为李括号。

线性近似表达：
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& \text{ 当 }{\varphi }_1\text{为小量}\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& \text{ 当 }{\varphi }_2\text{为小量}\end{array}$$

对于$\varphi =\theta \mathbf{n}$：
$$J_l(\varphi )=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })\mathbf{n}{\mathbf{n}}^T+\frac{1-cos\theta }{\theta }{\mathbf{n}}^{\wedge }$$

$$J_l(\varphi {)}^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})\mathbf{n}{\mathbf{n}}^T-\frac{\theta }{2}{\mathbf{n}}^{\wedge }$$

$$J_r(\varphi )=J_l(-\varphi )$$