李代数的求导及扰动模型

1.引言

李代数的求导以及扰动模型有什么用?当然就是求导的用,但在什么情况下需要求导,在机器人领域和3d视觉领域当然是函数的变量是旋转矩阵或变换矩阵,多发生在优化问题上!
举个例子,比如说在PnP问题中,如果使用基于优化的方法进行求解需要我们求解公式(1.1):

T ∗ = a r g m i n T 1 2 ∑ i = 1 n ∥ u i − 1 s i K T P i ∥ 2   ( 1.1 ) T^{*}=arg min_{T}\frac{1}{2}\sum_{i=1}^{n}\left\| u_{i}-\frac{1}{s_{i}}KTP_{i}\right\|^{2} \ (1.1) T=argminT21i=1nuisi1KTPi2 (1.1)

当然非线性优化的方法有很多最速下降法、牛顿法、共轭梯度法、拟牛顿法、信赖区域法和最小二乘法等等,但是不管使用什么方法大多数都逃不掉求雅可比矩阵或者是Hession矩阵,而这些导数是关于R,T的,所以我们需要学习李代数的求导。

2.BCH公式及近似模型

BCH公式实质是在讨论李群的乘法与李代数的加法之间的关系,这个讨论的问题如果写成公式就是(2.1):

e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) = e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) ?   ( 2.1 ) exp(\phi_{1}^{\wedge})exp(\phi_{2}^{\wedge})=exp((\phi_{1}+\phi_{2})^{\wedge})? \ (2.1) exp(ϕ1)exp(ϕ2)=exp((ϕ1+ϕ2))? (2.1)

该等式是否成立呢?如果不成立,它们之间又是什么关系呢?
公式(2.2)即为BCH近似公式:
l n ( e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 ,   当 ϕ 1 为 小 量 , J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 ,   当 ϕ 2 为 小 量   ( 2.2 ) ln(exp(\phi_{1}^{\wedge})exp(\phi_{2}^{\wedge}))^{\vee}\approx \left\{\begin{matrix} J_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2} ,\ 当\phi_{1}为小量,\\ J_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1} ,\ 当\phi_{2}为小量\\ \end{matrix}\right. \ (2.2) ln(exp(ϕ1)exp(ϕ2)){Jl(ϕ2)1ϕ1+ϕ2, ϕ1Jr(ϕ1)1ϕ2+ϕ1 ϕ2 (2.2)

以第一个近似式为例。该式告诉我们,当对一个旋转矩阵 R 2 R_{2} R2(李代数为 ϕ 2 \phi_{2} ϕ2)左乘一个微小旋转矩阵 R 1 R_{1} R1(李代数为 ϕ 1 \phi_{1} ϕ1)时,可以近似的看作,在原有李代数 ϕ 2 \phi_{2} ϕ2上加上一项 J l ( ϕ 2 ) − 1 ϕ 1 J_{l}(\phi_{2})^{-1}\phi_{1} Jl(ϕ2)1ϕ1。同样第二个近似描述了右乘一个微小位移的情况。使用时需注意左乘还是右乘,之后的解析大多数以左乘为例。
左乘BCH近似雅可比 J l J_{l} Jl为公式(2.3):
J l = J = s i n θ θ I + ( 1 − s i n θ θ ) a a T + 1 − c o s θ θ a ∧   ( 2.3 ) J_{l}=J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta}{\theta})aa^{T}+\frac{1-cos\theta }{\theta}a^{\wedge} \ (2.3) Jl=J=θsinθI+(1θsinθ)aaT+θ1cosθa (2.3)
它的逆为:
J l − 1 = θ 2 c o t θ 2 I + ( 1 − θ 2 c o t θ 2 ) a a T − θ 2 a ∧   ( 2.4 ) J_{l}^{-1}=\frac{\theta}{2}cot\frac{\theta}{2}I+(1-\frac{\theta}{2}cot\frac{\theta}{2})aa^{T}-\frac{\theta}{2}a^{\wedge} \ (2.4) Jl1=2θcot2θI+(12θcot2θ)aaT2θa (2.4)
而右乘雅可比仅需要对自变量取负号即可:
J r ( ϕ ) = J l ( − ϕ )   ( 2.5 ) J_{r}(\phi)=J_{l}(-\phi) \ (2.5) Jr(ϕ)=Jl(ϕ) (2.5)

为了方便进行求导和引出扰动模型,我们重新叙述BCH近似的意义。假定对某个旋转 R R R,对应的李代数为 ϕ \phi ϕ。给他左乘一个微小旋转,记作 Δ R \Delta R ΔR,对应的李代数为 Δ ϕ \Delta\phi Δϕ。那么在李群上,得到的结果就是 Δ R ⋅ R \Delta R\cdot R ΔRR。而在李代数上,根据BCH近似,为 J l ( ϕ ) − 1 Δ ϕ + ϕ J_{l}(\phi)^{-1}\Delta\phi+\phi Jl(ϕ)1Δϕ+ϕ。因此可以写成(2.6)
e x p ( Δ ϕ ∧ ) e x p ( ϕ ∧ ) = e x p ( ( J l − 1 ( ϕ ) Δ ϕ + ϕ ) ∧ )   ( 2.6 ) exp(\Delta \phi^{\wedge})exp(\phi^{\wedge})=exp((J_{l}^{-1}(\phi)\Delta \phi+\phi)^{\wedge}) \ (2.6) exp(Δϕ)exp(ϕ)=exp((Jl1(ϕ)Δϕ+ϕ)) (2.6)

李代数的求导及扰动模型_第1张图片

根据式子2.7近似为李群上带左右雅可比的乘法

李代数的求导及扰动模型_第2张图片

