视觉SLAM十四讲学习4 （2）李代数求导

前言

本篇继续李群和李代数。

回顾

群是一种集合加上一种运算，满足封闭性，结合律，幺元和逆。

李群是一种连续群，李代数表达李群在局部上的性质。

正交群 S O ( 3 ) = { R 3 × 3 ∣ R R T = I } SO(3)=\{R_{3\times 3}|RR^T=I \} SO(3)={R3×3​∣RRT=I}与李代数 s o ( 3 ) = { ϕ 3 ∣ ϕ ∧ ∈ R 3 × 3 } so(3)=\{\phi_3|\phi^\land \in {\bf R^{3\times 3}} \} so(3)={ϕ3​∣ϕ∧∈R3×3}存在指数映射 R = e ϕ ∧ R=e^{\phi^\land} R=eϕ∧，可以通过罗德里格斯公式进行转换： R = n n T ( 1 − cos ⁡ θ ) + I cos ⁡ θ + n ∧ sin ⁡ θ , ϕ = θ n R=nn^T(1-\cos \theta)+I\cos\theta+n^\land\sin \theta,\phi=\theta n R=nnT(1−cosθ)+Icosθ+n∧sinθ,ϕ=θn。李括号可表示为： [ ϕ 1 , ϕ 2 ] = ( ϕ 1 ∧ ϕ 2 ∧ − ϕ 2 ∧ ϕ 1 ∧ ) ∨ [\phi_1,\phi_2]=(\phi_1^\land\phi_2^\land-\phi_2^\land\phi_1^\land)^\lor [ϕ1​,ϕ2​]=(ϕ1∧​ϕ2∧​−ϕ2∧​ϕ1∧​)∨。

欧式群 S E ( 3 ) = { T ∣ T = [ R t 0 1 ] , R ∈ S O ( 3 ) , t 3 × 1 } SE(3)=\{T|T=\begin{bmatrix}R & t \\ 0 & 1 \end{bmatrix},R\in SO(3),t_{3\times1}\} SE(3)={T∣T=[R0​t1​],R∈SO(3),t3×1​}与李代数 s e ( 3 ) = { δ ∣ δ = [ ρ ϕ ] , δ ∧ = [ ϕ ∧ ρ 0 0 ] ∈ R 4 × 4 } se(3)=\{\delta|\delta=\begin{bmatrix}\rho \\ \phi \end{bmatrix},\delta^\land=\begin{bmatrix}\phi^\land &\rho \\ 0 & 0 \end{bmatrix}\in {\bf R^{4\times 4}}\} se(3)={δ∣δ=[ρϕ​],δ∧=[ϕ∧0​ρ0​]∈R4×4}也存在指数映射 T = e δ ∧ T=e^{\delta^\land} T=eδ∧。 T T T与 δ \delta δ的转换关系为： T = [ R t 0 1 ] = e δ ∧ = [ e ϕ ∧ J ρ 0 1 ] J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) n n T + 1 − cos ⁡ θ θ n ∧ ϕ = θ n T=\begin{bmatrix} R & t \\ 0 & 1 \\ \end{bmatrix}=e^{\delta^\land} =\begin{bmatrix} e^{\phi^\land} &J\rho \\ 0 & 1 \\ \end{bmatrix} \\ \quad \\ J=\frac{\sin \theta}{\theta}I+(1-\frac{\sin\theta}{\theta})nn^T+\frac{1-\cos \theta}{\theta}n^\land \\ \phi = \theta n T=[R0​t1​]=eδ∧=[eϕ∧0​Jρ1​]J=θsinθ​I+(1−θsinθ​)nnT+θ1−cosθ​n∧ϕ=θn 李括号表示为 [ δ 1 , δ 2 ] = ( δ 1 ∧ δ 2 ∧ − δ 2 ∧ δ 1 ∧ ) ∨ [\delta_1,\delta_2]=(\delta_1^\land\delta_2^\land-\delta_2^\land\delta_1^\land)^{\lor} [δ1​,δ2​]=(δ1∧​δ2∧​−δ2∧​δ1∧​)∨

李群与李代数求导 BCH公式

S O ( 3 ) , S E ( 3 ) SO(3),SE(3) SO(3),SE(3)与它们的李代数具有指数映射的关系。在实数域中，指数上的加法等同于两个指数的乘法。然而在李群上并不成立。

两个李代数的指数映射的乘积符合BCH公式： e ϕ 1 ∧ e ϕ 2 ∧ = e ϕ 1 ∧ + ϕ 2 ∧ + 1 2 [ ϕ 1 , ϕ 2 ] ∧ + … e^{\phi_1^\land}e^{\phi_2^\land}=e^{\phi_1^\land+\phi_2^\land+\frac{1}{2}[\phi_1,\phi_2]^\land+\dots} eϕ1∧​eϕ2∧​=eϕ1∧​+ϕ2∧​+21​[ϕ1​,ϕ2​]∧+…

