半闲居士视觉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=0∞1n!An(4.18)
  
  对 so(3) 中任意 ϕ ，也可以通过泰勒展开来定义它的指数映射：
  
  exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n(4.19)
• ϕ 是三维向量，其模长和方向分别记作 θ 和 a ，有 ϕ=θa ， a 是长度为 1 的方向向量。关于 a∧ 有两条性质：
  
  {a∧a∧=aaT−Ia∧a∧a∧=−a∧(4.20)
  
  利用式 (4.20) 的 a∧ 高阶项的性质可以将式 (4.19) 写成：
  
  exp(ϕ∧)=exp(θa∧)=∑n=0∞1n!(θa∧)n=I+θa∧+12!θ2a∧a∧+13!θ3a∧a∧a∧+14!θ4(a∧)4+⋯=aaT−a∧a∧+θa∧+12!θ2a∧a∧−13!θ3a∧−14!θ4(a∧)2+⋯=aaT+(θ−13!θ3+15!θ5−⋯)a∧−(1−12!θ2+14!θ4−⋯)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧(4.21)
  
  整理得到：
  
  exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧(4.22)
  
  式 (4.22) 与罗德里格斯公式是一样的，说明 so(3)的物理意义就是旋转向量 。

• 给定旋转矩阵 R 时，亦能求李代数 so(3) 对应的元素：
  

  ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)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=0∞1n!(ϕ∧)n0T∑n=0∞1(n+1)!(ϕ∧)nρ1⎤⎦⎥⎥=[R0TJρ1]=T(4.24)
  
  右上角的 J 可整理为：
  
  J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧(4.25)

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

李群	映射	李代数
李群 SO(3) R∈R3×3 RRT=I det(R)=1	三维旋转 exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧指数映射←−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− 对数映射θ=arccos(tr(R)−12),Ra=a−→−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−	李代数 so(3) ϕ∈R3 ϕ∧=⎡⎣⎢0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎤⎦⎥
李群 SE(3) T∈R4×4 T=[R0Tt1]	三维变换 exp(ξ∧)=[exp(ϕ∧)0TJρ1] J=sinθθI+(1−sinθθ)aaT+1−cosθθ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+(1−sinθθ)aaT+1−cosθθa∧(4.29)
    
    它的逆为：
    
    J−1l=θ2cotθ2I+(1−θ2cotθ2)aaT−θ2a∧(4.30)
  • 式 (4.28) 中 右乘雅可比
    
    Jr(ϕ)=Jl(−ϕ)(4.31)
• 直观写法
  
  • 设某个旋转 R 对应的李代数为 ϕ ，给该旋转 左乘 一个微小的旋转 ΔR （对应的李代数为 Δϕ ）， 李群上的 ΔR⋅R 对应了李代数上的 BCH 近似值：
    
    exp(Δϕ∧)exp(ϕ∧)ΔR⋅R=exp⎡⎣⎢⎢⎛⎝⎜⎜ϕ+[Jl(ϕ)]−1左雅可比的逆Δϕ⎞⎠⎟⎟∧⎤⎦⎥⎥(4.32)
    
    在李群上左乘小量时，李代数上的加法相差左雅可比的逆。
  • 反之，李代数上让 ϕ+Δϕ ，可近似为李群上带左右雅可比的乘法：
    
    exp[(ϕ+Δϕ)∧]=exp[(ϕ+[Jl(ϕ)]−1[Jl(ϕ)]Δϕ)∧]=exp[(JlΔϕ)∧]exp(ϕ∧)=exp(ϕ∧)exp[(JrΔϕ)∧](4.33)
    
    李代数上进行小量加法时，相当于李群上左（右）乘一个带左（右）雅可比的量。
  • 对于 SE(3) ，亦有类似的 BCH 近似公式：
    
    ⎧⎩⎨⎪⎪exp(Δξ∧)exp(ξ∧)≈exp[(J−1lΔξ+ξ)∧]exp(ξ∧)exp(Δξ∧)≈exp[(J−1rΔξ+ξ)∧](4.34)
    
    式 (4.34) 中的 Jl,Jr 形式比较复杂，是 6×6 的矩阵。（鉴于在计算中 不需要 该雅可比，这里略去它的实际形式）

SO(3) 李代数上的求导

• 相机位姿估计问题描述
  设相机某时刻的位姿为 T ，观测到位于世界坐标系的点 p ，产生一个观测数据 z ，由坐标变换得：
  
  z=Tp+w(4.35)
  
  式 (4.35) 中的 w 为观测噪声，该噪声使得 z=Tp 一般不能精确地满足，故计算实际数据与理想观测值的误差：
  
  e=z−Tp(4.36)
  
  如果共有 N 个路标点和观测，对相机的位姿估计就是寻找最优的 T 使得整体误差最小化：
  
  minTJ(T)=∑i=1N||zi−Tpi||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[(ϕ+δϕ)∧]作BCH近似p−exp(ϕ∧)pδϕ=limδϕ→0exp[(Jlδϕ)∧]作泰勒展开exp(ϕ∧)p−exp(ϕ∧)pδϕ≈limδϕ→0[I+(Jlδϕ)∧]保留泰勒展开一阶项exp(ϕ∧)p−exp(ϕ∧)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(ϕ∧)p−exp(ϕ∧)pφ≈limφ→0(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)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(ξ∧)p−exp(ξ∧)pδξ≈limδξ→0(I+δξ∧)exp(ξ∧)p−exp(ξ∧)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 的矩阵。