半闲居士视觉SLAM十四讲笔记（4）李群与李代数

半闲居士视觉SLAM十四讲笔记（4）李群与李代数

本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接：
作者：宋洋鹏（youngpan1101）
邮箱： yangpeng_song@163

---

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

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

---

李群与李代数

指数与对数映射

SO(3) $SO(3)$ 上的指数映射

• 李代数 so(3) $mathfrak{so} (3)$ 到 李群 SO(3) $SO(3)$ 的映射关系：
  R=exp(ϕ∧)(4.17) $${boldsymbol R} = {rm exp}({boldsymbol phi}^{land}) tag{4.17}$$
• 任意矩阵的指数映射在收敛情况下可以进行 泰勒展开 ${color{Red}{泰勒展开}}$：
  exp(A)=∑i=0∞1n!An(4.18) $${rm exp}(boldsymbol A) = sum_{i=0}^infty frac{1}{n!} {boldsymbol A}^{n} tag{4.18}$$
  对 so(3) $mathfrak{so} (3)$ 中任意 ϕ $boldsymbol phi$，也可以通过泰勒展开来定义它的指数映射：
  exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n(4.19) $$color{Blue}{ {rm exp}({boldsymbol phi}^{land}) = sum_{n=0}^infty frac{1}{n!} ({boldsymbol phi}^{land})^{n} } tag{4.19}$$
