视觉 SLAM 十四讲 - 李代数求导与扰动模型

SLAM 中的位姿（旋转矩阵 $\mathbf{R}\in SO(3)$ 或变换矩阵 $\mathbf{T}\in SE(3)$）构成李群，其对乘法封闭，但​​对加法不封闭​​。两个旋转矩阵相加不再是有效的旋转矩阵，无法直接通过加法定义导数。李代数求导（尤其是扰动模型）是 SLAM 位姿优化的数学基础，解决了旋转矩阵的不可加性问题，提升了计算效率。

1. BCH 公式与近似形式

Baker-Campbell-Hausdorff(BCH) 公式在李群与李代数理论中具有重要地位。设 $\mathbf{A},\mathbf{B}$ 为李代数中的元素，其指数映射 $\exp (\mathbf{A}),\exp (\mathbf{B})$ 的乘积可表示为：
$$\exp (\mathbf{A})\exp (\mathbf{B})=\exp (\mathbf{C})$$其中，$[,]$ 为李括号，且 $\mathbf{C}=\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\cdots$

BCH 的线性近似表达如下：
$$\ln (\exp ({\mathbf{\varphi }}_1^{\wedge })\exp ({\mathbf{\varphi }}_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{\mathbf{J}}_l({\mathbf{\varphi }}_2{)}^{-1}{\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2当{\mathbf{\varphi }}_1为小量\\ \\ {\mathbf{J}}_r({\mathbf{\varphi }}_1{)}^{-1}{\mathbf{\varphi }}_2+{\mathbf{\varphi }}_1当{\mathbf{\varphi }}_2为小量\end{array}$$其中：

• ${\mathbf{J}}_l(\mathbf{\varphi })=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$，${\mathbf{J}}_l(\mathbf{\varphi }{)}^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\mathbf{a}{\mathbf{a}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$
• ${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$

假设存在旋转矩阵 $\mathbf{R}$，对应的李代数为 $\mathbf{\varphi }$。对 $\mathbf{R}$ 左乘一个微小旋转 $\mathbf{\Delta }\mathbf{R}$，对应的李代数为 $\mathbf{\Delta }\mathbf{\varphi }$。在李群上有 $\mathbf{\Delta }\mathbf{R}\mathbf{R}$，而在李代数上，根据 BCH 近似，为 ${\mathbf{J}}_l^{-1}(\mathbf{\varphi })\Delta \mathbf{\varphi }+\mathbf{\varphi }$，从而有：
$$\exp (\Delta {\mathbf{\varphi }}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })=\exp (({\mathbf{J}}_l^{-1}(\mathbf{\varphi })\Delta \mathbf{\varphi }+\mathbf{\varphi }{)}^{\wedge })$$右乘 $\mathbf{R}\mathbf{\Delta }\mathbf{R}$ 的话，则有：
$$\exp ({\mathbf{\varphi }}^{\wedge })\exp (\Delta {\mathbf{\varphi }}^{\wedge })=\exp (({\mathbf{J}}_r^{-1}(\mathbf{\varphi })\Delta \mathbf{\varphi }+\mathbf{\varphi }{)}^{\wedge })$$反之，如果我们对李代数中的一个 $\mathbf{\varphi }$ 加上 $\Delta \mathbf{\varphi }$，则有：
$$\exp ((\mathbf{\varphi }+\Delta \mathbf{\varphi }{)}^{\wedge })=\{\begin{array}{l}\exp ((\mathbf{\varphi }+{\mathbf{J}}_l^{-1}(\mathbf{\varphi }){\mathbf{J}}_l(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge })=\exp (({\mathbf{J}}_l(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\\ \\ \exp ((\mathbf{\varphi }+{\mathbf{J}}_r^{-1}(\mathbf{\varphi }){\mathbf{J}}_r(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge })=\exp ({\mathbf{\varphi }}^{\wedge })\exp (({\mathbf{J}}_r(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge })\end{array}$$同理，对于 $SE(3)$，也有类似的 BCH 近似：
$$\{\begin{array}{l}\exp (\Delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })=\exp (({\mathbf{J}}_l^{-1}(\mathbf{\xi })\Delta \mathbf{\xi }+\mathbf{\xi }{)}^{\wedge })\\ \\ \exp ({\mathbf{\xi }}^{\wedge })\exp (\Delta {\mathbf{\xi }}^{\wedge })=\exp (({\mathbf{J}}_r^{-1}(\mathbf{\xi })\Delta \mathbf{\xi }+\mathbf{\xi }{)}^{\wedge })\end{array}$$

2. 李代数求导

在 SLAM 问题中，我们常需计算目标函数关于位姿变换矩阵 $\mathbf{T}\in SE(3)$（或其旋转部分 $\mathbf{R}\in SO(3)$）的导数。然而，$SO(3)$ 群和 $SE(3)$ 群是​​光滑流形​​而非​​向量空间​​，不具有线性运算（加法和数乘）的封闭性，因此不能直接在其上定义普通意义上的导数。若将 $\mathbf{T}$ 视为普通矩阵进行微分操作，则必须施加严格的​​约束​​以确保结果仍在群中。​​相反，李代数 $so(3)$ 和 $se(3)$ 作为向量空间，具有良好的加法运算和标量乘法结构​​。这使得将目标函数的导数转化为定义在李代数（即切空间）上的导数运算成为可能。因此，​​利用李代数建立求导框架是解决该问题的有效方法。​

我们对一个空间点 $\mathbf{p}$ 进行旋转 $\mathbf{R}$ （对应李代数 $\mathbf{\varphi }$）得到 $\mathbf{R}\mathbf{p}$，则旋转后的坐标 $\mathbf{R}\mathbf{p}$ 相对于旋转矩阵 $\mathbf{R}$ 的导数为：
$$\frac{\partial \mathbf{R}\mathbf{p}}{\partial \mathbf{R}}=\frac{\partial \exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\partial \mathbf{\varphi }}$$按照导数的定义有：
$$\begin{array}{rl}\frac{\partial \left(\exp ({\varphi }^{\wedge })\mathbf{p}\right)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{\exp ((\varphi +\delta \varphi {)}^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp (({\mathbf{J}}_l\delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{(\mathbf{I}+({\mathbf{J}}_l\delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{({\mathbf{J}}_l\delta \varphi {)}^{\wedge }\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{-(\exp ({\varphi }^{\wedge })\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\delta \varphi }{\delta \varphi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$上述推导中，用到了如下公式：

• 对 $\exp (({\mathbf{J}}_l\delta \varphi {)}^{\wedge })$ 进行一阶泰勒展开：$\exp (({\mathbf{J}}_l\delta \varphi {)}^{\wedge })\approx \mathbf{I}+({\mathbf{J}}_l\delta \varphi {)}^{\wedge }$
• 关于 $({\mathbf{J}}_l\delta \varphi {)}^{\wedge }\exp ({\varphi }^{\wedge })\mathbf{p}=-(\exp ({\varphi }^{\wedge })\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\delta \varphi$ 的证明。对于两个向量 $\mathbf{a}$ 和 $\mathbf{b}$，则有 $\mathbf{a}\times \mathbf{b}={\mathbf{a}}^{\wedge }\mathbf{b}$。又因为 $\mathbf{a}\times \mathbf{b}=-\mathbf{b}\times \mathbf{a}$，所以有 $\mathbf{a}\times \mathbf{b}=-\mathbf{b}\times \mathbf{a}=-{\mathbf{b}}^{\wedge }\mathbf{a}$，令 $\mathbf{a}={\mathbf{J}}_l\delta \varphi$，$\mathbf{b}=\exp ({\varphi }^{\wedge })\mathbf{p}$ 即可证明

上述推导使用的是 ${\mathbf{J}}_l$，也可以使用 ${\mathbf{J}}_r$，推导如下：
$$\begin{array}{rl}\frac{\partial \left(\exp ({\varphi }^{\wedge })\mathbf{p}\right)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{\exp ((\varphi +\delta \varphi {)}^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })\exp (({\mathbf{J}}_r\delta \varphi {)}^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })(\mathbf{I}+({\mathbf{J}}_r\delta \varphi {)}^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })({\mathbf{J}}_r\delta \varphi {)}^{\wedge }\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{-\exp ({\varphi }^{\wedge }){\mathbf{p}}^{\wedge }{\mathbf{J}}_r\delta \varphi }{\delta \varphi }=-\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{J}}_r\end{array}$$因此，旋转后的点相对于李代数的导数为：
$$\frac{\partial \mathbf{R}\mathbf{p}}{\partial \mathbf{\varphi }}=\{\begin{array}{l}-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\\ \\ -\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{J}}_r\end{array}$$由于直接计算 ${\mathbf{J}}_l$ 和 ${\mathbf{J}}_r$ 较为复杂，​​ 因此在 ​​实际应用中，我们通常避免直接计算它们，而是采用扰动模型来计算导数。​

3. 扰动模型

3.1. SO(3)

我们对 $\mathbf{R}$ 添加一次左扰动 $\Delta \mathbf{R}$（对应李代数 $\mathbf{\phi }$），然后对 $\mathbf{\phi }$ 进行求导，即有：
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{(\mathbf{I}+{\phi }^{\wedge })\exp ({\varphi }^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\mathbf{R}\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\phi }{\phi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$$如果添加右扰动 $\Delta \mathbf{R}$（对应李代数 $\mathbf{\phi }$），则有：
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })\exp ({\phi }^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })(\mathbf{I}+{\phi }^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}{\phi }^{\wedge }\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{-\mathbf{R}{\mathbf{p}}^{\wedge }\phi }{\phi }=-\mathbf{R}{\mathbf{p}}^{\wedge }\end{array}$$注意到：$-(\mathbf{R}\mathbf{p}{)}^{\wedge }\ne -\mathbf{R}{\mathbf{p}}^{\wedge }$，而是满足如下关系：
$$(\mathbf{R}\mathbf{p}{)}^{\wedge }=\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^T$$证明如下：
参考博客：多视图几何：本质矩阵与基础矩阵，对于任意旋转矩阵 $\mathbf{R}$ 和向量 $\mathbf{a}$ 和 $\mathbf{b}$，我们有：
$$\mathbf{R}(\mathbf{a}\times \mathbf{b})=(\mathbf{R}\mathbf{a})\times (\mathbf{R}\mathbf{b})$$对任意向量 $\mathbf{v}$，可得：

• 左边：$(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{v}=(\mathbf{R}\mathbf{p})\times \mathbf{v}$
• 右边：$\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^T\mathbf{v}=\mathbf{R}(\mathbf{p}\times ({\mathbf{R}}^T\mathbf{v}))=(\mathbf{R}\mathbf{p})\times (\mathbf{R}{\mathbf{R}}^T\mathbf{v})=(\mathbf{R}\mathbf{p})\times \mathbf{v}$

证毕。

3.2. SE(3)

对一个空间点 $\mathbf{p}$ 进行旋转 $\mathbf{T}$ （对应李代数 $\mathbf{\xi }$）得到 $\mathbf{T}\mathbf{p}$（$\mathbf{p}$ 为齐次坐标），给 $\mathbf{T}$ 左乘一个扰动 $\Delta \mathbf{T}=\exp (\delta \mathbf{\xi })$，且有 $\delta \mathbf{\xi }=[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T$，不妨设 $\mathbf{p}=\left[\begin{array}{c}{\mathbf{p}}^{\prime }\\ 1\end{array}\right]$，那么：
$$\begin{array}{rl}\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp (\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t})+\delta \mathbf{\rho }\\ 0^T\end{array}\right]}{{\left[\begin{array}{c}\delta \mathbf{\rho },\delta \mathbf{\varphi }\end{array}\right]}^T}\\ & =\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (\mathbf{T}\mathbf{p}{)}^{\odot }\end{array}$$其中，$\odot$ 为新定义的运算符。
关于上述推导过程的说明：

• $\delta {\mathbf{\xi }}^{\wedge }\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}=\delta \left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\mathbf{p}=\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t}\\ 1\end{array}\right]$
• 根据矩阵求导，我们有：$$\frac{\mathrm{d}\left[\begin{array}{c}a\\ b\end{array}\right]}{\mathrm{d}\left[\begin{array}{c}x\\ y\end{array}\right]}={\left(\frac{\mathrm{d}[a,b{]}^T}{\mathrm{d}\left[\begin{array}{c}x\\ y\end{array}\right]}\right)}^T={\left[\begin{array}{cc}\frac{\mathrm{d}a}{\mathrm{d}x}& \frac{\mathrm{d}b}{\mathrm{d}x}\\ & \\ \frac{\mathrm{d}a}{\mathrm{d}y}& \frac{\mathrm{d}b}{\mathrm{d}y}\end{array}\right]}^T=\left[\begin{array}{cc}\frac{\mathrm{d}a}{\mathrm{d}x}& \frac{\mathrm{d}a}{\mathrm{d}y}\\ & \\ \frac{\mathrm{d}b}{\mathrm{d}x}& \frac{\mathrm{d}b}{\mathrm{d}y}\end{array}\right]$$所以：
  $$\begin{array}{r}\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t})+\delta \mathbf{\rho }\\ 0^T\end{array}\right]}{{\left[\begin{array}{c}\delta \mathbf{\rho },\delta \mathbf{\varphi }\end{array}\right]}^T}=\left[\begin{array}{cc}\frac{\partial (\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t})+\delta \mathbf{\rho })}{\partial \delta \mathbf{\rho }}& \frac{\partial (\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t})+\delta \mathbf{\rho })}{\partial \delta \mathbf{\varphi }}\\ & \\ 0^T& 0^T\end{array}\right]\end{array}$$注意到：$\frac{\partial (\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t})+\delta \mathbf{\rho })}{\partial \delta \mathbf{\rho }}=\mathbf{I}$，而 $\frac{\partial (\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t})+\delta \mathbf{\rho })}{\partial \delta \mathbf{\varphi }}$ 的计算方式如下：
  $$\frac{\partial (\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t})+\delta \mathbf{\rho })}{\partial \delta \mathbf{\varphi }}=\frac{\partial (\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t}))}{\partial \delta \mathbf{\varphi }}=\frac{\partial (-(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t}{)}^{\wedge }\delta \mathbf{\varphi })}{\partial \delta \mathbf{\varphi }}=-(\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{t}{)}^{\wedge }$$

