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

1. 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   Φ 1   i s   s m a l l , J r ( Φ 1 ) − 1 Φ 2 + Φ 1 i f   Φ 2   i s   s m a l l .

这其中，左乘BCH近似的Jacobi Jl J l 为：

Jl=J=sinθθI+(1−sinθθ)a⃗ a⃗ ⊤+1−cosθθa⃗ ∧ J l = J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a → a → ⊤ + 1 − cos ⁡ θ θ a → ∧

Jl J 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 Jr J r 可对自变量取负号即可得到：

Jr(Φ)=Jl(−Φ) J r ( Φ ) = J l ( − Φ )

1. BCH近似的意义. 旋转 R R 对应的李代数为 Φ Φ , 左乘微小旋转 △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 △ Φ ) ∧ )

2. SO(3)李代数的求导。两种思路：

   • [x] 用李代数表示姿态，根据李代数加法对李代数求导；
   • [x] 对李群左乘或右乘微小的扰动，对该扰动求导.

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

∂(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 扰动模型的求导公式推导

问题：对 R R 的一次扰动量为 △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 → ) ∧ .