• ϕ $boldsymbol phi$ 是三维向量，其模长和方向分别记作 θ $theta$ 和 a $boldsymbol a$，有 ϕ=θa ${boldsymbol phi} = theta {boldsymbol a}$， a $boldsymbol a$ 是长度为 1 的方向向量。关于 a∧ ${boldsymbol a}^{land}$ 有两条性质：
  {a∧a∧=aaT−Ia∧a∧a∧=−a∧(4.20) $$begin{cases} {boldsymbol a}^{land} {boldsymbol a}^{land} = {boldsymbol a} {boldsymbol a}^{T} - {boldsymbol I} \ {boldsymbol a}^{land} {boldsymbol a}^{land} {boldsymbol a}^{land} = - {boldsymbol a}^{land} \ end{cases} tag{4.20}$$
  利用式 (4.20) 的 a∧ ${boldsymbol a}^{land}$ 高阶项的性质可以将式 (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) $$begin{aligned} {rm exp}({boldsymbol phi}^{land}) &= {rm exp}(theta {boldsymbol a}^{land}) = sum_{n=0}^infty frac{1}{n!} (theta {boldsymbol a}^{land})^{n} \ &= {boldsymbol I} + theta {boldsymbol a}^{land} + frac{1}{2!} theta ^{2} {boldsymbol a}^{land} {boldsymbol a}^{land} + frac{1}{3!} theta ^{3} {boldsymbol a}^{land} {boldsymbol a}^{land} {boldsymbol a}^{land} + frac{1}{4!} theta ^{4} ({boldsymbol a}^{land})^{4} + cdots \ &= {boldsymbol a} {boldsymbol a}^{T} - {boldsymbol a}^{land} {boldsymbol a}^{land} + theta {boldsymbol a}^{land} + frac{1}{2!} theta^{2} {boldsymbol a}^{land} {boldsymbol a}^{land} - frac{1}{3!} theta^{3} {boldsymbol a}^{land} - frac{1}{4!} theta^{4} ({boldsymbol a}^{land})^{2} + cdots \ &= {boldsymbol a} {boldsymbol a}^{T} + left( theta - frac{1}{3!} theta^{3} + frac{1}{5!} theta^{5} - cdots right) {boldsymbol a}^{land} - left( 1 - frac{1}{2!} theta^{2} + frac{1}{4!} theta^{4} - cdots right) {boldsymbol a}^{land} {boldsymbol a}^{land} \ &= {boldsymbol a}^{land} {boldsymbol a}^{land} + {boldsymbol I} + {rm sin}theta ;{boldsymbol a}^{land} - {rm cos}theta ; {boldsymbol a}^{land} {boldsymbol a}^{land} \ &= (1 - {rm cos}theta) {boldsymbol a}^{land} {boldsymbol a}^{land} + {boldsymbol I} + {rm sin}theta ; {boldsymbol a}^{land} \ &= {rm cos}theta ; {boldsymbol I} + (1-{rm cos}theta) {boldsymbol a} {boldsymbol a}^{T} + {rm sin}theta ; {boldsymbol a}^{land} end{aligned} tag{4.21}$$
  整理得到：
  exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧(4.22) $${rm exp}(theta {boldsymbol a}^{land}) = {rm cos}theta ; {boldsymbol I} + (1-{rm cos}theta) {boldsymbol a} {boldsymbol a}^{T} + {rm sin}theta ; {boldsymbol a}^{land} tag{4.22}$$
  式 (4.22) 与罗德里格斯公式是一样的，说明 so(3)的物理意义就是旋转向量 $color{Red} {{mathfrak{so} (3)} 的物理意义就是旋转向量 }$。

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

  ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨(4.23) $${boldsymbol phi} = ln({boldsymbol R})^{lor} = left( sum_{n=0}^infty frac{(-1)^{n}}{n+1} ({boldsymbol R} - {boldsymbol I})^{n+1}right) ^{lor} tag{4.23}$$
  但实际当中没必要这样求，在旋转向量小节已经介绍了旋转矩阵到旋转向量的转换关系：
  θ=arccos(tr(R)−12),Rn=n $$theta = {rm arccos} left( frac { {rm tr} ({boldsymbol R}) -1}{2} right), quad {boldsymbol R} {boldsymbol n} = {boldsymbol n}$$

• 指数映射只是一个 满射 $color{Red}{满射}$

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

SE(3) $SE(3)$ 上的指数映射

• se(3) $mathfrak{se} (3)$ 到 SE(3) $SE(3)$ 的指数映射：
  exp(ξ∧)=⎡⎣⎢⎢∑n=0∞1n!(ϕ∧)n0T∑n=0∞1(n+1)!(ϕ∧)nρ1⎤⎦⎥⎥=[R0TJρ1]=T(4.24) $${rm exp}({boldsymbol xi}^{land}) = begin{bmatrix} sum_{n=0}^infty frac{1}{n!} ({boldsymbol phi}^{land})^{n} & sum_{n=0}^infty frac{1}{(n+1)!} ({boldsymbol phi}^{land})^{n} {boldsymbol rho} \ {boldsymbol 0}^{T} & 1 \ end{bmatrix} = begin{bmatrix} {boldsymbol R} & {boldsymbol J} {boldsymbol rho} \ {boldsymbol 0}^{T} & 1 \ end{bmatrix} = {boldsymbol T} tag{4.24}$$
  右上角的 J ${boldsymbol J}$ 可整理为：
  J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧(4.25) $${boldsymbol J} = frac{{rm sin}theta}{theta} {boldsymbol I} + left( 1 - frac{{rm sin}theta}{theta} right) {boldsymbol a} {boldsymbol a}^{T} + frac{1 - {rm cos}theta}{theta} {boldsymbol a}^{land} tag{4.25}$$

SO(3),SE(3),so(3),se(3) $SO(3), SE(3), mathfrak{so} (3), mathfrak{se} (3)$ 的对应关系

李群	映射	李代数
李群 SO(3) $SO(3)$ R∈R3×3 ${boldsymbol R} in {mathbb R}^{3 times 3}$ RRT=I ${boldsymbol R}{boldsymbol R}^{T} = {boldsymbol I}$ det(R)=1 $det({boldsymbol R})=1$	三维旋转 exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧指数映射←−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− $color{Blue}{underleftarrow{ {rm exp}(theta {boldsymbol a}^{land}) = {rm cos}theta ; {boldsymbol I} + (1-{rm cos}theta) {boldsymbol a} {boldsymbol a}^{T} + {rm sin}theta ; {boldsymbol a}^{land} quad 指数映射}}$ 对数映射θ=arccos(tr(R)−12),Ra=a−→−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− $color{Red}{underrightarrow{ 对数映射 quad theta = {rm arccos} left( frac { {rm tr} ({boldsymbol R}) -1}{2} right), quad {boldsymbol R} {boldsymbol a} = {boldsymbol a} }}$	李代数 so(3) $mathfrak{so} (3)$ ϕ∈R3 ${boldsymbol phi} in {mathbb R}^{3}$ ϕ∧=⎡⎣⎢0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎤⎦⎥ ${boldsymbol phi} ^{land} = begin{bmatrix} 0 & -phi_{3} & phi_{2} \ phi_{3} & 0 & -phi_{1} \ -phi_{2} & phi_{1} & 0 \ end{bmatrix}$
李群 SE(3) $SE(3)$ T∈R4×4 ${boldsymbol T} in {mathbb R}^{4 times 4}$ T=[R0Tt1] ${boldsymbol T} = begin{bmatrix} {boldsymbol R} & {boldsymbol t} \ 0^{T} & 1 \ end{bmatrix}$	三维变换 exp(ξ∧)=[exp(ϕ∧)0TJρ1] $color{Blue}{ {rm exp}({boldsymbol xi}^{land}) = begin{bmatrix} {rm exp}(boldsymbol phi ^{land}) & {boldsymbol J} {boldsymbol rho} \ {boldsymbol 0}^{T} & 1 \ end{bmatrix} }$ J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧指数映射←−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− $color{Blue}{underleftarrow{ {boldsymbol J} = frac{{rm sin}theta}{theta} {boldsymbol I} + left( 1 - frac{{rm sin}theta}{theta} right) {boldsymbol a} {boldsymbol a}^{T} + frac{1 - {rm cos}theta}{theta} {boldsymbol a}^{land} quad 指数映射}}$ 对数映射θ=arccos(tr(R)−12),Ra=a,t=Jρ−→−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− $color{Red}{underrightarrow{ 对数映射 quad theta = {rm arccos} left( frac { {rm tr} ({boldsymbol R}) -1}{2} right), quad {boldsymbol R} {boldsymbol a} = {boldsymbol a}, quad {boldsymbol t} = {boldsymbol J}{boldsymbol rho} }}$	李代数 se(3) $mathfrak{se} (3)$ ξ=[ρϕ]∈R6 ${boldsymbol xi} = begin{bmatrix} {boldsymbol rho} \ {boldsymbol phi} \ end{bmatrix} in {mathbb R}^{6}$ ξ∧=[ϕ∧0Tρ0] ${boldsymbol xi}^{land} = begin{bmatrix} {boldsymbol phi}^{land} & {boldsymbol rho} \ {boldsymbol 0}^{T} & 0 \ end{bmatrix}$

---

——————————– 分割线<< 家有小武，如有一母 $color{Green}{家有小武，如有一母}$ >>分割线 ——————————–

---

李代数求导与扰动模型

BCH 公式与近似形式

• 相机位姿估计优化过程中 求导 $color{Red}{求导}$是非常必要的，李群元素只有乘法（没有加法），无从定义导数，这也是 使用李代数的目的 $color{Red}{使用李代数的目的}$。
• 当 so(3) $mathfrak{so} (3)$ 上做两个李代数的加法时， SO(3) $SO(3)$ 上是否对应着两个矩阵的乘积？ 相当于：
  exp(ϕ∧1)exp(ϕ∧2)=exp((ϕ1+ϕ2)∧)(4.26) $${rm exp}({boldsymbol phi}_{1}^{land}) {rm exp}({boldsymbol phi}_{2}^{land}) = {rm exp} left( ({boldsymbol phi}_{1} + {boldsymbol phi}_{2})^{land} right) tag{4.26}$$
  式 (4.26) 在ϕ∧为矩阵时并不成立 $color{Red}{ 式 (4.26) 在 {boldsymbol phi}^{land} 为矩阵时并不成立}$，两个李代数指数映射乘积的完整形式由 Baker-Campbell-Hausdorff 公式（ BCH公式 $color{Red}{BCH公式}$）给出，由于完整形式复杂，这里给出展开式的前几项：
  ln(exp(A)exp(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯(4.27) $$ln left( {rm exp}({boldsymbol A}) {rm exp}({boldsymbol B}) right) = {boldsymbol A} + {boldsymbol B} + frac{1}{2}[{boldsymbol A},{boldsymbol B}] + frac{1}{12}[{boldsymbol A},[{boldsymbol A},{boldsymbol B}]] - frac{1}{12}[{boldsymbol B},[{boldsymbol A},{boldsymbol B}]] + cdots tag{4.27}$$
  式 (4.27) 中的 [,] $[,]$ 为李括号。当处理两个矩阵指数之积时，会产生一些李括号组成的余项。
  考虑 SO(3) $SO(3)$ 上的李代数 ln(exp(ϕ∧1)exp(ϕ∧2))∨ $ln ( {rm exp}({boldsymbol phi}_{1}^{land}) {rm exp}({boldsymbol phi}_{2}^{land}) )^{lor}$ ， 当 ϕ1 ${boldsymbol phi}_{1}$ 或 ϕ2 ${boldsymbol phi}_{2}$ 为 小量 $color{Red}{小量}$时， 可忽略其高阶项 $color{Red}{可忽略其高阶项}$，BCH 具有线性近似形式：
  ln(exp(ϕ∧1)exp(ϕ∧2))∨≈{[Jl(ϕ2)]−1ϕ1+ϕ2[Jr(ϕ1)]−1ϕ2+ϕ1ifϕ1issmallifϕ2issmall(4.28) $$ln ( {rm exp}({boldsymbol phi}_{1}^{land}) {rm exp}({boldsymbol phi}_{2}^{land}) )^{lor} approx begin{cases} left[ {boldsymbol J}_{l}({boldsymbol phi}_{2}) right] ^{-1}{boldsymbol phi}_{1}+{boldsymbol phi}_{2} & {rm if}; {boldsymbol phi}_{1} ; {rm is ; small} \ left[ {boldsymbol J}_{r}({boldsymbol phi}_{1}) right] ^{-1}{boldsymbol phi}_{2}+{boldsymbol phi}_{1} & {rm if}; {boldsymbol phi}_{2} ; {rm is ; small} \ end{cases} tag{4.28}$$
  由式 (4.28) 中的第一个近似可知，当对一个旋转矩阵 R2 ${boldsymbol R}_{2}$ （李代数为 ϕ2 ${boldsymbol phi}_{2}$）左乘一个微小旋转矩阵 R1 ${boldsymbol R}_{1}$ （李代数为 ϕ1 ${boldsymbol phi}_{1}$）时，可以近似认为在原有的李代数 ϕ2 ${boldsymbol phi}_{2}$上，加上了一项 Jl(ϕ2)−1ϕ1 ${boldsymbol J}_{l}({boldsymbol phi}_{2})^{-1}{boldsymbol phi}_{1}$ 。
  
  • 式 (4.28) 中 左乘雅可比 $color{Red}{左乘雅可比}$
    Jl=J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧(4.29) $${boldsymbol J}_{l} = {boldsymbol J} = frac{{rm sin}theta}{theta} {boldsymbol I} + left( 1 - frac{{rm sin}theta}{theta} right) {boldsymbol a} {boldsymbol a}^{T} + frac{1 - {rm cos}theta}{theta} {boldsymbol a}^{land}tag{4.29}$$
    它的逆为：
    J−1l=θ2cotθ2I+(1−θ2cotθ2)aaT−θ2a∧(4.30) $${boldsymbol J}_{l}^{-1} = frac{theta}{2} {rm cot} frac{theta}{2} {boldsymbol I} + left( 1 - frac{theta}{2} {rm cot} frac{theta}{2} right) {boldsymbol a} {boldsymbol a}^{T} - frac{theta}{2} {boldsymbol a}^{land} tag{4.30}$$
  • 式 (4.28) 中 右乘雅可比 $color{Red}{右乘雅可比}$
    Jr(ϕ)=Jl(−ϕ)(4.31) $${boldsymbol J}_{r}({boldsymbol phi}) = {boldsymbol J}_{l}(-{boldsymbol phi}) tag{4.31}$$
• 直观写法
  
  • 设某个旋转 R ${boldsymbol R}$ 对应的李代数为 ϕ ${boldsymbol phi}$，给该旋转 左乘 $color{Red}{左乘}$一个微小的旋转 ΔR $Delta {boldsymbol R}$（对应的李代数为 Δϕ $Delta {boldsymbol phi}$ ）， 李群上的 ΔR⋅R $Delta {boldsymbol R} cdot {boldsymbol R}$ 对应了李代数上的 BCH 近似值：
    exp(Δϕ∧)exp(ϕ∧)ΔR⋅R=exp⎡⎣⎢⎢⎛⎝⎜⎜ϕ+[Jl(ϕ)]−1左雅可比的逆Δϕ⎞⎠⎟⎟∧⎤⎦⎥⎥(4.32) $$underbrace{ {rm exp} (Delta {boldsymbol phi}^{land}) {rm exp} ({boldsymbol phi}^{land}) }_{color{Red}{ Delta {boldsymbol R} cdot {boldsymbol R} }} = {rm exp} left[ left( {boldsymbol phi} + underbrace{ left[ {boldsymbol J}_{l}({boldsymbol phi}) right]^{-1} }_{ color{Red}{左雅可比的逆} } Delta {boldsymbol phi} right) ^{land} right] tag{4.32}$$
    在李群上左乘小量时，李代数上的加法相差左雅可比的逆。
  • 反之，李代数上让 ϕ+Δϕ ${boldsymbol phi} + Delta {boldsymbol phi}$ ，可近似为李群上带左右雅可比的乘法：
    exp[(ϕ+Δϕ)∧]=exp[(ϕ+[Jl(ϕ)]−1[Jl(ϕ)]Δϕ)∧]=exp[(JlΔϕ)∧]exp(ϕ∧)=exp(ϕ∧)exp[(JrΔϕ)∧](4.33) $$begin{aligned} {rm exp} left[ left( {boldsymbol phi} + Delta {boldsymbol phi} right) ^{land} right] &= {rm exp} left[ left( {boldsymbol phi} + left[ {boldsymbol J}_{l}({boldsymbol phi}) right]^{-1} left[ {boldsymbol J}_{l}({boldsymbol phi}) right] Delta {boldsymbol phi} right) ^{land} right] \ &= {rm exp} left[ ( color{Red} {{boldsymbol J}_{l}} Delta {boldsymbol phi})^{land} right] {rm exp} ({boldsymbol phi}^{land}) \ &= {rm exp} ({boldsymbol phi}^{land}) {rm exp} left[ ( color{Red} {{boldsymbol J}_{r}} Delta {boldsymbol phi})^{land} right] end{aligned} tag{4.33}$$
    李代数上进行小量加法时，相当于李群上左（右）乘一个带左（右）雅可比的量。
  • 对于 SE(3) $SE(3)$ ，亦有类似的 BCH 近似公式：
    ⎧⎩⎨⎪⎪exp(Δξ∧)exp(ξ∧)≈exp[(J−1lΔξ+ξ)∧]exp(ξ∧)exp(Δξ∧)≈exp[(J−1rΔξ+ξ)∧](4.34) $$begin{cases} {rm exp}(Delta {boldsymbol xi}^{land}) {rm exp}({boldsymbol xi}^{land}) approx {rm exp} left[ ({boldsymbol {mathcal J}}_{l}^{-1} Delta {boldsymbol xi} + {boldsymbol xi})^{land} right] \ {rm exp}({boldsymbol xi}^{land}) {rm exp}(Delta {boldsymbol xi}^{land}) approx {rm exp} left[ ({boldsymbol {mathcal J}}_{r}^{-1} Delta {boldsymbol xi} + {boldsymbol xi})^{land} right]\ end{cases} tag{4.34}$$
    式 (4.34) 中的 Jl,Jr ${boldsymbol {mathcal J}}_{l}, ; {boldsymbol {mathcal J}}_{r}$ 形式比较复杂，是 6×6 $6 times 6$ 的矩阵。（鉴于在计算中 不需要 $color{Red}{不需要}$该雅可比，这里略去它的实际形式）

SO(3) $SO(3)$ 李代数上的求导

• 相机位姿估计问题描述
  设相机某时刻的位姿为 T ${boldsymbol T}$，观测到位于世界坐标系的点 p ${boldsymbol p}$，产生一个观测数据 z ${boldsymbol z}$，由坐标变换得：
  z=Tp+w(4.35) $${boldsymbol z} = {boldsymbol T}{boldsymbol p} + {boldsymbol w} tag{4.35}$$
  式 (4.35) 中的 w ${boldsymbol w}$ 为观测噪声，该噪声使得 z=Tp ${boldsymbol z} = {boldsymbol T}{boldsymbol p}$ 一般不能精确地满足，故计算实际数据与理想观测值的误差：
  e=z−Tp(4.36) $${boldsymbol e} = {boldsymbol z} - {boldsymbol T}{boldsymbol p} tag{4.36}$$
  如果共有 N $N$ 个路标点和观测，对相机的位姿估计就是寻找最优的 T${boldsymbol T}$ 使得整体误差最小化：
  minTJ(T)=∑i=1N||zi−Tpi||22(4.37) $$min_{boldsymbol T} J({boldsymbol T}) = sum_{i=1}^N || {boldsymbol z}_{i} - {boldsymbol T} {boldsymbol p}_{i} ||_{2}^{2} tag{4.37}$$
  需要计算目标函数 J $J$ 关于变换矩阵 T${boldsymbol T}$ 的导数来求解式 (4.37)，也就是说， 通过构建与位姿相关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值 $color{Red}{通过构建与位姿相关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值}$。
  李代数由向量组成，相比 SO(3),SE(3) $SO(3), SE(3)$ 具有良好的加法运算，所以使用李代数解决求导问题思路有两种：
  
  • 对 R ${boldsymbol R}$ 对应的李代数加上小量，求相对于小量的变化率（导数模型）；
  • 对 R ${boldsymbol R}$ 左乘或右乘一个小量，求相对于小量的李代数的变化率（扰动模型）。

李代数求导

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

∂(Rp)∂R(4.38) $$frac{partial ({boldsymbol Rp})}{partial {boldsymbol R}} tag{4.38}$$
因为 SO(3) $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) $$begin{aligned} frac{partial ( exp( {boldsymbol phi}^{land} ) {boldsymbol p} )} {partial {boldsymbol phi}} &= lim_{delta {boldsymbol phi} to 0} frac { overbrace { exp left[ ({boldsymbol phi} + delta {boldsymbol phi})^{land} right] }^{color{Red}{作BCH近似}} {boldsymbol p} - exp({boldsymbol phi}^{land}) {boldsymbol p} } {delta {boldsymbol phi}} \ &= lim_{delta {boldsymbol phi} to 0} frac { overbrace{ exp left[ ( {boldsymbol J}_{l} delta {boldsymbol phi} )^{land} right] }^{color{Red}{作泰勒展开}} exp({boldsymbol phi}^{land}) {boldsymbol p} - exp({boldsymbol phi}^{land}) {boldsymbol p} } { delta {boldsymbol phi} } \ &approx lim_{delta {boldsymbol phi} to 0} frac { overbrace{ left[ {boldsymbol I} + ( {boldsymbol J}_{l} delta {boldsymbol phi} )^{land} right] }^{color{Red}{保留泰勒展开一阶项}} exp({boldsymbol phi}^{land}) {boldsymbol p} - exp({boldsymbol phi}^{land}) {boldsymbol p} } { delta {boldsymbol phi} } \ &= lim_{delta {boldsymbol phi} to 0} frac { overbrace{ ( {boldsymbol J}_{l} delta {boldsymbol phi} )^{land} exp({boldsymbol phi}^{land}) {boldsymbol p} }^{color{Red}{符号land类似外积 times ，a times b = - b times a}} } { delta {boldsymbol phi} } \ &= lim_{delta {boldsymbol phi} to 0} frac { -left[ exp ({boldsymbol phi}^{land}) {boldsymbol p} right]^{land} {boldsymbol J}_{l} delta {boldsymbol phi} } { delta {boldsymbol phi} } = - ({boldsymbol {Rp}})^{land} {boldsymbol J}_{l} end{aligned} tag{4.39}$$

扰动模型（左乘）

SO(3) $SO(3)$ 上的扰动模型

• 对 R ${boldsymbol R}$ 进行一次扰动 ΔR $Delta {boldsymbol R}$ （另一种求导方式），以避免计算式 (4.39) 中比较 复杂 $color{red}{复杂}$的 Jl ${boldsymbol J}_{l}$ 。
• 设左扰动 ΔR $Delta {boldsymbol R}$ 对应的李代数为 φ ${boldsymbol varphi}$ ， 对 φ ${boldsymbol varphi}$ 求导，得：
  ∂(Rp)∂φ=limφ→0exp(φ∧)可作泰勒展开exp(ϕ∧)p−exp(ϕ∧)pφ≈limφ→0(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)pφ=limφ→0φ∧Rpφ=limφ→0−(Rp)∧φφ=−(Rp)∧(4.40) $$begin{aligned} frac{partial ({boldsymbol Rp})}{partial {boldsymbol varphi}} &= lim_{{boldsymbol varphi} to 0} frac{ overbrace{ exp ({boldsymbol varphi}^{land}) }^{color{Red}{可作泰勒展开}} exp ({boldsymbol phi}^{land}) {boldsymbol p} - exp ({boldsymbol phi}^{land}) {boldsymbol p}} { {boldsymbol varphi} } \ &approx lim_{{boldsymbol varphi} to 0} frac{({boldsymbol I} + {boldsymbol varphi}^{land}) exp ({boldsymbol phi}^{land}) {boldsymbol p} - exp ({boldsymbol phi}^{land}) {boldsymbol p}} { {boldsymbol varphi} } \ &= lim_{{boldsymbol varphi} to 0} frac{ {boldsymbol varphi}^{land} {boldsymbol {Rp}} } { {boldsymbol varphi} } \ &= lim_{{boldsymbol varphi} to 0} frac{ -({boldsymbol {Rp}})^{land} {boldsymbol varphi} } { {boldsymbol varphi} } = -({boldsymbol {Rp}})^{land} end{aligned} tag{4.40}$$
  式 (4.40) 扰动模型相比于式 (4.39) 李代数求导，省去了雅可比 Jl ${boldsymbol J}_{l}$ 的计算，使得 扰动模型更为实用 $color{Red}{扰动模型更为实用}$。（ps: 该求导公式在位姿估计中具有重要意义）

SE(3) $SE(3)$ 上的扰动模型

• 假设空间点 p ${boldsymbol p}$ 经过一次变换 T ${boldsymbol T}$（对应的李代数为 ξ ${boldsymbol xi}$）后变为 Tp ${boldsymbol Tp}$ 。当给 T ${boldsymbol T}$ 左乘一个扰动 ΔT=exp(δξ∧) $Delta {boldsymbol T} = exp (delta {boldsymbol xi}^{land})$，设扰动项的李代数为 δξ=[δρ,δϕ]T $delta {boldsymbol xi} = [delta {boldsymbol rho}, delta {boldsymbol phi}]^{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) $$begin{aligned} frac{partial ({boldsymbol {Tp}})}{partial delta{boldsymbol xi}} &= lim_{delta{boldsymbol xi} to 0} frac{ overbrace{ exp (delta {boldsymbol xi}^{land}) }^{color{Red}{可作泰勒展开}} exp ({boldsymbol xi}^{land}) {boldsymbol p} - exp ({boldsymbol xi}^{land}) {boldsymbol p}} { delta {boldsymbol xi} } \ &approx lim_{delta{boldsymbol xi} to 0} frac{ ({boldsymbol I} + delta {boldsymbol xi}^{land}) exp ({boldsymbol xi}^{land}) {boldsymbol p} - exp ({boldsymbol xi}^{land}) {boldsymbol p} } { delta {boldsymbol xi} } \ &= lim_{delta{boldsymbol xi} to 0} frac{ delta {boldsymbol xi}^{land} exp ({boldsymbol xi}^{land}) {boldsymbol p} } { delta {boldsymbol xi} } \ &= lim_{delta{boldsymbol xi} to 0} frac{ begin{bmatrix}delta {boldsymbol phi}^{land} & delta {boldsymbol rho} \{boldsymbol 0}^{T} & 1 \ end{bmatrix} begin{bmatrix}{boldsymbol {Rp}} + {boldsymbol t} \1 \ end{bmatrix} } { delta {boldsymbol xi} } \ &= lim_{delta{boldsymbol xi} to 0} frac{ begin{bmatrix}delta {boldsymbol phi}^{land} ({boldsymbol {Rp}} + {boldsymbol t}) + delta {boldsymbol rho} \0 \ end{bmatrix} } { delta {boldsymbol xi} } \ &= begin{bmatrix}{boldsymbol I} & -({boldsymbol {Rp}} + {boldsymbol t})^{land} \{boldsymbol 0}^{T} & {boldsymbol 0}^{T} \ end{bmatrix} = ({boldsymbol {Tp}})^{bigodot} end{aligned} tag{4.41}$$
  式 (4.41) 中的运算符 ⨀ $bigodot$ 的含义：将一个齐次坐标的空间点变换成一个 4×6 $4 times 6$ 的矩阵。