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=1∑n∥∥∥∥ui−si1KTPi∥∥∥∥2 (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, 当ϕ1为小量,Jr(ϕ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+θ1−cosθ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)
Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θ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
ΔR⋅R。而在李代数上,根据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((Jl−1(ϕ)Δϕ+ϕ)∧) (2.6)
根据式子2.7近似为李群上带左右雅可比的乘法
在视觉slam上直接截的图,4.36=2.8,4.37=2.9,参考文献[6]为 T. Barfoot,State estimation for robotics:A matrix lie group approach,2016.
使用李代数进行求导的思路分为两种:
- 用李代数表示姿态,然后根据李代数加法对李代数求导。
- 对李群左乘或右乘微小扰动,然后对该扰动求导,成为左扰动和右扰动模型。
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)仍然含有形式比较复杂的 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(ϕ∧)p−exp(ϕ∧)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(ϕ∧)p−exp(ϕ∧)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(ϕ)p−exp(ϕ∧)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.2我们把最后的结果定义成一个算符 ⨀ ^{\bigodot } ⨀,它把一个齐次坐标的空间点变换成一个 4 × 6 4 \times 6 4×6的矩阵。
至于实现李代数有什么编程的库,这个有很多,比如说《视觉SLAM十四讲》中提到的Sophus以及ROS都可以做,就是几个变换,剩下的一般结合到非线性优化以及一些求导问题,还有一些对应的库。个人觉得李群和李代数掌握这么多也就够了。