视觉SLAM十四讲|【二】李群与李代数

视觉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