《视觉SLAM十四讲》学习笔记-李代数求导与扰动模型的一些重要公式 slam

BCH(Baker-Campbell-Hausdorff)公式：
ln(exp(A)exp(B))=A+B+12[A,B]+112[A,[A,B]]?112[B,[A,B]]+?ln ? ( exp ? ( A ) exp ? ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] ? 1 12 [ B , [ A , B ] ] + ?

当为SO(3)上的李代数 ln(exp(Φ∧1)exp(Φ∧2))∨ln ? ( exp ? ( Φ 1 ∧ ) exp ? ( Φ 2 ∧ ) ) ∨ 时，BCH变为：

ln(exp(Φ∧1)exp(Φ∧2))∨≈{Jl(Φ2)?1Φ1+Φ2Jr(Φ1)?1Φ2+Φ1if Φ1 is small,if Φ2 is small.ln ? ( exp ? ( Φ 1 ∧ ) exp ? ( Φ 2 ∧ ) ) ∨ ≈ { J l ( Φ 2 ) ? 1 Φ 1 + Φ 2 i fΦ 1i ss m a l l , J r ( Φ 1 ) ? 1 Φ 2 + Φ 1 i fΦ 2i ss m a l l .
这其中，左乘BCH近似的Jacobi JlJ l为：
Jl=J=sinθθI+(1?sinθθ)a? a? ?+1?cosθθa? ∧J l = J = sin ? θ θ I + ( 1 ? sin ? θ θ ) a → a → ? + 1 ? cos ? θ θ a → ∧
JlJ l的逆为
J?1l=θ2cotθ2I+(1?θ2cotθ2)a? a? ??θ2a? ∧J l ? 1 = θ 2 cot ? θ 2 I + ( 1 ? θ 2 cot ? θ 2 ) a → a → ? ? θ 2 a → ∧
另外，右乘Jacobi JrJ r可对自变量取负号即可得到：
Jr(Φ)=Jl(?Φ)J r ( Φ ) = J l ( ? Φ )

BCH近似的意义. 旋转 RR 对应的李代数为 ΦΦ , 左乘微小旋转 △R△ R 所对应的李代数为 △Φ△ Φ .
则李群上的结果为 △R?R△ R ? R ,李代数的结果根据BCH变为 J?1l(Φ)△Φ+ΦJ l ? 1 ( Φ ) △ Φ + Φ :

exp(△Φ∧)exp(Φ∧)=exp((Φ+J?1l(Φ)△Φ)∧)exp ? ( △ Φ ∧ ) exp ? ( Φ ∧ ) = exp ? ( ( Φ + J l ? 1 ( Φ ) △ Φ ) ∧ )
若在李代数上做加法，Φ→Φ+△ΦΦ → Φ + △ Φ :
exp((Φ+△Φ)∧)=exp((Jl△Φ)∧)exp(Φ∧)=exp(Φ∧)exp((Jr△Φ)∧)exp ? ( ( Φ + △ Φ ) ∧ ) = exp ? ( ( J l △ Φ ) ∧ ) exp ? ( Φ ∧ ) = exp ? ( Φ ∧ ) exp ? ( ( J r △ Φ ) ∧ )【《视觉SLAM十四讲》学习笔记-李代数求导与扰动模型的一些重要公式】
SO(3)李代数的求导。两种思路：
- [x] 用李代数表示姿态，根据李代数加法对李代数求导；
- [x] 对李群左乘或右乘微小的扰动，对该扰动求导.

3.1 问题：对一个空间点 p?p → 进行旋转，得到 Rp?R p → ,求旋转后的点相对于旋转的导数，即 ?Rp? ?R? R p → ? R .
设RR 对应的李代数为 ΦΦ , 则问题表述为:

?(exp(Φ∧)p? )?Φ? ( exp ? ( Φ ∧ ) p → ) ? Φ

?(exp(Φ∧)p? )?Φ=limδΦ→0exp((Φ+δΦ)∧)p? ?exp(Φ∧)p? δΦ=limδΦ→0exp((JlδΦ)∧)exp(Φ∧)p? ?exp(Φ∧)p? δΦ(BCH linear approximation)≈limδΦ→0I+((JlδΦ)∧)exp(Φ∧)p? ?exp(Φ∧)p? δΦ(Taylor's formula)=limδΦ→0(JlδΦ)∧exp(Φ∧)p? δΦ=limδΦ→0?(exp(Φ∧)p? )∧JlδΦδΦ(cross product)=?(Rp? )∧Jl.? ( exp ? ( Φ ∧ ) p → ) ? Φ = lim δ Φ → 0 exp ? ( ( Φ + δ Φ ) ∧ ) p → ? exp ? ( Φ ∧ ) p → δ Φ = lim δ Φ → 0 exp ? ( ( J l δ Φ ) ∧ ) exp ? ( Φ ∧ ) p → ? exp ? ( Φ ∧ ) p → δ Φ( BCH linear approximation ) ≈ lim δ Φ → 0 I + ( ( J l δ Φ ) ∧ ) exp ? ( Φ ∧ ) p → ? exp ? ( Φ ∧ ) p → δ Φ( Taylor's formula ) = lim δ Φ → 0 ( J l δ Φ ) ∧ exp ? ( Φ ∧ ) p → δ Φ = lim δ Φ → 0 ? ( exp ? ( Φ ∧ ) p → ) ∧ J l δ Φ δ Φ( cross product ) = ? ( R p → ) ∧ J l .
所以旋转后的点相对于李代数的导数为：

?(Rp? )?Φ=(?Rp? )∧Jl? ( R p → ) ? Φ = ( ? R p → ) ∧ J l
3.2 扰动模型的求导公式推导
问题：对 RR 的一次扰动量为 △R△ R 。设左扰动 △R△ R 对应的李代数为 φφ ，对 φφ 求导即为问题解：

?Rp? ?φ=limφ→0exp(φ∧)exp(Φ∧)p? ?exp(Φ∧)p? φ.? R p → ? φ = lim φ → 0 exp ? ( φ ∧ ) exp ? ( Φ ∧ ) p → ? exp ? ( Φ ∧ ) p → φ .
?Rp? ?φ=limφ→0exp(φ∧)exp(Φ∧)p? ?exp(Φ∧)p? φ≈limφ→0(1+φ∧)exp(Φ∧)p? ?exp(Φ∧)p? φ=limφ→0φ∧exp(Φ∧)p? φ=limφ→0φ∧Rp? φ=limφ→0?(Rp? )∧φφ=?(Rp? )∧.? R p → ? φ = lim φ → 0 exp ? ( φ ∧ ) exp ? ( Φ ∧ ) p → ? exp ? ( Φ ∧ ) p → φ ≈ lim φ → 0 ( 1 + φ ∧ ) exp ? ( Φ ∧ ) p → ? exp ? ( Φ ∧ ) p → φ = lim φ → 0 φ ∧ exp ? ( Φ ∧ ) p → φ = lim φ → 0 φ ∧ R p → φ = lim φ → 0 ? ( R p → ) ∧ φ φ = ? ( R p → ) ∧ .