半闲居士视觉SLAM十四讲笔记(4)李群与李代数 - part 2 指数与对数映射、李代数求导与扰动模型

本系列文章由 youngpan1101 出品,转载请注明出处。
文章链接: http://blog.csdn.net/youngpan1101/article/details/71087919
作者:宋洋鹏(youngpan1101)
邮箱: [email protected]



该讲详细资料下载链接 【Baidu Yun】【Video】【Code】

若您觉得本博文对您有帮助,请支持高博的新书《视觉SLAM十四讲》,【点击购买】
若您觉得本博文对您有帮助,请支持高博的新书《视觉SLAM十四讲》,【点击购买】
若您觉得本博文对您有帮助,请支持高博的新书《视觉SLAM十四讲》,【点击购买】


李群与李代数

指数与对数映射

SO(3) 上的指数映射

  • 李代数 so(3) 到 李群 SO(3) 的映射关系:
    R=exp(ϕ)(4.17)
  • 任意矩阵的指数映射在收敛情况下可以进行
    exp(A)=i=01n!An(4.18)

    so(3) 中任意 ϕ ,也可以通过泰勒展开来定义它的指数映射:
    exp(ϕ)=n=01n!(ϕ)n(4.19)
  • ϕ 是三维向量,其模长和方向分别记作 θ a ,有 ϕ=θa a 是长度为 1 的方向向量。关于 a 有两条性质:
    {aa=aaTIaaa=a(4.20)

    利用式 (4.20) 的 a 高阶项的性质可以将式 (4.19) 写成:
    exp(ϕ)=exp(θa)=n=01n!(θa)n=I+θa+12!θ2aa+13!θ3aaa+14!θ4(a)4+=aaTaa+θa+12!θ2aa13!θ3a14!θ4(a)2+=aaT+(θ13!θ3+15!θ5)a(112!θ2+14!θ4)aa=aa+I+sinθacosθaa=(1cosθ)aa+I+sinθa=cosθI+(1cosθ)aaT+sinθa(4.21)

    整理得到:
    exp(θa)=cosθI+(1cosθ)aaT+sinθa(4.22)

    式 (4.22) 与罗德里格斯公式是一样的,说明 so(3)
  • 给定旋转矩阵 R 时,亦能求李代数 so(3) 对应的元素:

    ϕ=ln(R)=(n=0(1)nn+1(RI)n+1)(4.23)

    但实际当中没必要这样求,在旋转向量小节已经介绍了旋转矩阵旋转向量的转换关系:
    θ=arccos(tr(R)12),Rn=n

  • 指数映射只是一个

    每个 SO(3) 中的元素都可以找到一个 so(3) 元素与之对应;但有可能存在多个 so(3) 中的元素对应同一个 SO(3)

SE(3) 上的指数映射

  • se(3) SE(3) 的指数映射:
    exp(ξ)=n=01n!(ϕ)n0Tn=01(n+1)!(ϕ)nρ1=[R0TJρ1]=T(4.24)

    右上角的 J 可整理为:
    J=sinθθI+(1sinθθ)aaT+1cosθθa(4.25)

SO(3),SE(3),so(3),se(3) 的对应关系

李群 映射 李代数


李群 SO(3)
RR3×3
RRT=I
det(R)=1
三维旋转

exp(θa)=cosθI+(1cosθ)aaT+sinθa

θ=arccos(tr(R)12),Ra=a

李代数 so(3)
ϕR3
ϕ=0ϕ3ϕ2ϕ30ϕ1ϕ2ϕ10


李群 SE(3)
TR4×4
T=[R0Tt1]
三维变换

exp(ξ)=[exp(ϕ)0TJρ1]
J=sinθθI+(1sinθθ)aaT+1cosθθa

θ=arccos(tr(R)12),Ra=a,t=Jρ

李代数 se(3)
ξ=[ρϕ]R6
ξ=[ϕ0Tρ0]

——————————– 分割线<< >>分割线 ——————————–


李代数求导与扰动模型

BCH 公式与近似形式

  • 相机位姿估计优化过程中 是非常必要的,李群元素只有乘法(没有加法),无从定义导数,这也是 使
  • so(3) 上做两个李代数的加法时, SO(3) 上是否对应着两个矩阵的乘积? 相当于:
    exp(ϕ1)exp(ϕ2)=exp((ϕ1+ϕ2))(4.26)

     (4.26) ϕ ,两个李代数指数映射乘积的完整形式由 Baker-Campbell-Hausdorff 公式( BCH )给出,由于完整形式复杂,这里给出展开式的前几项:
    ln(exp(A)exp(B))=A+B+12[A,B]+112[A,[A,B]]112[B,[A,B]]+(4.27)

    式 (4.27) 中的 [,] 为李括号。当处理两个矩阵指数之积时,会产生一些李括号组成的余项。
    考虑 SO(3) 上的李代数 ln(exp(ϕ1)exp(ϕ2)) , 当 ϕ1 ϕ2 时, BCH 具有线性近似形式
    ln(exp(ϕ1)exp(ϕ2)){[Jl(ϕ2)]1ϕ1+ϕ2[Jr(ϕ1)]1ϕ2+ϕ1ifϕ1issmallifϕ2issmall(4.28)

    由式 (4.28) 中的第一个近似可知,当对一个旋转矩阵 R2 (李代数为 ϕ2 )左乘一个微小旋转矩阵 R1 (李代数为 ϕ1 )时,可以近似认为在原有的李代数 ϕ2 上,加上了一项 Jl(ϕ2)1ϕ1
    • 式 (4.28) 中
      Jl=J=sinθθI+(1sinθθ)aaT+1cosθθa(4.29)

      它的逆为:
      J1l=θ2cotθ2I+(1θ2cotθ2)aaTθ2a(4.30)
    • 式 (4.28) 中
      Jr(ϕ)=Jl(ϕ)(4.31)
  • 直观写法
    • 设某个旋转 R 对应的李代数为 ϕ ,给该旋转 一个微小的旋转 ΔR (对应的李代数为 Δϕ ), 李群上的 ΔRR 对应了李代数上的 BCH 近似值:
      exp(Δϕ)exp(ϕ)ΔRR=expϕ+[Jl(ϕ)]1Δϕ(4.32)

      在李群上左乘小量时,李代数上的加法相差左雅可比的逆。
    • 反之,李代数上让 ϕ+Δϕ ,可近似为李群上带左右雅可比的乘法:
      exp[(ϕ+Δϕ)]=exp[(ϕ+[Jl(ϕ)]1[Jl(ϕ)]Δϕ)]=exp[(JlΔϕ)]exp(ϕ)=exp(ϕ)exp[(JrΔϕ)](4.33)

      李代数上进行小量加法时,相当于李群上左(右)乘一个带左(右)雅可比的量。
    • 对于 SE(3) ,亦有类似的 BCH 近似公式:
      exp(Δξ)exp(ξ)exp[(J1lΔξ+ξ)]exp(ξ)exp(Δξ)exp[(J1rΔξ+ξ)](4.34)

      式 (4.34) 中的 Jl,Jr 形式比较复杂,是 6×6 的矩阵。(鉴于在计算中 该雅可比,这里略去它的实际形式)

SO(3) 李代数上的求导

  • 相机位姿估计问题描述
    设相机某时刻的位姿为 T ,观测到位于世界坐标系的点 p ,产生一个观测数据 z ,由坐标变换得:
    z=Tp+w(4.35)

    式 (4.35) 中的 w 为观测噪声,该噪声使得 z=Tp 一般不能精确地满足,故计算实际数据与理想观测值的误差:
    e=zTp(4.36)

    如果共有 N 个路标点和观测,对相机的位姿估计就是寻找最优的 T 使得整体误差最小化:
    minTJ(T)=i=1N||ziTpi||22(4.37)

    需要计算目标函数 J 关于变换矩阵 T 的导数来求解式 (4.37),也就是说, 姿姿
    李代数由向量组成,相比 SO(3),SE(3) 具有良好的加法运算,所以使用李代数解决求导问题思路有两种:
    • R 对应的李代数加上小量,求相对于小量的变化率(导数模型);
    • R 左乘或右乘一个小量,求相对于小量的李代数的变化率(扰动模型)。

李代数求导

若对空间点 p 进行了旋转,得到了 Rp,计算旋转之后点的坐标相对于旋转的导数,不严谨地记为(这里并不能按照矩阵微分来定义导数,只是一个记号):

(Rp)R(4.38)

因为 SO(3) 没有加法,该导数无从计算。
转而计算:
(exp(ϕ)p)ϕ=limδϕ0exp[(ϕ+δϕ)]BCHpexp(ϕ)pδϕ=limδϕ0exp[(Jlδϕ)]exp(ϕ)pexp(ϕ)pδϕlimδϕ0[I+(Jlδϕ)]exp(ϕ)pexp(ϕ)pδϕ=limδϕ0(Jlδϕ)exp(ϕ)p×a×b=b×aδϕ=limδϕ0[exp(ϕ)p]Jlδϕδϕ=(Rp)Jl(4.39)

扰动模型(左乘)

SO(3) 上的扰动模型
  • R 进行一次扰动 ΔR (另一种求导方式),以避免计算式 (4.39) 中比较 Jl
  • 设左扰动 ΔR 对应的李代数为 φ , 对 φ 求导,得:
    (Rp)φ=limφ0exp(φ)exp(ϕ)pexp(ϕ)pφlimφ0(I+φ)exp(ϕ)pexp(ϕ)pφ=limφ0φRpφ=limφ0(Rp)φφ=(Rp)(4.40)

    式 (4.40) 扰动模型相比于式 (4.39) 李代数求导,省去了雅可比 Jl 的计算,使得 。(ps: 该求导公式在位姿估计中具有重要意义
SE(3) 上的扰动模型
  • 假设空间点 p 经过一次变换 T (对应的李代数为 ξ )后变为 Tp 。当给 T 左乘一个扰动 ΔT=exp(δξ) ,设扰动项的李代数为 δξ=[δρ,δϕ]T ,有:
    (Tp)δξ=limδξ0exp(δξ)exp(ξ)pexp(ξ)pδξlimδξ0(I+δξ)exp(ξ)pexp(ξ)pδξ=limδξ0δξexp(ξ)pδξ=limδξ0[δϕ0Tδρ1][Rp+t1]δξ=limδξ0[δϕ(Rp+t)+δρ0]δξ=[I0T(Rp+t)0T]=(Tp)(4.41)

    式 (4.41) 中的运算符 的含义:将一个齐次坐标的空间点变换成一个 4×6 的矩阵。

你可能感兴趣的:(视觉SLAM十四讲,映射,扰动模型,李代数求导)