视觉SLAM十四讲第四讲笔记
这一讲主要是要理解什么是 李群,李代数。 SO(3), SE(3)与对应李代数的表示方式。理解什么是BCH和 BCH近似的意义。学会在李代数上的 扰动模型。
上一讲中,已经了解到了旋转平移的表示方法,但是在SLAM中,除了表示之外,我们还需要进行估计和优化。因为在SLAM中位姿是未知的,而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。 一种典型的方式是把它构建成一个优化问题,求解最优的R,t ,使得误差最小化。
旋转矩阵自身是带有约束的(正交且行列式为1)。它们作为优化变量时,会引入额外的约束,使优化变得困难。通过李群——李代数间的转换关系,我们希望把位姿估计变成无约束的优化问题,简化求解方式。
一、什么是李群,李代数。
回顾上一讲,我们把三维旋转矩阵构成了特殊正交群 S O ( 3 ) SO(3) SO(3),把变换矩阵构成了特殊欧式群 S E ( 3 ) SE(3) SE(3):
S O ( 3 ) = { R ∈ R x × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \{R \in \R^{x \times 3} | RR^T=I, det(R)=1\} SO(3)={R∈Rx×3∣RRT=I,det(R)=1}
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \left\{ \begin{array}{c} T = \bigg[\begin{array}{c} R \quad t\\0^T \quad 1 \end{array}\bigg] \in \R^{4 \times 4} | R \in SO(3), t \in \R^3 \end{array} \right\} SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}
这里有一个性质,就是旋转矩阵也好,变换矩阵也好,它们对加法是不封闭的。也就是对于任意两个旋转矩阵
R
1
,
R
2
R_1, R_2
R1,R2,它们按照加法的定义,和不再是一个旋转矩阵,对于变换矩阵亦是如此。
R
1
+
R
2
∉
S
O
(
3
)
R_1 + R_2 \notin SO(3)
R1+R2∈/SO(3)
但是它们对乘法是封闭的:
R
1
R
2
∈
S
O
(
3
)
,
T
1
T
2
∈
S
E
(
3
)
R_1R_2 \in SO(3), \quad T_1T_2 \in SE(3)
R1R2∈SO(3),T1T2∈SE(3)
我们知道乘法对应着旋转或变换的复合——两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个 运算的集合,我们把它叫做 群。
群是一种集合加上一种运算的代数结构。 我们把集合记作 A A A,运算记作 ⋅ \cdot ⋅。那么群可以记作 G ( A , ⋅ ) G(A, \cdot) G(A,⋅)。群的运算满足以下条件:
- 封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall a_1, a_2 \in A, \quad a_1 \cdot a_2 \in A ∀a1,a2∈A,a1⋅a2∈A
- 结合律: ∀ a 1 , a 2 , a 3 ∈ A ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_1, a_2, a_3 \in A \quad (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3) ∀a1,a2,a3∈A(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
- 幺元: ∃ a 0 ∈ A , s . t . ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \exist a_0 \in A, \quad s.t. \forall a\in A, \quad a_0 \cdot a = a \cdot a_0 =a ∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a
- 逆: ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 \forall a \in A, \quad \exist a^{-1} \in A, \quad s.t. a\cdot a^{-1} = a_0 ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0
李群是指具有连续(光滑)性质的群。 S O ( n ) SO(n) SO(n)和 S E ( n ) SE(n) SE(n),它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动,所以它们都是李群。
我们再考虑一个任意旋转矩阵
R
R
R,我们知道它满足:
R
R
T
=
I
RR^T = I
RRT=I
现在,我们给旋转矩阵加个时间的性质,也就是它会随时间连续变化,即为时间的函数
R
(
t
)
R(t)
R(t)。根据旋转矩阵的性质,我们有:
R
(
t
)
R
(
t
)
T
=
I
R(t)R(t)^T = I
R(t)R(t)T=I
等式两边对时间求导可得:
R
˙
(
t
)
R
(
t
)
T
+
R
(
t
)
R
˙
(
t
)
T
=
0
\dot{R}(t) R(t)^T + R(t) \dot{R}(t)^T = 0
R˙(t)R(t)T+R(t)R˙(t)T=0
即:
R
˙
(
t
)
R
(
t
)
T
=
−
(
R
(
t
)
R
˙
(
t
)
T
)
\dot{R}(t) R(t)^T = -(R(t) \dot{R}(t)^T)
R˙(t)R(t)T=−(R(t)R˙(t)T)
也就是说,
R
˙
(
t
)
R
(
t
)
T
\dot{R}(t)R(t)^T
R˙(t)R(t)T是一个反对称矩阵。我们用上一讲的表示符号表示
∧
\land
∧
R
˙
(
t
)
R
(
t
)
T
=
ϕ
(
t
)
∧
\dot{R}(t)R(t)^T = \phi(t)^{\land}
R˙(t)R(t)T=ϕ(t)∧
式子两边右乘
R
(
t
)
R(t)
R(t),由于
R
R
R是正交矩阵,有:
R
˙
(
t
)
=
ϕ
(
t
)
∧
R
(
t
)
=
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
R
(
t
)
\dot{R}(t) = \phi(t)^{\land}R(t) = \bigg[ \begin{matrix} 0 \quad -\phi_3 \quad \phi_2 \\ \phi_3 \quad 0 \quad -\phi_1 \\ -\phi_2 \quad \phi_1 \quad 0 \end{matrix} \bigg] R(t)
R˙(t)=ϕ(t)∧R(t)=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]R(t)
可以看到,每对旋转矩阵求一次导数,只需左乘一个$ ϕ^{\land}(t)
矩
阵
即
可
。
如
果
把
矩阵即可。 如果把
矩阵即可。如果把R(t)$在0附近进行一阶泰勒展开:
R
(
t
)
≈
R
(
t
0
)
+
R
˙
(
t
0
)
(
t
−
t
0
)
=
I
+
ϕ
(
t
0
)
∧
(
t
)
R(t) \approx R(t_0) + \dot{R}(t_0)(t-t_0) = I + \phi(t_0)^{\land}(t)
R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧(t)
设,在
t
0
t_0
t0附近,
ϕ
\phi
ϕ保持为常数
ϕ
(
t
0
)
=
ϕ
0
\phi(t_0) = \phi_0
ϕ(t0)=ϕ0,
R
˙
(
t
)
=
ϕ
(
t
0
)
∧
R
(
t
)
=
ϕ
0
∧
R
(
t
)
\dot{R}(t) = \phi(t_0)^{\land}R(t) = \phi_0^{\land}R(t)
R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t)
我们知道初始值
R
(
0
)
=
I
R(0)=I
R(0)=I,所以:
R
(
t
)
=
e
x
p
(
ϕ
0
∧
t
)
R(t) = exp(\phi_0^{\land}t)
R(t)=exp(ϕ0∧t)
我们看到, 旋转矩阵 R R R与另一个反对称矩阵 ϕ 0 \phi_0 ϕ0通过指数关系发生了联系。那么给定某时刻的 R R R,我们就能求得一个 ϕ \phi ϕ。 这个 ϕ \phi ϕ正是对应到 S O ( 3 ) SO(3) SO(3)上的李代数 s o ( 3 ) so(3) so(3)。而 e x p ( ϕ ∧ ) exp(\phi^{\land}) exp(ϕ∧)正是李群与李代数间的指数/对数映射。
每个李群都有与之对应的李代数。 李代数描述了李群的局部性质。
李代数的定义 如下:
上面说的
ϕ
\phi
ϕ就是对应
S
O
(
3
)
SO(3)
SO(3)的李代数。每个
ϕ
\phi
ϕ都可以生成一个反对称矩阵:
Φ
=
ϕ
∧
=
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
∈
R
3
×
3
\Phi = \phi^{\land} = \bigg[ \begin{matrix} 0 \quad -\phi_3 \quad \phi_2 \\ \phi_3 \quad 0 \quad -\phi_1 \\ -\phi_2 \quad \phi_1 \quad 0 \end{matrix} \bigg] \in \R^{3 \times 3}
Φ=ϕ∧=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]∈R3×3
在这个定义下,两个向量
ϕ
1
,
ϕ
2
\phi_1, \phi_2
ϕ1,ϕ2的李括号为:
[ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ [\phi_1, \phi_2] = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^{\vee} [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨
s
o
(
3
)
so(3)
so(3)的元素是3维向量或者3维反对称矩阵。 它们是一个由三维向量组成的集合,每个向量对应到一个反对称矩阵,可以表达旋转矩阵的导数。
s
o
(
3
)
=
{
ϕ
∈
R
3
,
Φ
=
ϕ
∧
∈
R
3
×
3
}
so(3) = \{\phi \in \R^3, \Phi=\phi^{\land} \in \R^{3 \times 3} \}
so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
现在讲了
S
O
(
3
)
SO(3)
SO(3),特殊欧式群
S
E
(
3
)
SE(3)
SE(3)的李代数同理可得:
s
e
(
3
)
=
{
ϵ
=
[
ρ
ϕ
]
∈
R
6
,
ρ
∈
R
3
,
ϕ
∈
s
o
(
3
)
,
ϵ
∧
=
[
ϕ
∧
ρ
0
T
0
]
∈
R
4
×
4
}
se(3) = \left \{ \epsilon=\bigg[ \begin{matrix} \rho \\ \phi \end{matrix}\bigg] \in \R^6, \rho \in \R^3, \phi \in so(3), \epsilon^{\land}= \bigg[ \begin{matrix} \phi^{\land} \quad \rho \\ 0^T \quad 0 \end{matrix}\bigg] \in \R^{4 \times 4} \} \right.
se(3)={ϵ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ϵ∧=[ϕ∧ρ0T0]∈R4×4}
我们把每个
s
e
(
3
)
se(3)
se(3)元素记作
ϵ
\epsilon
ϵ,它是一个六维向量。前三维为平移,记作
ρ
\rho
ρ;后三维为旋转,记作
ϕ
\phi
ϕ, 实质上是
s
o
(
3
)
so(3)
so(3)元素。同时,我们拓展了
∧
\land
∧符号的含义。在
s
e
(
3
)
se(3)
se(3)中, 同样使用
∧
\land
∧符号,将一个六维向量转换成四维矩阵,但这里不再表示反对称:
ϵ
∧
=
[
ϕ
∧
ρ
0
T
0
]
∈
R
4
×
4
\epsilon^{\land}= \bigg[ \begin{matrix} \phi^{\land} \quad \rho \\ 0^T \quad 0 \end{matrix}\bigg] \in \R^{4 \times 4}
ϵ∧=[ϕ∧ρ0T0]∈R4×4
我们仍使用
∧
\land
∧和
∨
\vee
∨符号来指代“从向量到矩阵”和“从矩阵到向量”的关系。
ϵ
1
,
ϵ
2
\epsilon_1, \epsilon_2
ϵ1,ϵ2的李括号为:
[ ϵ 1 , ϵ 2 ] = ( ϵ 1 ∧ ϵ 2 ∧ − ϵ 2 ∧ ϵ 1 ∧ ) ∨ [\epsilon_1, \epsilon_2] = (\epsilon_1^{\land}\epsilon_2^{\land} - \epsilon_2^{\land}\epsilon_1^{\land})^{\vee} [ϵ1,ϵ2]=(ϵ1∧ϵ2∧−ϵ2∧ϵ1∧)∨
二、指数与对数的映射
1. SO(3)上的指数映射
上面提到, e x p ( ϕ ∧ ) exp(\phi^{\land}) exp(ϕ∧)是 S O ( 3 ) SO(3) SO(3)上的指数映射。那怎么计算呢?
任意矩阵的指数映射可以写成一个泰勒展开,但是只有在收敛的情况下才会有结果,其结果仍是一个矩阵。
e
x
p
(
A
)
=
∑
n
=
0
∞
1
n
!
A
n
exp(A) = \sum_{n=0}^{\infty}\frac{1}{n!}A^n
exp(A)=n=0∑∞n!1An
所以,根据上面公式,我们有:
e
x
p
(
ϕ
∧
)
=
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
exp(\phi^{\land}) = \sum_{n=0}^{\infty}\frac{1}{n!}({\phi^{\land}})^n
exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n
假设我们用
θ
,
a
\theta,a
θ,a分别定义
ϕ
\phi
ϕ的模长和方向,我们就可以用
ϕ
=
θ
a
\phi=\theta a
ϕ=θa表示。这里的
a
a
a是一个长度为1的方向向量。首先对于
a
∧
a^{\land}
a∧有以下两个性质:
a
∧
a
∧
=
a
a
T
−
I
a
∧
a
∧
a
∧
=
−
a
∧
a^{\land}a^{\land} = aa^T - I \\ a^{\land}a^{\land}a^{\land}=-a^{\land}
a∧a∧=aaT−Ia∧a∧a∧=−a∧
所以我们把上面的指数映射转换为:
也就是 罗德里格斯公式。
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
∧
R = cos\theta I + (1-cos \theta) nn^T + sin \theta n^{\land}
R=cosθI+(1−cosθ)nnT+sinθn∧
这表明,
s
o
(
3
)
so(3)
so(3)实际上就是由所谓的旋转向量组成的空间,而指数映射即罗德里格斯公式。 通过它们,我们把
s
o
(
3
)
so(3)
so(3)中任意一个向量对应到了一个位于
S
O
(
3
)
SO(3)
SO(3)中的旋转矩阵。
2. SE(3)上的指数映射
同理我们可以推导出
s
e
(
3
)
se(3)
se(3)上的指数映射。
e
x
p
(
ϵ
∧
)
=
[
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
ρ
0
T
1
]
≜
[
R
J
ρ
0
T
1
]
=
T
\begin{aligned} exp(\epsilon^{\land}) &= \bigg[ \begin{matrix} \begin{aligned} \sum_{n=0}^{\infty}\frac{1}{n!}&(\phi^{\land})^n \quad \sum_{n=0}^{\infty}\frac{1}{(n+1)!}(\phi^{\land})^n \rho \\ &0^T \quad \quad \quad \quad \quad \quad 1 \end{aligned} \end{matrix}\bigg] \\ & \triangleq \bigg[ \begin{matrix} R \quad J\rho \\ 0^T \quad 1 \end{matrix}\bigg] = T \end{aligned}
exp(ϵ∧)=[n=0∑∞n!1(ϕ∧)nn=0∑∞(n+1)!1(ϕ∧)nρ0T1]≜[RJρ0T1]=T
ϵ
\epsilon
ϵ的指数映射左上角的
R
R
R是我们熟知的欧式正交群
S
O
(
3
)
SO(3)
SO(3)中的元素,与李代数
s
e
(
3
)
se(3)
se(3)当中的旋转部分
ϕ
\phi
ϕ对应。 而右上角的
J
J
J则可整理为(
ϕ
=
θ
a
\phi=\theta a
ϕ=θa):
J
=
s
i
n
θ
θ
I
+
(
1
−
s
i
n
θ
θ
)
a
a
T
+
1
−
c
o
s
θ
θ
a
∧
J = \frac{sin\theta}{\theta}I + (1-\frac{sin\theta}{\theta})aa^T + \frac{1-cos\theta}{\theta}a^{\land}
J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
总结如下:
三、李代数求导与扰动模型
1. BCH公式与近似形式
使用李代数的一大动机是为了进行优化,而在优化过程中导数是非常必要的信息。 但是,当我们在
S
O
(
3
)
SO(3)
SO(3)中完成两个矩阵乘法时, 李代数中
s
o
(
3
)
so (3)
so(3)上发生了什么改变呢?反过来说,当
s
o
(
3
)
so(3)
so(3)上做两个李代数的加法时,
S
O
(
3
)
SO(3)
SO(3)上是否对应着两个矩阵的乘积?
两个李代数指数映射乘积的完整形式, 由Baker-Campbell-Hausdorf公式BCH公式给出 由于它完整的形式较复杂,我们给出它展开式的前几项:
l
n
(
e
x
p
(
A
)
e
x
p
(
B
)
)
=
A
+
B
+
1
2
[
A
,
B
]
+
1
12
[
A
,
[
A
,
B
]
]
−
1
12
[
B
,
[
A
,
B
]
]
+
.
.
.
ln(exp(A)exp(B)) = A + B + \frac{1}{2}[A, B] + \frac{1}{12}[A, [A, B]] - \frac{1}{12}[B, [A,B]] + ...
ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+...
其中 [] 为李括号。BCH公式告诉我们,当处理两个矩阵指数之积时,它们会产生一些由李括号组成的余项。特别地,考虑
S
O
(
3
)
SO(3)
SO(3)上的李代数
l
n
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
ln(exp(\phi_1^{\land})exp(\phi_2^{\land}))^{\vee}
ln(exp(ϕ1∧)exp(ϕ2∧))∨,当
ϕ
1
\phi_1
ϕ1或
ϕ
2
\phi_2
ϕ2为小量时,小量二次以上的项都可以被忽略掉。此时,BCH 拥有线性近似表达:
l
n
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
i
f
ϕ
1
is small
,
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
i
f
ϕ
2
is small
,
ln(exp(\phi_1^{\land})exp(\phi_2^{\land}))^{\vee} \approx \left \{ \begin{matrix} J_l(\phi_2)^{-1}\phi_1+\phi_2 \quad if \phi_1 \text{is small},\\ J_r(\phi_1)^{-1}\phi_2+\phi_1 \quad if \phi_2 \text{is small}, \end{matrix} \right.
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2ifϕ1is small,Jr(ϕ1)−1ϕ2+ϕ1ifϕ2is small,
以第一个近似为例。该式告诉我们,当对一个旋转矩阵
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近似下,分成了左乘近似和右乘近似两种。
以左乘为例,左乘BCH近似雅可比
J
l
J_l
Jl:
J
l
=
J
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
J_l = J = \frac{\sin \theta}{\theta} I + (1-\frac{\sin \theta}{\theta})aa^T + \frac{1-\cos\theta}{\theta}a^{\land}
Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
它的逆为:
J
l
−
1
=
θ
2
c
o
t
θ
2
I
+
(
1
−
θ
2
c
o
t
θ
2
)
a
a
T
−
θ
2
a
∧
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^{\land}
Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧
而右乘雅可比仅需要对自变量取负号即可:
J
r
(
ϕ
)
=
J
L
(
−
ϕ
)
J_r(\phi) = J_L(-\phi)
Jr(ϕ)=JL(−ϕ)
我们就可以谈论李群乘法与李代数加法的关系了。
假定对某个旋转
R
R
R,对应的李代数为
ϕ
\phi
ϕ。我们给它左乘一个微小旋转,记作
△
R
\triangle R
△R,对应的李代数为
△
ϕ
\triangle \phi
△ϕ。 那么,在李群上,得到的结果就是
△
R
⋅
R
\triangle R \cdot R
△R⋅R,而在李代数上,根据BCH近似,为:
J
l
−
1
(
ϕ
)
△
ϕ
+
ϕ
J_l^{-1}(\phi) \triangle \phi + \phi
Jl−1(ϕ)△ϕ+ϕ。合并起来,可以简单地写成:
e
x
p
(
△
ϕ
∧
)
e
x
p
(
ϕ
∧
)
=
e
x
p
(
(
ϕ
+
J
l
−
1
(
ϕ
)
△
ϕ
)
∧
)
exp(\triangle \phi^{\land})exp(\phi^{\land}) = exp((\phi + J_l^{-1}(\phi)\triangle \phi)^{\land})
exp(△ϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)△ϕ)∧)
反之,如果我们在李代数上进行加法,让一个
ϕ
\phi
ϕ加上
Δ
ϕ
\Delta \phi
Δϕ,那么可以近似为李群上带左右雅可比的乘法:
e
x
p
(
(
ϕ
+
Δ
ϕ
)
∧
)
=
e
x
p
(
(
J
l
Δ
ϕ
)
∧
)
e
x
p
(
ϕ
∧
)
=
e
x
p
(
ϕ
∧
)
e
x
p
(
J
r
Δ
ϕ
)
∧
)
exp((\phi+\Delta \phi)^{\land}) = exp((J_l\Delta\phi)^{\land})exp(\phi^{\land})=exp(\phi^{\land})exp(J_r\Delta\phi)^{\land})
exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp(JrΔϕ)∧)
同理对于
S
E
(
3
)
SE(3)
SE(3),有类似的BCH近似公式:
e
x
p
(
Δ
ϵ
∧
)
e
x
p
(
ϵ
∧
)
≈
e
x
p
(
(
J
l
−
1
Δ
ϵ
+
ϵ
)
∧
)
e
x
p
(
ϵ
∧
)
e
x
p
(
Δ
ϵ
∧
)
≈
e
x
p
(
(
J
r
−
1
Δ
ϵ
+
ϵ
)
∧
)
exp(\Delta \epsilon^{\land})exp(\epsilon^{\land}) \approx exp((J^{-1}_l \Delta \epsilon + \epsilon)^{\land}) \\ exp(\epsilon^{\land})exp(\Delta \epsilon^{\land}) \approx exp((J^{-1}_r \Delta \epsilon + \epsilon)^{\land})
exp(Δϵ∧)exp(ϵ∧)≈exp((Jl−1Δϵ+ϵ)∧)exp(ϵ∧)exp(Δϵ∧)≈exp((Jr−1Δϵ+ϵ)∧)
2. SO(3)李代数上的求导
在SLAM中,我们要估计一个相机的位置和姿态,该位姿是由
S
O
(
3
)
SO(3)
SO(3)上的旋转矩阵或
S
E
(
3
)
SE(3)
SE(3)上的变换矩阵描述的。假设机器人的某个时刻位姿为
T
T
T。它观察到了一个世界坐标位于
P
P
P的点,产生了一个观测数据
z
z
z。由变换关系可知:
z
=
T
p
+
w
z=Tp + w
z=Tp+w
由于观测噪声
w
w
w的存在,
z
z
z往往不可能精确地满足
z
=
T
p
z=Tp
z=Tp的关系。所以,我们通常会计算理想的观测与实际数据的误差:
e
=
z
−
T
p
e = z - Tp
e=z−Tp
假设有
N
N
N个这样的路标点和观测,于是就有
N
N
N个上式。那么对于机器人的位姿估计,相当于是找一个最优的
T
T
T,使得整体误差最小化:
min
T
J
(
T
)
=
∑
i
=
1
N
∣
∣
z
i
−
T
p
i
∣
∣
2
2
\min_TJ(T) = \sum_{i=1}^N ||z_i-Tp_i||^2_2
TminJ(T)=i=1∑N∣∣zi−Tpi∣∣22
求解此问题,需要计算目标函数 J J J关于变换矩阵 T T T的导数。这里重点要说的是,我们经常会构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。 然而, S O ( 3 ) SO(3) SO(3), S E ( 3 ) SE(3) SE(3)上并没有良好定义的加法,它们只是群。如果我们把 T T T当成一个普通矩阵来处理优化,那就必须对它加以约束。而从李代数角度来说,由于李代数由向量组成,具有良好的加法运算。因此,使用李代数解决求导问题的思路分为两种:
- 用李代数表示姿态,然后对根据李代数加法来对李代数求导。
- 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型。
第一种方式对应到李代数的求导模型,而第二种则对应到扰动模型。
首先考虑李代数的求导:
考虑
S
O
(
3
)
SO(3)
SO(3)上的情况。设旋转矩阵
R
R
R对应的李代数为
ϕ
\phi
ϕ:
∂
(
e
x
p
(
ϕ
∧
)
p
)
∂
ϕ
=
lim
δ
ϕ
→
0
e
x
p
(
(
ϕ
+
δ
ϕ
)
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
δ
ϕ
=
lim
δ
ϕ
→
0
e
x
p
(
(
J
l
δ
ϕ
)
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
δ
ϕ
≈
lim
δ
ϕ
→
0
(
I
+
(
J
l
δ
ϕ
)
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
δ
ϕ
=
lim
δ
ϕ
→
0
(
J
l
δ
ϕ
)
∧
e
x
p
(
ϕ
∧
)
p
δ
ϕ
=
lim
δ
ϕ
→
0
−
(
e
x
p
(
ϕ
∧
)
p
)
∧
J
l
δ
ϕ
δ
ϕ
=
−
(
R
p
)
∧
J
l
\begin{aligned} \frac{\partial (exp(\phi^{\land})p)}{\partial \phi} &=\lim_{\delta \phi \rightarrow0}\frac{exp((\phi + \delta \phi)^{\land})p-exp(\phi ^{\land})p}{\delta \phi} \\ &=\lim_{\delta \phi \rightarrow0}\frac{exp((J_l\delta \phi)^{\land})exp(\phi ^{\land})p - exp(\phi ^{\land})p}{\delta \phi} \\ & \approx \lim_{\delta \phi \rightarrow0}\frac{(I + (J_l\delta \phi)^{\land})exp(\phi ^{\land})p - exp(\phi ^{\land})p}{\delta \phi} \\ &= \lim_{\delta \phi \rightarrow0}\frac{(J_l\delta \phi)^{\land}exp(\phi ^{\land})p}{\delta \phi} \\ &= \lim_{\delta \phi \rightarrow0}\frac{-(exp(\phi^{\land})p)^{\land}J_l \delta \phi}{\delta \phi} = -(Rp)^{\land}J_l \end{aligned}
∂ϕ∂(exp(ϕ∧)p)=δϕ→0limδϕexp((ϕ+δϕ)∧)p−exp(ϕ∧)p=δϕ→0limδϕexp((Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p≈δϕ→0limδϕ(I+(Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p=δϕ→0limδϕ(Jlδϕ)∧exp(ϕ∧)p=δϕ→0limδϕ−(exp(ϕ∧)p)∧Jlδϕ=−(Rp)∧Jl
第二行的近似为BCH线性近似,第三行为泰勒展开舍去高阶项后近似,第四行至第五行将反对称符号看作叉积,交换之后变号。于是,我们推导了旋转后的点相对于李代数的导数:
∂
R
p
∂
ϕ
=
(
−
R
p
)
∧
J
l
\frac{\partial Rp}{\partial \phi} = (-Rp)^{\land} J_l
∂ϕ∂Rp=(−Rp)∧Jl
不过,由于这里仍然含有形式比较复杂的 J l J_l Jl,我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。
再来看一下扰动模型(左乘)
另一种求导方式,是对
R
R
R进行一次扰动
△
R
\triangle R
△R。这个扰动可以乘在左边也可以乘在右边,最后结果会有一点儿微小的差异,我们以左扰动为例。设左扰动
△
R
\triangle R
△R对应的李代数为
φ
\varphi
φ。然后,对
φ
\varphi
φ求导,即:
∂
(
R
p
)
∂
φ
=
lim
φ
→
0
e
x
p
(
φ
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
φ
≈
lim
φ
→
0
(
I
+
φ
∧
)
e
x
p
(
ϕ
∧
)
p
−
e
x
p
(
ϕ
∧
)
p
φ
=
lim
φ
→
0
φ
∧
R
p
φ
=
lim
φ
→
0
−
(
R
p
)
∧
φ
φ
=
−
(
R
p
)
∧
\begin{aligned} \frac{\partial (Rp)}{\partial \varphi} &= \lim_{\varphi \rightarrow0}\frac{exp(\varphi^{\land})exp(\phi ^{\land})p-exp(\phi ^{\land})p}{\varphi} \\ &\approx \lim_{\varphi \rightarrow0}\frac{(I+\varphi^{\land})exp(\phi ^{\land})p-exp(\phi ^{\land})p}{\varphi} \\ &=\lim_{\varphi \rightarrow0}\frac{\varphi^{\land}Rp}{\varphi} = \lim_{\varphi \rightarrow0}\frac{-(Rp)^{\land}\varphi}{\varphi}=-(Rp)^{\land} \end{aligned}
∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧
可见,扰动模型相比于直接对李代数求导,省去了一个雅可比
J
l
J_l
Jl的计算。