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

2023-12-25 17:26:25

视觉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) R1?R2?∈SO(3)
T 1 T 2 ∈ S E ( 3 ) T_1T_2 \in SE(3) T1?T2?∈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(?∧)=[R0T?Jρ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

文章来源:https://blog.csdn.net/qq_43443531/article/details/135201369
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。