对于 S O ( 3 ) SO(3) SO(3)与 s o ( 3 ) so(3) so(3)，当指数乘积有一方是小量时，可以将BCH公式的高阶小量省去： e Δ ϕ ∧ e ϕ ∧ = e ( J l ( ϕ ) − 1 Δ ϕ + ϕ ) ∧ e ϕ ∧ e Δ ϕ ∧ = e ( ϕ + J r ( ϕ ) − 1 Δ ϕ ) ∧ e ( ϕ + Δ ϕ ) ∧ = e ( J l ( ϕ ) Δ ϕ ) ∧ e ϕ ∧ = e ϕ ∧ e ( J r ( ϕ ) Δ ϕ ) ∧ J l ( ϕ ) = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) n n T + 1 − cos ⁡ θ θ n ∧ J r ( ϕ ) = J l ( − ϕ ) ϕ = θ n e^{\Delta\phi^\land}e^{\phi^\land}= e^{(J_l(\phi)^{-1}\Delta\phi+\phi)^\land} \\ e^{\phi^{\land}}e^{\Delta\phi^\land}=e^{(\phi+J_r(\phi)^{-1}\Delta\phi)^\land} \\ \quad \\ e^{(\phi+\Delta \phi)^\land}=e^{(J_l(\phi)\Delta\phi)^\land}e^{\phi^\land}=e^{\phi^\land}e^{(J_r(\phi)\Delta\phi)^\land} \\ \quad \\ J_l(\phi)=\frac{\sin \theta}{\theta}I+(1-\frac{\sin \theta}{\theta})nn^T+\frac{1-\cos \theta}{\theta}n^\land \\ J_r(\phi)=J_l(-\phi) \\ \phi = \theta n eΔϕ∧eϕ∧=e(Jl​(ϕ)−1Δϕ+ϕ)∧eϕ∧eΔϕ∧=e(ϕ+Jr​(ϕ)−1Δϕ)∧e(ϕ+Δϕ)∧=e(Jl​(ϕ)Δϕ)∧eϕ∧=eϕ∧e(Jr​(ϕ)Δϕ)∧Jl​(ϕ)=θsinθ​I+(1−θsinθ​)nnT+θ1−cosθ​n∧Jr​(ϕ)=Jl​(−ϕ)ϕ=θn

对于 S E ( 3 ) SE(3) SE(3)与 s e ( 3 ) se(3) se(3)也有与以上相同的BCH近似表达式，但是并不常用。

李代数求导

对于一个向量旋转 R p Rp Rp，求其关于旋转的导数时，不能表示为 ∂ R p / ∂ R \partial Rp/\partial R ∂Rp/∂R，这是因为旋转矩阵 R R R对于加减法是不连续的。而向量加法是连续的，因此要旋转求导时，要从 S O ( 3 ) SO(3) SO(3)转换为李代数的形式： ∂ e ϕ ∧ p ∂ ϕ = e ( ϕ + Δ ϕ ) ∧ p − e ϕ ∧ p Δ ϕ = e ( J l ( ϕ ) Δ ϕ ) ∧ e ϕ ∧ p − e ϕ ∧ p Δ ϕ = ( e ( J l ( ϕ ) Δ ϕ ) ∧ − 1 ) e ϕ ∧ p Δ ϕ = ( J l ( ϕ ) Δ ϕ ) ∧ e ϕ ∧ p Δ ϕ = ( J l ( ϕ ) Δ ϕ ) ∧ R p Δ ϕ = − ( R p ) ∧ J l ( ϕ ) Δ ϕ Δ ϕ = − ( R p ) ∧ J l ( ϕ ) \frac{\partial e^{\phi^\land}p}{\partial \phi} = \frac {e^{(\phi+\Delta \phi)^\land}p-e^{\phi^\land}p}{\Delta \phi} \\ = \frac {e^{(J_l(\phi)\Delta \phi)^\land}e^{\phi^\land}p-e^{\phi^\land}p}{\Delta \phi}\\ = \frac {(e^{(J_l(\phi)\Delta \phi)^\land}-1)e^{\phi^\land}p}{\Delta \phi}\\ = \frac {(J_l(\phi)\Delta \phi)^\land e^{\phi^\land}p}{\Delta \phi}\\ = \frac {(J_l(\phi)\Delta \phi)^\land Rp}{\Delta \phi}\\ = -\frac {(Rp)^\land J_l(\phi)\Delta \phi}{\Delta \phi}\\ = -(Rp)^\land J_l(\phi)\\ ∂ϕ∂eϕ∧p​=Δϕe(ϕ+Δϕ)∧p−eϕ∧p​=Δϕe(Jl​(ϕ)Δϕ)∧eϕ∧p−eϕ∧p​=Δϕ(e(Jl​(ϕ)Δϕ)∧−1)eϕ∧p​=Δϕ(Jl​(ϕ)Δϕ)∧eϕ∧p​=Δϕ(Jl​(ϕ)Δϕ)∧Rp​=−Δϕ(Rp)∧Jl​(ϕ)Δϕ​=−(Rp)∧Jl​(ϕ)

扰动模型

