视觉SLAM十四讲解读-(v2.p85)SE(3)上的李代数求导

1. 问题背景和目标

在 $SE(3)$ 上考虑扰动模型，不再介绍直接李代数上的求导。假设空间点 $\mathbf{p}$ 经过变换 $T$（对应李代数为 $\xi$）得到 $T\mathbf{p}$，对 $T$ 左乘一个扰动 $\Delta T=\exp (\delta {\xi }^{\wedge })$，设扰动项的李代数为 $\delta \xi =[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T$，目标是计算 $\frac{\partial (T\mathbf{p})}{\partial \delta \xi }$。

2. 根据导数定义展开

按照导数的定义：
$\frac{\partial (T\mathbf{p})}{\partial \delta \xi }={\lim }_{\delta \xi \to 0}\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\mathbf{p}-\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }$
这一步遵循导数定义，分子是函数在有扰动（$\delta \xi$ 不为 0）和无扰动（$\delta \xi =0$）时的函数值之差，分母是扰动李代数的增量 $\delta \xi$，通过取极限 $\delta \xi \to 0$ 来得到导数。

3. 利用近似展开

当 $\delta \xi$ 很小时，$\exp (\delta {\xi }^{\wedge })\approx \mathbf{I}+\delta {\xi }^{\wedge }$，则：
${\lim }_{\delta \xi \to 0}\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\mathbf{p}-\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }={\lim }_{\delta \xi \to 0}\frac{(\mathbf{I}+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\mathbf{p}-\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }$
这里将 $\exp (\delta {\xi }^{\wedge })$ 用近似式替换，为后续化简做准备。

4. 化简分子

对分子进行化简：
$(\mathbf{I}+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\mathbf{p}-\exp ({\xi }^{\wedge })\mathbf{p}=\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })\mathbf{p}$
所以原式变为：
${\lim }_{\delta \xi \to 0}\frac{\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }$
这一步通过简单的代数运算，消去了分子中的 $\mathbf{I}\exp ({\xi }^{\wedge })\mathbf{p}$ 与 $-\exp ({\xi }^{\wedge })\mathbf{p}$，得到剩余部分。

5. 展开 $\delta {\xi }^{\wedge }$ 和进一步化简

设 $\delta \xi =[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T$，则 $\delta {\xi }^{\wedge }=\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]$，设 $R=\exp ({\mathbf{\varphi }}^{\wedge })$，$\mathbf{t}$ 是平移向量（$T=\left[\begin{array}{cc}R& \mathbf{t}\\ 0^T& 1\end{array}\right]$），$\exp ({\xi }^{\wedge })\mathbf{p}=\left[\begin{array}{c}R\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]$。
$\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })\mathbf{p}=\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}R\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]=\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(R\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]$
则：
${\lim }_{\delta \xi \to 0}\frac{\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }={\lim }_{\delta \xi \to 0}\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(R\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]}{\delta \xi }$
$={\lim }_{\delta \xi \to 0}\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(R\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]}{[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T}$

6. 利用矩阵求导规则和极限运算

根据矩阵求导规则，对于 $\frac{d\left[\begin{array}{c}\mathbf{a}\\ \mathbf{b}\end{array}\right]}{d\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]}=\left[\begin{array}{cc}\frac{d\mathbf{a}}{d\mathbf{x}}& {\frac{d\mathbf{a}}{d\mathbf{y}}}^T\\ \frac{d\mathbf{b}}{d\mathbf{x}}& {\frac{d\mathbf{b}}{d\mathbf{y}}}^T\end{array}\right]$，这里 $\mathbf{a}=\delta {\mathbf{\varphi }}^{\wedge }(R\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }$，$\mathbf{b}=0$，$\mathbf{x}=\delta \mathbf{\rho }$，$\mathbf{y}=\delta \mathbf{\varphi }$。
$\frac{d\mathbf{a}}{d\mathbf{\rho }}=\mathbf{I}$，$\frac{d\mathbf{a}}{d\mathbf{\varphi }}=\left[\begin{array}{cc}\mathbf{I}& -(R\mathbf{p}+\mathbf{t}{)}^{\wedge }\end{array}\right]$（通过求导运算得到），$\frac{d\mathbf{b}}{d\mathbf{\rho }}=0$，$\frac{d\mathbf{b}}{d\mathbf{\varphi }}=0$。
所以：
${\lim }_{\delta \xi \to 0}\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(R\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]}{[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T}={\left[\begin{array}{cc}\mathbf{I}& -(R\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]}^{\odot }=(T\mathbf{p}{)}^{\odot }$
这里最后的结果定义了一个算符 $\odot$，它将一个齐次坐标的空间点变换成一个 $4\times 6$ 的矩阵。

综上，通过以上详细推导步骤，得到了在 $SE(3)$ 扰动模型下 $\frac{\partial (T\mathbf{p})}{\partial \delta \xi }=(T\mathbf{p}{)}^{\odot }$，即变换之后点的坐标相对于扰动李代数的导数表达式。