SLAM中为什么要用李代数？
本人从SLAM问题的角度出发得出的结论目前是这样的：在SLAM问题中，为了表示旋转姿态需要旋转矩阵或者四元数，在后端优化过程中为了求解非线性优化问题需要求雅可比矩阵，即求导；在运动学的描述中也需要求导；这两种情况中一般是对位姿求导，这时就产生了一个矛盾：根据导数的定义可知，对某一个变量求导，定义中需要该变量产生微小增量，为了保证物理意义的正确，我们希望该变量对加法是封闭的，而旋转矩阵或者四元数对加法都不封闭，所以引入李代数。李代数对李括号具有封闭性，李代数和李群中的元素通过指数映射和对数映射一一对应（对于 S O ( 3 ) SO(3) SO(3)李群需要规定旋转角在 ± π \pm \pi ±π之间），SLAM中引入的李代数的李括号刚好都是加法，这样我们就可以用李代数进行求导过程。
BCH线性近似解决了什么问题？
BCH线性近似提供了在SLAM中的李代数上做加法 与 李群上做乘法的对应关系。
指数映射与对数映射
每个李群都有与之对应的李代数，李代数描述了李群的局部性质，即单位元的正切空间。
与 S O ( 3 ) SO(3) SO(3)对应的李代数为 s o ( 3 ) \mathfrak{so}(3) 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 = e x p ( ϕ ∧ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n R = exp(\phi^{\wedge}) = \sum_{n=0}^{\infty} \frac{1}{n!}(\phi^{\wedge})^{n} R=exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n
如果把 ϕ \phi ϕ写成 θ n \theta \mathbf{n} θn，经过推导会发现：
e x p ( θ n ∧ ) = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ∧ exp(\theta \mathbf{n}^{\wedge}) = cos\theta I + (1-cos \theta) \mathbf{n} \mathbf{n}^{T} + sin \theta \mathbf{n}^{\wedge} exp(θn∧)=cosθI+(1−cosθ)nnT+sinθn∧
这与罗德里格斯公式如出一辙。
这说明 s o ( 3 ) \mathfrak{so}(3) so(3)的物理意义是和旋转矩阵 R R R对应的旋转向量。
SO(3)上的对数映射
ϕ = l n ( R ) ∨ = ( ∑ n = 0 ∞ ( − 1 ) n n + 1 ( R − I ) n + 1 ) \phi = ln(R)^{\vee} = \left ( \sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(R-I)^{n+1} \right) ϕ=ln(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)
一般不用，要求 S O ( 3 ) SO(3) SO(3)的对数映射其实就是求对应的旋转向量，直接求 θ \theta θ和特征值为1的特征向量即可。
BCH线性近似
BCH公式提供了两个李代数指数映射乘积的完整形式：
l n ( e x p ( A ) e x p ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + ... 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(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+...
二元运算符 [ , ] [,] [,]为李括号。
线性近似表达：
l n ( e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 当 ϕ 1 为小量 J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 当 ϕ 2 为小量 ln(exp(\phi_{1}^{\wedge})exp(\phi_{2}^{\wedge}))^{\vee} \approx \begin{cases} J_{l}(\phi_{2})^{-1}\phi_{1} + \phi_{2} & \text{ 当 } \phi_{1}\text{为小量} \\ J_{r}(\phi_{1})^{-1}\phi_{2} + \phi_{1} & \text{ 当 } \phi_{2}\text{为小量} \end{cases} ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1 当 ϕ1为小量 当 ϕ2为小量
对于 ϕ = θ n \phi = \theta \mathbf{n} ϕ=θn：
J l ( ϕ ) = s i n θ θ I + ( 1 − s i n θ θ ) n n T + 1 − c o s θ θ n ∧ J_{l}(\phi) = \frac{sin\theta}{\theta}I + (1 - \frac{sin\theta}{\theta})\mathbf{n}\mathbf{n}^{T} + \frac{1 - cos\theta}{\theta}\mathbf{n}^{\wedge} Jl(ϕ)=θsinθI+(1−θsinθ)nnT+θ1−cosθn∧
J l ( ϕ ) − 1 = θ 2 c o t θ 2 I + ( 1 − θ 2 c o t θ 2 ) n n T − θ 2 n ∧ J_{l}(\phi)^{-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} Jl(ϕ)−1=2θcot2θI+(1−2θcot2θ)nnT−2θn∧
J r ( ϕ ) = J l ( − ϕ ) J_{r}(\phi) = J_{l}(-\phi) Jr(ϕ)=Jl(−ϕ)