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

本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接： http://blog.csdn.net/youngpan1101/article/details/71087919
作者：宋洋鹏（youngpan1101）
邮箱： yangpeng_song@163.com

---

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

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

---

李群与李代数

指数与对数映射

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

• 李代数 $\mathfrak{so} (3)$ 到 李群 $SO(3)$ 的映射关系：
  $${\boldsymbol R} = {\rm exp}({\boldsymbol \phi}^{\land}) \tag{4.17}$$
• 任意矩阵的指数映射在收敛情况下可以进行${\color{Red}{泰勒展开}}$：
  $${\rm exp}(\boldsymbol A) = \sum_{i=0}^\infty \frac{1}{n!} {\boldsymbol A}^{n} \tag{4.18}$$
  对 $\mathfrak{so} (3)$ 中任意 $\boldsymbol \phi$，也可以通过泰勒展开来定义它的指数映射：
  $$\color{Blue}{ {\rm exp}({\boldsymbol \phi}^{\land}) = \sum_{n=0}^\infty \frac{1}{n!} ({\boldsymbol \phi}^{\land})^{n} } \tag{4.19}$$
• $\boldsymbol \phi$ 是三维向量，其模长和方向分别记作 $\theta$ 和 $\boldsymbol a$，有 ${\boldsymbol \phi} = \theta {\boldsymbol a}$，$\boldsymbol a$ 是长度为 1 的方向向量。关于 ${\boldsymbol a}^{\land}$ 有两条性质：
  $$\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) 的 ${\boldsymbol a}^{\land}$ 高阶项的性质可以将式 (4.19) 写成：
  $$\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}$$
  整理得到：
  $${\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) 与罗德里格斯公式是一样的，说明 $\color{Red} {{\mathfrak{so} (3)} 的物理意义就是旋转向量 }$。

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

  $${\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}$$
  但实际当中没必要这样求，在旋转向量小节已经介绍了旋转矩阵到旋转向量的转换关系：
  $$\theta = {\rm arccos} \left( \frac { {\rm tr} ({\boldsymbol R}) -1}{2} \right), \quad {\boldsymbol R} {\boldsymbol n} = {\boldsymbol n}$$

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

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

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

• $\mathfrak{se} (3)$ 到 $SE(3)$ 的指数映射：
  $${\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}$$
  右上角的 ${\boldsymbol J}$ 可整理为：
  $${\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), \mathfrak{so} (3), \mathfrak{se} (3)$ 的对应关系

李群	映射	李代数
李群 $SO(3)$ ${\boldsymbol R} \in {\mathbb R}^{3 \times 3}$ ${\boldsymbol R}{\boldsymbol R}^{T} = {\boldsymbol I}$ $det({\boldsymbol R})=1$	三维旋转 $\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 指数映射}}$ $\color{Red}{\underrightarrow{ 对数映射 \quad \theta = {\rm arccos} \left( \frac { {\rm tr} ({\boldsymbol R}) -1}{2} \right), \quad {\boldsymbol R} {\boldsymbol a} = {\boldsymbol a} }}$	李代数 $\mathfrak{so} (3)$ ${\boldsymbol \phi} \in {\mathbb R}^{3}$ ${\boldsymbol \phi} ^{\land} = \begin{bmatrix} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \\ \end{bmatrix}$
李群 $SE(3)$ ${\boldsymbol T} \in {\mathbb R}^{4 \times 4}$ ${\boldsymbol T} = \begin{bmatrix} {\boldsymbol R} & {\boldsymbol t} \\ 0^{T} & 1 \\ \end{bmatrix}$	三维变换 $\color{Blue}{ {\rm exp}({\boldsymbol \xi}^{\land}) = \begin{bmatrix} {\rm exp}(\boldsymbol \phi ^{\land}) & {\boldsymbol J} {\boldsymbol \rho} \\ {\boldsymbol 0}^{T} & 1 \\ \end{bmatrix} }$ $\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 指数映射}}$ $\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} }}$	李代数 $\mathfrak{se} (3)$ ${\boldsymbol \xi} = \begin{bmatrix} {\boldsymbol \rho} \\ {\boldsymbol \phi} \\ \end{bmatrix} \in {\mathbb R}^{6}$ ${\boldsymbol \xi}^{\land} = \begin{bmatrix} {\boldsymbol \phi}^{\land} & {\boldsymbol \rho} \\ {\boldsymbol 0}^{T} & 0 \\ \end{bmatrix}$

---

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

---

李代数求导与扰动模型

BCH 公式与近似形式