在视觉slam上直接截的图,4.36=2.8,4.37=2.9,参考文献[6]为 T. Barfoot,State estimation for robotics:A matrix lie group approach,2016.

使用李代数进行求导的思路分为两种:

  1. 用李代数表示姿态,然后根据李代数加法对李代数求导。
  2. 对李群左乘右乘微小扰动,然后对该扰动求导,成为左扰动和右扰动模型。

3. SO(3)上的李代数求导

假设我们对一个空间点 p p p进行了旋转,得到 R p Rp Rp。现在要计算旋转之后点的坐标相对于旋转的导数,我们非正式的记为(3.1)(这里为什么说是非正式的呢?因为李群上无法定义加法,所以无法定义传统意义上的导数):
∂ ( R p ) ∂ R   ( 3.1 ) \frac{\partial (Rp)}{\partial R} \ (3.1) R(Rp) (3.1)

由于SO(3)没有加法,所以该导数无法按照导数的定义进行计算。设 R R R对应的李代数为 ϕ \phi ϕ,我们转而计算:
∂ ( e x p ( ϕ ∧ ) p ) ∂ ϕ   ( 3.2 ) \frac{\partial (exp(\phi^{\wedge})p)}{\partial \phi} \ (3.2) ϕ(exp(ϕ)p) (3.2)

下面将根据导数的定义进行手写推导:

李代数的求导及扰动模型_第3张图片

不过式(3.3)仍然含有形式比较复杂的 J l J_{l} Jl,我们不太希望计算它。所以下面讲解了扰动模型,提供了更为简单的俄导数计算方式。

4.SO(3)上的扰动模型(左乘)

另一种求导方式是对R进行一次扰动 Δ R \Delta R ΔR,看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边,最后结果会有一点微小的差异,我们以左乘为例。
设左扰动 Δ R \Delta R ΔR对应的李代数为 φ \varphi φ,然后对 φ \varphi φ进行求导:
∂ ( R p ) ∂ φ = lim ⁡ φ → 0 e x p ( φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ   ( 4.1 ) \frac{\partial (Rp)}{\partial \varphi }=\displaystyle \lim_{\varphi \to 0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi} \ (4.1) φ(Rp)=φ0limφexp(φ)exp(ϕ)pexp(ϕ)p (4.1)

公式(4.1)的求导比公式(3.3)更简单,相信如果看了(3.3)的手写推导,这个(4.2)一下就能看明白,所以不具体讲解了,把公式列出来:
∂ ( R p ) ∂ φ = lim ⁡ φ → 0 e x p ( φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ \frac{\partial (Rp)}{\partial \varphi }=\displaystyle \lim_{\varphi \to 0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi} φ(Rp)=φ0limφexp(φ)exp(ϕ)pexp(ϕ)p
= lim ⁡ φ → 0 ( I + φ ∧ ) e x p ( ϕ ) p − e x p ( ϕ ∧ ) p φ = lim ⁡ φ → 0 φ ∧ R p φ = \displaystyle \lim_{\varphi \to 0}\frac{(I+\varphi ^{\wedge})exp(\phi)p-exp(\phi^{\wedge})p}{\varphi} = \displaystyle \lim_{\varphi \to 0}\frac{\varphi^{\wedge}Rp}{\varphi } =φ0limφ(I+φ)exp(ϕ)pexp(ϕ)p=φ0limφφRp
= lim ⁡ φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧   ( 4.2 ) =\displaystyle \lim_{\varphi \to 0}\frac{-(Rp)^{\wedge}\varphi}{\varphi }=-(Rp)^{\wedge} \ (4.2) =φ0limφ(Rp)φ=(Rp) (4.2)

相比于直接对李代数求导,省去了一个雅可比J_{l}的计算。这使得扰动模型更为实用。

4.SE(3)上的扰动模型(左乘)

最后,我们给出了SE(3)上的扰动模型,而直接李代数上的求导就不再介绍了。
假设某空间点 p p p经过一次变换 T T T(对应李代数为 ξ \xi ξ),得到 T p Tp Tp。现在,给 T T T左乘一个扰动 Δ T = e x p ( δ ξ ∧ ) \Delta T=exp(\delta \xi^{\wedge}) ΔT=exp(δξ)
根据se(3)的定义:
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 }   ( 3.1 ) se(3)=\left\{\xi=\begin{bmatrix} \rho \\ \phi \\ \end{bmatrix} \in \texttt{R}^{6},\rho \in \texttt{R}^{3},\phi \in so(3),\xi^{\wedge}=\begin{bmatrix} \phi^{\wedge}&\rho \\ 0^{T} &0 \\ \end{bmatrix} \in \texttt{R}^{4\times4} \right\} \ (3.1) se(3)={ξ=[ρϕ]R6,ρR3,ϕso(3),ξ=[ϕ0Tρ0]R4×4} (3.1)

则扰动项的李代数为 δ ξ = [ δ ρ , δ ϕ ] T \delta \xi=[\delta \rho,\delta \phi]^{T} δξ=[δρ,δϕ]T,进行手写推导:

李代数的求导及扰动模型_第4张图片

根据公式4.2我们把最后的结果定义成一个算符 ⨀ ^{\bigodot } ,它把一个齐次坐标的空间点变换成一个 4 × 6 4 \times 6 4×6的矩阵。

至于实现李代数有什么编程的库,这个有很多,比如说《视觉SLAM十四讲》中提到的Sophus以及ROS都可以做,就是几个变换,剩下的一般结合到非线性优化以及一些求导问题,还有一些对应的库。个人觉得李群和李代数掌握这么多也就够了。

你可能感兴趣的:(3D视觉基础,线性代数)