除了通过李代数定义求导，还可以通过旋转左乘微小扰动的扰动模型求导： ∂ e Δ ϕ ∧ e ϕ ∧ p ∂ Δ ϕ = lim ⁡ Δ ϕ → 0 e Δ ϕ ∧ e ϕ ∧ p − e ϕ ∧ p Δ ϕ = lim ⁡ Δ ϕ → 0 ( 1 + Δ ϕ ∧ ) e ϕ ∧ p − e ϕ ∧ p Δ ϕ = lim ⁡ Δ ϕ → 0 Δ ϕ ∧ ( e ϕ ∧ p ) Δ ϕ = lim ⁡ Δ ϕ → 0 − ( e ϕ ∧ p ) ∧ Δ ϕ Δ ϕ = − ( R p ) ∧ \frac{\partial e^{\Delta \phi ^\land}e^{\phi^\land}p}{\partial \Delta\phi} =\lim_{\Delta\phi \to 0} \frac {e^{\Delta \phi ^\land}e^{\phi^\land}p-e^{\phi^\land}p} {\Delta \phi} \\ = \lim_{\Delta\phi \to 0}\frac {(1+\Delta \phi^\land)e^{\phi^\land}p - e^{\phi^\land }p }{\Delta \phi} \\ = \lim_{\Delta\phi \to 0}\frac{\Delta \phi^\land (e^{\phi^\land}p)}{\Delta \phi}\\ =\lim_{\Delta\phi \to 0} -\frac {(e^{\phi^\land}p)^\land \Delta \phi}{\Delta \phi}\\ = -(Rp)^\land\\ ∂Δϕ∂eΔϕ∧eϕ∧p​=Δϕ→0lim​ΔϕeΔϕ∧eϕ∧p−eϕ∧p​=Δϕ→0lim​Δϕ(1+Δϕ∧)eϕ∧p−eϕ∧p​=Δϕ→0lim​ΔϕΔϕ∧(eϕ∧p)​=Δϕ→0lim​−Δϕ(eϕ∧p)∧Δϕ​=−(Rp)∧ 扰动模型比直接李代数求导更加简洁。

S E ( 3 ) SE(3) SE(3)的扰动模型

现在来求欧式群的扰动模型： ∂ e Δ δ ∧ e δ ∧ p ∂ Δ δ = lim ⁡ Δ δ → 0 e Δ δ ∧ e δ ∧ p − e δ ∧ p Δ δ = lim ⁡ Δ δ → 0 Δ δ ∧ ( e δ ∧ p ) Δ δ = lim ⁡ Δ δ → 0 [ Δ ϕ ∧ Δ ρ 0 0 ] [ e ϕ ∧ p + J ρ 1 ] [ Δ ρ Δ ϕ ] = lim ⁡ Δ δ → 0 [ Δ ϕ ∧ ( e ϕ ∧ p + J ρ ) + Δ ρ 0 ] [ Δ ρ Δ ϕ ] = [ I − ( R p + t ) ∧ 0 T 0 T ] \frac{\partial e^{\Delta \delta ^\land}e^{\delta^\land}p}{\partial \Delta\delta} =\lim_{\Delta\delta \to 0} \frac {e^{\Delta \delta ^\land}e^{\delta^\land}p-e^{\delta^\land}p} {\Delta \delta} \\ = \lim_{\Delta\delta \to 0}\frac{\Delta \delta^\land (e^{\delta^\land}p)}{\Delta \delta}\\ \quad \\ =\lim_{\Delta\delta \to 0} \frac {\begin{bmatrix} {\Delta \phi^\land} & \Delta\rho \\ 0 & 0 \end{bmatrix} \begin{bmatrix} e^{\phi^\land}p + J\rho \\ 1 \end{bmatrix}} {\begin{bmatrix} \Delta\rho \\ \Delta\phi \end{bmatrix}} \\ \quad \\ =\lim_{\Delta\delta \to 0} \frac {\begin{bmatrix} {\Delta \phi^\land}(e^{\phi^\land}p + J\rho)+\Delta\rho \\ 0 \end{bmatrix}}{\begin{bmatrix} \Delta\rho \\ \Delta\phi \end{bmatrix}} \\ \quad \\ = \begin{bmatrix} I & -(Rp+t)^\land \\ 0^T & 0^T \end{bmatrix} \\ ∂Δδ∂eΔδ∧eδ∧p​=Δδ→0lim​ΔδeΔδ∧eδ∧p−eδ∧p​=Δδ→0lim​ΔδΔδ∧(eδ∧p)​=Δδ→0lim​[ΔρΔϕ​][Δϕ∧0​Δρ0​][eϕ∧p+Jρ1​]​=Δδ→0lim​[ΔρΔϕ​][Δϕ∧(eϕ∧p+Jρ)+Δρ0​]​=[I0T​−(Rp+t)∧0T​]

后记

本篇记录了李代数求导的方法，包括定义求导与扰动模型求导。下一篇将学习非线性优化，然后就是Eigen库、 Sophus库、ceres库和g2o库的使用实战。