【视觉SLAM入门】2 旋转--李群与李代数

"川泽纳污"

    • 0. 一个例子
    • 1. 群和李群
    • 2. 李代数
      • 2.1 推导和性质
      • 2.2 s o ( 3 ) \mathscr{so(3)} so(3) s e ( 3 ) \mathscr{se(3)} se(3)
      • 2.3 计算李代数的幂    e x p ( ϕ \;exp(\phi exp(ϕ^ ) ) )
      • 2.4 李代数乘法
      • 2.5 从李代数乘法到导数:
        • 2.5.1 直接求导
        • 2.5.2 扰动模型求导(常用)
    • 3. S i m ( 3 ) Sim(3) Sim(3)与李代数

  • 本节及上一节讨论围绕这个问题展开 机器人如何表示自身位姿? \quad\large\textcolor{red}{机器人如何表示自身位姿?} 机器人如何表示自身位姿?
    注意:
  • Λ \boldsymbol {^\Lambda} Λ 记做“向量拉伸成矩阵”,(见外积)
  • V \boldsymbol {^V} V 记做“矩阵坍缩成向量”

0. 一个例子

今天所做的一切都是为了求解下边这个问题展开的! \textcolor{blue}{今天所做的一切都是为了求解下边这个问题展开的!} 今天所做的一切都是为了求解下边这个问题展开的!

\qquad 机器人位姿    T \;T T ,观察到世界坐标系中的点    p \;p p,产生观测数据    z , 误差为 w 或 e \;z, 误差为w或e z,误差为we
z = T p + w \qquad\qquad\qquad\qquad\qquad\qquad z = Tp +w z=Tp+w
\qquad 共有N个这样的观测数据和路标,我们的任务是寻找最优的T,使误差最小化
min ⁡ T J ( T ) = ∑ i = 1 N ∣ ∣ z i − T p i ∣ ∣ 2 2 \qquad\qquad\qquad\qquad\qquad\qquad \min\limits_TJ(\boldsymbol{T})=\sum\limits_{i=1}\limits^N||\boldsymbol{z_i-Tpi}||^2_2 TminJ(T)=i=1N∣∣ziTpi22

  • 最终要求解的是目标函数 J \boldsymbol{J} J关于变换矩阵 T \boldsymbol T T的导数
  • 我们经常会构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值

1. 群和李群

  • 特殊正交群与特殊欧式群:
    S O ( 3 ) \qquad \qquad \qquad SO(3) SO(3) = { R ∈ R 3 x 3 ∣ R R T = I , d e t ( R ) = 1 R \in \R^{3x3} | RR^T=I, det(R)=1 RR3x3RRT=I,det(R)=1}

S E ( 3 ) \quad \qquad \qquad SE(3) SE(3) = { T = [ R t 0 T 1 ] ∈ R 4 x 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 T= \begin{bmatrix} \quad R&t&\\\quad0^T&1 \end{bmatrix} \in \R^{4x4} | R \in SO(3), t \in \R^3 T=[R0Tt1]R4x4RSO(3),tR3}

不难发现, R 和 T R和T RT 对加法不封闭,即 R 1 + R 2 ∉ S O ( 3 ) R_1+R_2 \notin SO(3) R1+R2/SO(3)


群: 只有一个运算的集合,即一种集合( 记作    A 记作\; A 记作A )+ 一种代数结构( 记作 . 记作 . 记作.)。
群有四个性质,分别是 “封闭性、结合律、幺元、逆”
【视觉SLAM入门】2 旋转--李群与李代数_第1张图片常见的群:一般线性群    G L ( n ) \;GL(n) GL(n)(对乘法), 特殊正交群    S O ( n ) \; SO(n) SO(n) ,特殊欧式群    S E ( n ) \;SE(n) SE(n)


李群 S O ( 3 ) SO(3) SO(3)对乘法封闭,表示旋转,且连续,因为在实数空间物体的旋转是连续的。所以它是一种李群。 严谨地,李群是指具有连续(光滑)性质的群。

  • 由于 S O ( 3 ) SO(3) SO(3) S E ( 3 ) SE(3) SE(3)这两个李群对SLAM较为重要,主要讨论着两个李群。

2. 李代数

