《视觉SLAM十四讲》中SE(3)指数映射和左雅克比矩阵的推导

高博的书上给出了$SO(3)$的指数映射推导，但对于$SE(3)$,仅介绍了结论，没有给出详细推导。
最近在补李群和李代数基础，就当做加深理解，自己推一遍。


设$\mathbf{\xi }={\left[\rho ,\varphi \right]}^T\in \mathfrak{s}\mathfrak{e}(3)$，它的指数映射为:
$$\exp ({\mathbf{\xi }}^{\wedge })=\left[\begin{array}{cc}\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& \sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\mathbf{\rho }\\ 0^T& 1\end{array}\right]$$
令$\mathbf{\rho }=\theta \mathbf{a}$，那么:
$$\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }=\mathbf{J}.$$

下面证明：

已知${\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T,& 0\end{array}\right]$

那么
$$\begin{array}{rl}\exp ({\mathbf{\xi }}^{\wedge })& =\sum _{n=0}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T,& 0\end{array}\right]}^n\\ & =\mathbf{I}+\sum _{n=1}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \mathbf{\rho }\\ 0^T,& 0\end{array}\right]}^n\\ & =\left[\begin{array}{cc}\mathbf{I}& 0\\ 0& 1\end{array}\right]+\sum _{n=1}^{\infty }\frac{1}{n!}\left[\begin{array}{cc}(\theta {\mathbf{a}}^{\wedge }{)}^n& (\theta {\mathbf{a}}^{\wedge }{)}^{n-1}\mathbf{\rho }\\ 0^T,& 0\end{array}\right]\\ & =\left[\begin{array}{cc}\sum _{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n& \sum _{n=1}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^{n-1}\mathbf{\rho }\\ 0^T,& 1\end{array}\right]\end{array}$$

现在令$n=n-1$,那么$n=n+1$（做一步替换变量），带入上式右上角的一项，得
$$\exp ({\mathbf{\xi }}^{\wedge })=\left[\begin{array}{cc}\sum _{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n& \sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\mathbf{\rho }\\ 0^T,& 1\end{array}\right]$$

$SE(3)$的指数映射形式推导完毕。下面推导左雅克比的形式

设$\mathbf{\rho }=\theta \mathbf{a}$，

当$\Vert \mathbf{a}\Vert =1$时，已知可以用来化简的结论：
$$\begin{gathered} {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^T-\mathbf{I} \\ {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }={\mathbf{a}}^{\wedge }({\mathbf{a}}^T-\mathbf{I})=-{\mathbf{a}}^{\wedge } \end{gathered}$$
进而
$$\begin{array}{rl}\mathbf{J}& =\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & =\mathbf{I}+\frac{1}{2!}(\theta {\mathbf{a}}^{\wedge })+\frac{1}{3!}(\theta {\mathbf{a}}^{\wedge }{)}^2+\frac{1}{4!}(\theta {\mathbf{a}}^{\wedge }{)}^3+\cdots +\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & =\mathbf{I}+\frac{1}{2!}(\theta {\mathbf{a}}^{\wedge })+\frac{1}{3!}(\theta {\mathbf{a}}^{\wedge }{)}^2-\frac{1}{4!}{\theta }^3({\mathbf{a}}^{\wedge })-\frac{1}{5!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^2+\frac{1}{6!}{\theta }^5({\mathbf{a}}^{\wedge })-\frac{1}{7!}{\theta }^7({\mathbf{a}}^{\wedge }{)}^2+\cdots \\ & =\frac{1}{\theta }(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+\cdots ){\mathbf{a}}^{\wedge }+\frac{1}{\theta }(\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots )({\mathbf{a}}^{\wedge }{)}^2+\mathbf{I}\end{array}$$
由$\sin \theta$和$\cos \theta$的泰勒展开公式
$$\begin{array}{rl}\sin \theta & =\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\frac{1}{7!}{\theta }^7+\cdots \\ \cos \theta & =1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\frac{1}{6!}{\theta }^6+\cdots \end{array}$$
$\mathbf{J}$可进一步化简为
$$\begin{array}{rl}\mathbf{J}& =\frac{1}{\theta }(1-\cos \theta )({\mathbf{a}}^{\wedge })+\frac{\theta -\sin \theta }{\theta }(\mathbf{a}{\mathbf{a}}^T-\mathbf{I})+\mathbf{I}\\ & =\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\end{array}$$
左雅克比矩阵的形式推导完毕。

参考：https://blog.csdn.net/zengxyuyu/article/details/105646751#commentBox