• 相机位姿估计优化过程中$\color{Red}{求导}$是非常必要的，李群元素只有乘法（没有加法），无从定义导数，这也是$\color{Red}{使用李代数的目的}$。
• 当 $\mathfrak{so} (3)$ 上做两个李代数的加法时，$SO(3)$ 上是否对应着两个矩阵的乘积？ 相当于：
  $${\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}$$
  $\color{Red}{ 式\ (4.26)\ 在 {\boldsymbol \phi}^{\land} 为矩阵时并不成立}$，两个李代数指数映射乘积的完整形式由 Baker-Campbell-Hausdorff 公式（$\color{Red}{BCH公式}$）给出，由于完整形式复杂，这里给出展开式的前几项：
  $$\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)$ 上的李代数 $\ln ( {\rm exp}({\boldsymbol \phi}_{1}^{\land}) {\rm exp}({\boldsymbol \phi}_{2}^{\land}) )^{\lor}$ ， 当 ${\boldsymbol \phi}_{1}$ 或 ${\boldsymbol \phi}_{2}$ 为$\color{Red}{小量}$时，$\color{Red}{可忽略其高阶项}$，BCH 具有线性近似形式：
  $$\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) 中的第一个近似可知，当对一个旋转矩阵 ${\boldsymbol R}_{2}$ （李代数为 ${\boldsymbol \phi}_{2}$）左乘一个微小旋转矩阵 ${\boldsymbol R}_{1}$ （李代数为 ${\boldsymbol \phi}_{1}$）时，可以近似认为在原有的李代数 ${\boldsymbol \phi}_{2}$上，加上了一项 ${\boldsymbol J}_{l}({\boldsymbol \phi}_{2})^{-1}{\boldsymbol \phi}_{1}$ 。
  
  • 式 (4.28) 中$\color{Red}{左乘雅可比}$
    $${\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}$$
    它的逆为：
    $${\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}{右乘雅可比}$
    $${\boldsymbol J}_{r}({\boldsymbol \phi}) = {\boldsymbol J}_{l}(-{\boldsymbol \phi}) \tag{4.31}$$
• 直观写法
  
  • 设某个旋转 ${\boldsymbol R}$ 对应的李代数为 ${\boldsymbol \phi}$，给该旋转$\color{Red}{左乘}$一个微小的旋转 $\Delta {\boldsymbol R}$（对应的李代数为 $\Delta {\boldsymbol \phi}$ ）， 李群上的 $\Delta {\boldsymbol R} \cdot {\boldsymbol R}$ 对应了李代数上的 BCH 近似值：
    $$\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}$ ，可近似为李群上带左右雅可比的乘法：
    $$\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)$ ，亦有类似的 BCH 近似公式：
    $$\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) 中的 ${\boldsymbol {\mathcal J}}_{l}, \; {\boldsymbol {\mathcal J}}_{r}$ 形式比较复杂，是 $6 \times 6$ 的矩阵。（鉴于在计算中$\color{Red}{不需要}$该雅可比，这里略去它的实际形式）

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

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

李代数求导

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

$$\frac{\partial ({\boldsymbol Rp})}{\partial {\boldsymbol R}} \tag{4.38}$$
因为 $SO(3)$ 没有加法，该导数无从计算。
转而计算：
$$\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)$ 上的扰动模型

• 对 ${\boldsymbol R}$ 进行一次扰动 $\Delta {\boldsymbol R}$ （另一种求导方式），以避免计算式 (4.39) 中比较$\color{red}{复杂}$的 ${\boldsymbol J}_{l}$ 。
• 设左扰动 $\Delta {\boldsymbol R}$ 对应的李代数为 ${\boldsymbol \varphi}$ ， 对 ${\boldsymbol \varphi}$ 求导，得：
  $$\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) 李代数求导，省去了雅可比 ${\boldsymbol J}_{l}$ 的计算，使得$\color{Red}{扰动模型更为实用}$。（ps: 该求导公式在位姿估计中具有重要意义）

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

• 假设空间点 ${\boldsymbol p}$ 经过一次变换 ${\boldsymbol T}$（对应的李代数为 ${\boldsymbol \xi}$）后变为 ${\boldsymbol Tp}$ 。当给 ${\boldsymbol T}$ 左乘一个扰动 $\Delta {\boldsymbol T} = \exp (\delta {\boldsymbol \xi}^{\land})$，设扰动项的李代数为 $\delta {\boldsymbol \xi} = [\delta {\boldsymbol \rho}, \delta {\boldsymbol \phi}]^{T}$，有：
  $$\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 \times 6$ 的矩阵。