2.1 推导和性质

R R R为正交阵且 R R T = I RR^T=I RRT=I开始推导,如下(笔记潦草,勿怪):

【视觉SLAM入门】2 旋转--李群与李代数_第2张图片 \qquad\qquad\qquad 这里,可以知道 R ( t ) R ( t ) T = ϕ ( t ) R(t)R(t)^T=\phi(t) R(t)R(t)T=ϕ(t)^为反对称矩阵,上边记得 ^ 和v,分别表示把向量张成反对称矩阵(参考外积)和它的反向操作,接着对 R R R的导数进行研究

【视觉SLAM入门】2 旋转--李群与李代数_第3张图片以上可以看出,每次求导,仅需要左乘一个    ϕ ( t ) \; \phi(t) ϕ(t)^ 即可。
将R(t)进行泰勒展开:

R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) \qquad\qquad\qquad\qquad\qquad R(t) \approx R(t_0)+\dot{R}(t_0)(t-t_0) = I + \phi(t_0) R(t)R(t0)+R˙(t0)(tt0)=I+ϕ(t0)^ ( t ) (t) (t)

观察可得,这里 ϕ \phi ϕ实际反应的导数的性质。

  • 特别地,研究初始时刻    t 0 = 0 \;t_0=0 t0=0    R ( 0 ) = I \;R(0)=I R(0)=I:

R ˙ ( t ) = ϕ ( t 0 ) \qquad\qquad\qquad\qquad \dot{R}(t) = \phi(t_0) R˙(t)=ϕ(t0)^ R ( t ) = ϕ 0 R(t)=\phi_0 R(t)=ϕ0^ R ( t ) R(t) R(t)

解这个(一阶常线性)微分方程得 R ( t ) = e x p ( ϕ 0 \qquad R(t) = exp(\phi_0 R(t)=exp(ϕ0^ t ) t) t)

  • 至此, ϕ \phi ϕ就正是对应    S O ( 3 ) \;SO(3) SO(3) 上的李代数    s o ( 3 ) \; \mathscr{so(3)} so(3)。旋转矩阵R与李代数 ϕ 0 \phi_0 ϕ0 通过指数关系发生了联系,反映了R在局部的导数关系。

李代数: 记作    g \;\mathscr {g} g 描述了李群的局部性质,通用李代数定义和性质如下。定义中的二元运算被称为李括号。如,三维向量 R 3 R_3 R3上定义的叉积就是一种李代数    g = ( R 3 , R , \; \mathscr{g} = (\R^3,\R, g=(R3,R,X ) ) )

【视觉SLAM入门】2 旋转--李群与李代数_第4张图片

2.2 s o ( 3 ) \mathscr{so(3)} so(3) s e ( 3 ) \mathscr{se(3)} se(3)

so(3):三维

\qquad 因为    ϕ \;\phi ϕ 本来代表向量,但是每个R都可以生成一个矩阵,它们关系紧密,后文不区分它是v后的矩阵,还是^后的向量,择适者用。
\qquad s o ( 3 ) \mathscr{so(3)} so(3) 表示一个由三维向量组成的集合,每个向量对应到一个反对称矩阵,可以表达旋转矩阵的导数。它与 SO(3) 的关系由指数映射给定:
如下
R = e x p ( ϕ \qquad\qquad\qquad\qquad\qquad R=exp(\phi R=exp(ϕ^ ) ) )

它的李括号运算: [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) V [\phi_1,\phi_2] = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^V [ϕ1,ϕ2]=(Φ1Φ2Φ2Φ1)V

se(3):六维

也表示类似局部导数的性质,但是下文的v和^仅仅指代向量和矩阵的互换,与反对称无关。
定义如下:
在这里插入图片描述

李括号运算:
在这里插入图片描述

2.3 计算李代数的幂    e x p ( ϕ \;exp(\phi exp(ϕ^ ) ) )

so(3)的映射:
\qquad 这里称计算    e x p ( ϕ \;exp(\phi exp(ϕ^ ) ) ) 的步骤为指数映射(Exponential Map)
矩阵指数映射的计算公式: e x p ( ϕ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ) n \quad exp(\phi) = \sum\limits_{n=0}\limits^\infty{\frac{1}{n!}(\phi)^n} exp(ϕ)=n=0n!1(ϕ)n

