视觉SLAM十四讲|【二】李群与李代数
李群与李代数基础
群的性质
- 封闭性： ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall a_1, a_2 \in A, 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, (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, s.t. \forall a\in A, 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, \exist a^{-1} \in A, s.t. a \cdot a^{-1} = a_0 ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0
R
1
R
2
∈
S
O
(
3
)
R_1R_2 \in SO(3)
R1R2∈SO(3)
T
1
T
2
∈
S
E
(
3
)
T_1T_2 \in SE(3)
T1T2∈SE(3)
李代数的引出
定义
设一个三维向量
ϕ
(
t
)
∈
R
3
\phi(t)\in \mathbb{R^3}
ϕ(t)∈R3，有
ϕ
(
t
)
∧
=
R
˙
(
t
)
R
(
t
)
T
\phi(t)^{\land} = \dot{R}(t)R(t)^T
ϕ(t)∧=R˙(t)R(t)T
注意，
∧
\land
∧为矩阵化符号，得到一关于
ϕ
\phi
ϕ的反对称矩阵，因此该等式要求
R
˙
(
t
)
R
(
t
)
T
\dot{R}(t)R(t)^T
R˙(t)R(t)T为反对称矩阵。
证明
三维向量
ϕ
(
t
)
\phi(t)
ϕ(t)有什么意义呢？我们要联系一下旋转向量的性质：
R
R
T
=
I
RR^T = I
RRT=I
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
=
−
(
R
˙
(
t
)
R
(
t
)
T
)
T
\dot{R}(t)R(t)^T= - (\dot{R}(t)R(t)^T)^T
R˙(t)R(t)T=−(R˙(t)R(t)T)T
得证
R
˙
(
t
)
R
(
t
)
T
\dot{R}(t)R(t)^T
R˙(t)R(t)T为反对称矩阵。
性质
ϕ
(
t
)
∧
=
R
˙
(
t
)
R
(
t
)
T
\phi(t)^{\land} = \dot{R}(t)R(t)^T
ϕ(t)∧=R˙(t)R(t)T
两边同时右乘
R
(
t
)
R(t)
R(t)，有
ϕ
(
t
)
∧
R
(
t
)
=
R
˙
(
t
)
R
(
t
)
T
R
(
t
)
\phi(t)^{\land}R(t) = \dot{R}(t)R(t)^TR(t)
ϕ(t)∧R(t)=R˙(t)R(t)TR(t)
由于
R
(
t
)
T
R
(
t
)
=
I
R(t)^TR(t)=I
R(t)TR(t)=I
得到
ϕ
(
t
)
∧
R
(
t
)
=
R
˙
(
t
)
\phi(t)^{\land}R(t) = \dot{R}(t)
ϕ(t)∧R(t)=R˙(t)
即对
ϕ
(
t
)
∧
\phi(t)^{\land}
ϕ(t)∧通过右乘
R
(
t
)
R(t)
R(t)，或者对
R
(
t
)
R(t)
R(t)左乘
ϕ
(
t
)
∧
\phi(t)^{\land}
ϕ(t)∧，我们可以实现对旋转矩阵的求导
R
˙
(
t
)
=
ϕ
(
t
)
∧
R
(
t
)
\dot{R}(t) =\phi(t)^{\land}R(t)
R˙(t)=ϕ(t)∧R(t)
设
t
=
0
t=0
t=0时有
R
(
0
)
=
I
R(0)=I
R(0)=I，
t
0
=
0
t_0=0
t0=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)
又有
R
˙
(
t
)
=
ϕ
(
t
0
)
∧
R
(
t
)
\dot{R}(t)=\phi(t_0)^{\land}R(t)
R˙(t)=ϕ(t0)∧R(t)
解该微分方程，可以得到四元数与旋转矩阵的关系如下所示
R
(
t
)
=
e
x
p
(
ϕ
0
∧
t
)
R(t) = exp(\phi_0^{\land}t)
R(t)=exp(ϕ0∧t)
指数与对数映射
SO(3)上的指数映射
对于三维向量
ϕ
\phi
ϕ，我们令
ϕ
=
θ
a
\phi=\theta a
ϕ=θa，
θ
\theta
θ为模长，
a
a
a为单位方向向量，有以下性质：
a
∧
a
∧
=
a
a
T
−
I
a^{\land}a^{\land} = aa^T-I
a∧a∧=aaT−I
a
∧
a
∧
a
∧
=
−
a
∧
a^{\land}a^{\land}a^{\land} =-a^{\land}
a∧a∧a∧=−a∧
e
x
p
(
ϕ
∧
)
=
e
x
p
(
θ
a
∧
)
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
a
a
T
+
s
i
n
θ
a
∧
exp(\phi^{\land})=exp(\theta a^{\land}) = cos\theta I + (1-cos\theta)aa^T + sin\theta a^{\land}
exp(ϕ∧)=exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧
反回来，旋转向量的对数映射有
ϕ
=
l
n
(
R
)
∨
=
(
∑
n
=
0
(
−
1
)
n
n
+
1
(
R
−
I
)
n
+
1
)
∨
\phi = ln(R)^{\vee} = (\sum_{n=0}\frac{(-1)^n}{n+1}(R-I)^{n+1})^{\vee}
ϕ=ln(R)∨=(n=0∑n+1(−1)n(R−I)n+1)∨
SE(3)上的指数映射
e
x
p
(
ϵ
∧
)
=
[
R
J
ρ
0
T
1
]
=
T
exp(\epsilon^{\land})=\begin{bmatrix} R & J\rho \\ 0^T & 1 \end{bmatrix} = T
exp(ϵ∧)=[R0TJρ1]=T
其中，
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
\phi_1
ϕ1为小量，有
l
n
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
≈
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
ln(exp(\phi_1^{\land})exp(\phi_2^{\land}))^{\vee} \approx J_l(\phi_2)^{-1}\phi_1 + \phi_2
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈Jl(ϕ2)−1ϕ1+ϕ2
ϕ
2
\phi_2
ϕ2为小量，有
l
n
(
e
x
p
(
ϕ
1
∧
)
e
x
p
(
ϕ
2
∧
)
)
∨
≈
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
ln(exp(\phi_1^{\land})exp(\phi_2^{\land}))^{\vee} \approx J_r(\phi_1)^{-1}\phi_2 + \phi_1
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈Jr(ϕ1)−1ϕ2+ϕ1
J l = s i n θ θ I + ( 1 − s i n θ θ ) a a T + 1 − c o s θ θ a ∧ J_l = \frac{sin\theta}{\theta}I + (1- \frac{sin\theta}{\theta})aa^T + \frac{1-cos\theta}{\theta}a^{\land} Jl=θ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(−ϕ)
补充性质
l
n
(
R
e
x
p
(
ϕ
∧
)
)
∨
=
l
n
(
R
)
∨
+
J
r
−
1
ϕ
ln(Rexp(\phi^{\land}))^{\vee}=ln(R)^{\vee}+J_r^{-1}\phi
ln(Rexp(ϕ∧))∨=ln(R)∨+Jr−1ϕ
SO(3)伴随性质：
R
T
e
x
p
(
ϕ
∧
)
R
=
e
x
p
(
(
R
T
ϕ
)
∧
)
R^Texp(\phi^{\land})R = exp((R^T\phi)^{\land})
RTexp(ϕ∧)R=exp((RTϕ)∧)
a
∧
b
=
−
b
∧
a
a^{\land} b = -b^{\land}a
a∧b=−b∧a
831
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