添加右扰动的计算过程如下：
$$\begin{array}{rl}\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp ({\mathbf{\xi }}^{\wedge })\exp (\delta {\mathbf{\xi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp ({\mathbf{\xi }}^{\wedge })(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp ({\mathbf{\xi }}^{\wedge })\delta {\mathbf{\xi }}^{\wedge }\mathbf{p}}{\delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}^{\prime }\\ 1\end{array}\right]}{\delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\mathbf{R}\delta {\mathbf{\varphi }}^{\wedge }\mathbf{p}+\mathbf{R}\mathbf{\delta }\mathbf{\rho }\\ 0^T\end{array}\right]}{{\left[\begin{array}{c}\delta \mathbf{\rho },\delta \mathbf{\varphi }\end{array}\right]}^T}\\ & =\left[\begin{array}{cc}\frac{\partial (\mathbf{R}\delta {\mathbf{\varphi }}^{\wedge }\mathbf{p}+\mathbf{R}\mathbf{\delta }\mathbf{\rho })}{\partial \delta \mathbf{\rho }}& \frac{\partial (\mathbf{R}\delta {\mathbf{\varphi }}^{\wedge }\mathbf{p}+\mathbf{R}\mathbf{\delta }\mathbf{\rho })}{\partial \delta \mathbf{\varphi }}\\ & \\ 0^T& 0^T\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& -\mathbf{R}{\mathbf{p}}^{\wedge }\\ 0^T& 0^T\end{array}\right]\end{array}$$上述推导中应用了下面的等式：

• $\frac{\partial (\mathbf{R}\delta {\mathbf{\varphi }}^{\wedge }\mathbf{p}+\mathbf{R}\mathbf{\delta }\mathbf{\rho })}{\partial \delta \mathbf{\rho }}=\frac{\partial \mathbf{R}\delta \mathbf{\rho }}{\partial \delta \mathbf{\rho }}=\mathbf{R}$
• $\frac{\partial (\mathbf{R}\delta {\mathbf{\varphi }}^{\wedge }\mathbf{p}+\mathbf{R}\mathbf{\delta }\mathbf{\rho })}{\partial \delta \mathbf{\varphi }}=\frac{\partial (\mathbf{R}\delta {\mathbf{\varphi }}^{\wedge }\mathbf{p})}{\partial \delta \mathbf{\varphi }}=\frac{\partial (-\mathbf{R}{\mathbf{p}}^{\wedge }\delta \mathbf{\varphi })}{\partial \delta \mathbf{\varphi }}=-\mathbf{R}{\mathbf{p}}^{\wedge }$