定义 ϕ = θ a , a \phi=\theta a, a ϕ=θa,a是方向向量,模长1。推导可得:

在这里插入图片描述

和罗德里格斯公式一模一样,侧面反映了李代数 ≈ \approx 旋转向量,而李群是旋转矩阵。用迹则可以求    θ \;\theta θ    a \; a a,在    θ ∈ [ − π , π ] \;\theta \in [-\pi, \pi] θ[π,π] 内,是一一对应的。

se(3)的映射:
\quad 由上述内容,和so(3)推导一致可得
ξ Λ = [ ϕ Λ ρ 0 T 1 ] 故 e x p ( ξ Λ ) = [ R J ρ 0 T 1 ] \xi^\Lambda = \begin{bmatrix} \phi^\Lambda & \rho \\0^T &1 \end{bmatrix} \qquad故\qquad exp(\xi^\Lambda) = \begin{bmatrix} R & J\rho \\0^T &1 \end{bmatrix} ξΛ=[ϕΛ0Tρ1]exp(ξΛ)=[R0TJρ1]
\qquad 注意:旋转部分指数映射同 s o ( 3 ) \boldsymbol {so(3)} so(3),(不同才搞笑了)平移部分在做指数映射后多了一项系数矩阵    J \;\boldsymbol {J} J ,也有点像罗德里格斯公式(仍然设    ϕ = θ a \;\phi = \theta a ϕ=θa):

在这里插入图片描述
最后上一张所有推导的总结(图源《视觉SLAM14讲》)
【视觉SLAM入门】2 旋转--李群与李代数_第5张图片

2.4 李代数乘法

SO(3):
\qquad 李群乘法即李代数加法,但是矩阵的指数映射不满足常数的 e a ⋅ e b = e ( a + b ) e^a \cdot e^b = e^{(a+b)} eaeb=e(a+b)
\qquad 求解李代数指数映射乘积还得看BCH公式(Baker-Campbell-Hausdorff),如下:

l n ( e A ⋅ e B ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + . . . . . . \qquad\qquad\qquad ln(e^A\cdot e^B)=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...... ln(eAeB)=A+B+21[A,B]+121[A,[A,B]]121[B,[A,B]]+......

其中的 [ ] 表示李括号,当A或B为极小量的时候,BCH的线性近似表达如下:

在这里插入图片描述- 直观的理解上式:李群的R1和R2左乘近似(R1小)或右乘近似(R2小)而已。其实就是 Δ R ⋅ R \Delta R \cdot R ΔRR   R ⋅ Δ R \ R \cdot \Delta R  RΔR。一个微小位移。
\qquad 我们以左乘为例计算一下    J l − 1 \;J_l^{-1} Jl1

  1. 首先左乘的标注形式为:
    J l = J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a Λ \qquad\qquad\qquad J_l = J=\frac {\sin \theta}{\theta}I + (1-\frac {\sin \theta}{\theta})aa^T + \frac {1-\cos \theta}{\theta}a^\Lambda Jl=J=θsinθI+(1θsinθ)aaT+θ1cosθaΛ
  2. 计算它的逆矩阵
    J l − 1 = θ 2 cot ⁡ θ 2 ⋅ I + ( 1 − θ 2 cot ⁡ θ 2 ) a a T + θ 2 a Λ \qquad\qquad\qquad J_l^{-1}=\frac {\theta}{2}\cot \frac {\theta}{2} \cdot I+ (1-\frac {\theta}{2}\cot \frac {\theta}{2})aa^T + \frac {\theta}{2}a^\Lambda Jl1=2θcot2θI+(12θcot2θ)aaT+2θaΛ
  3. 则右乘雅克比仅对自变量取负号即可:
    J r ( ϕ ) = J l ( − ϕ ) \qquad\qquad\qquad\qquad\qquad\qquad J_r(\phi)=J_l(-\phi) Jr(ϕ)=Jl(ϕ)

SE(3)类似:
在这里插入图片描述

2.5 从李代数乘法到导数:

\qquad 参考前边的例子:我们知道,最终要求解的是目标函数 J \boldsymbol{J} J关于变换矩阵 T \boldsymbol T T的导数。从上边所有的铺垫到现在,有两种方法求解导数,分别介绍:

2.5.1 直接求导

对SO(3)
空间点p经过旋转
\qquad 便于理解的记法,设 R R R对应的李代数为 ϕ \phi ϕ
∂ ( R p ) ∂ R = ∂ ( e x p ( ϕ Λ ) p ) ∂ ϕ ⟹   − ( R p ) Λ J l \frac {\partial(Rp)} {\partial R} =\frac {\partial(exp(\phi^{\Lambda})p)} {\partial \phi} \\ \quad \\ \Longrightarrow\ -(Rp)^\Lambda J_l R(Rp)=ϕ(exp(ϕΛ)p) (Rp)ΛJl

  • 第二行中省略了很多推导步骤,包括导数定义展开、BCH线性近似、泰勒展开舍去高阶项近似、将反对称符号看做叉积,交换后变号。

2.5.2 扰动模型求导(常用)

对SO(3)
空间点p的旋转可以看成一次左扰动
\qquad 扰动记作    Δ R \;\Delta R ΔR,李代数为    φ \; \varphi φ,则
∂ ( R p ) ∂ φ = lim ⁡ φ → 0 e x p ( φ Λ ) e x p ( ϕ Λ ) p − e x p ( ϕ Λ ) p φ ⟹   − ( R p ) Λ \frac {\partial(Rp)} {\partial \varphi} =\lim_{\varphi \to 0} \frac {exp(\varphi^{\Lambda})exp(\phi^{\Lambda})p - exp(\phi^{\Lambda})p} {\varphi} \\ \quad \\ \Longrightarrow\ -(Rp)^\Lambda φ(Rp)=φ0limφexp(φΛ)exp(ϕΛ)pexp(ϕΛ)p (Rp)Λ

  • 常用,简单,在位姿估计中有重要意义

对SE(3)
直接上扰动    Δ T = e x p ( δ ξ Λ ) \;\Delta \boldsymbol T = exp(\delta \xi ^\Lambda) ΔT=exp(δξΛ),它的李代数    δ ξ = [ δ ρ , δ ϕ ] T \;\delta\boldsymbol \xi = [\delta \boldsymbol \rho, \delta \phi]^T δξ=[δρ,δϕ]T
直接上结果

∂ ( T p ) ∂ δ ξ = [ I − ( R p + t ) Λ 0 T 0 T ] = △ ( T p ) ⊙ \frac {\partial (Tp)}{\partial \delta \xi } = \begin{bmatrix} I&-(Rp+t)^\Lambda \\0^T & 0^T \end{bmatrix} =^{\triangle} (Tp)^\odot δξ(Tp)=[I0T(Rp+t)Λ0T]=(Tp)

其中 ⊙ \odot 表示,一个齐次坐标展开成4X6的矩阵

3. S i m ( 3 ) Sim(3) Sim(3)与李代数

\qquad 相似变换群( S i m Sim Sim),用于解决单目SLAM中的尺度问题,在这种情况下会显式表示出尺度    s \;s s,此时相机坐标系下的点经过相似变换(不是欧式变换):
描述该过程如下:

p ′ = [ s R t 0 T 1 ] p = s R p + t p' = \begin{bmatrix} \boldsymbol{sR}&t \\0^T&1 \end{bmatrix}p = s\boldsymbol{Rp+t} p=[sR0Tt1]p=sRp+t

S i m ( 3 ) Sim(3) Sim(3)如下:

S i m ( 3 ) = { [ S = s R t 0 T 1 ] ∈ R 4 × 4 } Sim(3) =\begin{Bmatrix} \begin{bmatrix}\boldsymbol S= \begin{matrix} \boldsymbol{sR}&t \\0^T&1 \end{matrix} \end{bmatrix}\in \R^{4\times4}\end{Bmatrix} Sim(3)={[S=sR0Tt1]R4×4}

  • 同样的,尽管多了一个维度,它仍然具有李代数(7维),指数映射,对数映射, J J J , 导数(扰动模型),以后做单目我再自行了解吧 。

你可能感兴趣的:(自动驾驶-SLAM,#,VSLAM,机器学习,人工智能,算法,SLAM,李群李代数)