李群李代数：李代数求导

雅可比

雅可比分为左乘雅可比和右乘雅可比，分别记为$J_l$和$J_r$。左乘雅可比即为李代数se(3)中的$J$
$$J_l=J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$
它的逆如下：
$$J_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}I+(1-\frac{\theta }{2}\cot \frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge }$$
右乘雅可比只需对自变量取负即可：
$$J_r(\varphi )=J_l(-\varphi )$$

BCH近似

李群的指数映射之积由BCH公式给出，其展开式的前几项如下：
$$\ln (exp(A)exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\cdots$$
考虑$SO(3)$上的李代数$\ln (exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }$，当${\varphi }_1$或${\varphi }_2$为小量时，二次以上的项可被忽略。此时，BCH有线性近似表达：
$$\ln (exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& {\varphi }_1为小量\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& {\varphi }_2为小量\end{array}$$

SO(3)

对$SO(3)$的BCH的近似进行定义：假设存在某个旋转$R$，对应李代数为$\varphi$。对其左乘一个微小旋转$\Delta R$，对应李代数$\Delta \varphi$。则在李群上，得到$\Delta R\cdot R$，对应李代数上，由BCH近似得到：$J_l^{-1}(\varphi )\Delta \varphi +\varphi$。
$$\begin{gathered} exp(\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((J_l^{-1}\Delta \varphi +\varphi {)}^{\vee }) \\ exp({\varphi }^{\wedge })exp(\Delta {\varphi }^{\wedge })=exp((J_r^{-1}\Delta \varphi +\varphi {)}^{\vee }) \end{gathered}$$
反之，若在李代数上进行加法，即$\varphi$加上一个$\Delta \varphi$，则由BCH近似得到李群上的乘法：
$$\begin{gathered} exp((\varphi +\Delta \varphi {)}^{\wedge })=exp((J_l\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge }) \\ exp((\varphi +\Delta \varphi {)}^{\wedge })=exp({\varphi }^{\wedge })exp((J_r\Delta \varphi {)}^{\wedge }) \end{gathered}$$

SE(3)

同样，对于$SE(3)$上的李代数也可由BCH近似得到：
$$\begin{gathered} exp(\Delta {\xi }^{\wedge })exp({\xi }^{\wedge })=exp(({\mathcal{J}}_l^{-1}\Delta \xi +\xi {)}^{\vee }) \\ exp({\xi }^{\wedge })exp(\Delta {\xi }^{\wedge })=exp(({\mathcal{J}}_r^{-1}\Delta \xi +\xi {)}^{\vee }) \end{gathered}$$
在李代数上进行加法同样可得李群上的乘法：
$$\begin{gathered} exp((\xi +\Delta \xi {)}^{\wedge })=exp(({\mathcal{J}}_l\Delta \xi {)}^{\wedge })exp({\xi }^{\wedge }) \\ exp((\xi +\Delta \xi {)}^{\wedge })=exp({\xi }^{\wedge })exp(({\mathcal{J}}_r\Delta \xi {)}^{\wedge }) \end{gathered}$$
其中${\mathcal{J}}_l$为$6\times 6$矩阵，表达式如下：
$$\begin{gathered} {\mathcal{J}}_l(\xi )=\left[\begin{array}{cc}{J}_l& Q_l\\ 0& J_l\end{array}\right] \\ {\mathcal{J}}_r(\xi )=\left[\begin{array}{cc}{J}_r& Q_r\\ 0& J_r\end{array}\right] \\ {\mathcal{J}}_r(\xi )={\mathcal{J}}_l(-\xi ) \end{gathered}$$
其中$Q$表达如下：
$$\begin{gathered} Q_l(\xi )=\sum _{n=0}^{\infty }\sum _{m=0}^{\infty }\frac{1}{(n+m+2)!}({\varphi }^{\wedge }{)}^n{\rho }^{\wedge }({\varphi }^{\wedge }{)}^m \\ {Q}_r(\xi )=Q_l(-\xi ) \end{gathered}$$
雅可比矩阵的逆如下：
$$\begin{gathered} {\mathcal{J}}_l^{-1}=\left[\begin{array}{cc}{J}_l^{-1}& -J_l^{-1}{Q}_l{J}_l^{-1}\\ 0& J_l^{-1}\end{array}\right] \\ {\mathcal{J}}_r^{-1}=\left[\begin{array}{cc}{J}_r^{-1}& -J_r^{-1}{Q}_r{J}_r^{-1}\\ 0& J_r^{-1}\end{array}\right] \end{gathered}$$

李代数求导

设空间内一点$p$进行旋转$R$，得到旋转后点$Rp$。记旋转后点相对于旋转的导数为：$\frac{\partial (Rp)}{\partial R}$

$SO(3)$无加法，该导数无法按照导数定义进行计算。设$R$对应李代数$\varphi$，则转而计算：$\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }$

求解该式可使用导数模型或扰动模型。

$SO(3)$导数模型

由导数定义进行计算：
$$\begin{array}{rl}\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{exp((\varphi +\delta \varphi {)}^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ 1:& =\underset{\delta \varphi \to 0}{\lim }\frac{exp((J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ 2:& \approx \underset{\delta \varphi \to 0}{\lim }\frac{(I+(J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ 3:& =\underset{\delta \varphi \to 0}{\lim }\frac{(J_l\delta \varphi {)}^{\wedge }exp({\varphi }^{\wedge })p}{\delta \varphi }\\ 4:& =\underset{\delta \varphi \to 0}{\lim }\frac{-(exp({\varphi }^{\wedge })p{)}^{\wedge }{J}_l\delta \varphi }{\delta \varphi }\\ 5:& =-(Rp{)}^{\wedge }{J}_l\end{array}$$
计算思路如下：

• 第一步：由李代数的加法转为李群的乘法
• 第二步：对$exp(J_l\delta \varphi {)}^{\wedge }$进行一阶泰勒展开
• 第三步：约去相同项
• 第四步：反对称符号实际为叉乘计算，由$a\times b=-b\times a$知$a^{\wedge }b=-b^{\wedge }a$，代入第三步分子
• 第五步：约去分母项，并将李代数写回李群

从而得到：$\frac{\partial (Rp)}{\partial R}=-(Rp{)}^{\wedge }{J}_l$

导数模型含有雅可比项，计算较为复杂，一般采用扰动模型进行计算。

SO(3)扰动模型（左乘）

对于旋转$R$，假设它受到一个扰动$\Delta R$，对应的李代数为$\phi$。求导也即看扰动结果相对于扰动的变换率。此处采用左扰动模型，也即左乘一个扰动$\Delta R$
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial R}& =\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }\\ 1:& \approx \underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }\\ 2:& =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }\\ 3:& =\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }\\ 4:& =-(Rp{)}^{\wedge }\end{array}$$
计算思路如下：

• 第一步：对$exp({\phi }^{\wedge })$进行一阶泰勒展开
• 第二步：约去相同项，将李代数写回李群
• 第三步：左右互换
• 第四步：约去坟墓想，得到结果。

从而得到左扰动模型：$\frac{\partial (Rp)}{\partial R}=-(Rp{)}^{\wedge }$

$SE(3)$扰动模型（左乘）

假设空间某点$p$经过变换$T$后得到点$Tp$，变换$T$对应的李代数为$\xi$。现在左乘一个扰动$\Delta T=exp(\delta {\xi }^{\wedge })$，对应李代数为$\delta \xi ={\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T$，则：
$$\begin{array}{rl}\frac{\partial (Tp)}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{\lim }\frac{exp(\delta {\xi }^{\wedge })exp({\xi }^{\wedge })p-exp({\xi }^{\wedge })p}{\delta \xi }\\ 1:& \approx \underset{\delta \xi \to 0}{\lim }\frac{(I+\delta {\xi }^{\wedge })exp({\xi }^{\wedge })p-exp({\xi }^{\wedge })p}{\delta \xi }\\ 2:& =\underset{\delta \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }exp({\xi }^{\wedge })p}{\delta \xi }\\ 3:& =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}exp({\varphi }^{\wedge })& J\rho \\ 0^T& 1\end{array}\right]p}{\delta \xi }\\ 4:& =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}Rp+t\\ 1\end{array}\right]}{\delta \xi }\\ 5:& =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t)+\delta \rho \\ 0\end{array}\right]}{\left[\begin{array}{c}\delta \rho \\ \delta \varphi \end{array}\right]}\\ 6:& =\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\\ 7:& \triangleq (Tp{)}^{\odot }\end{array}$$
计算思路如下：

• 第一步：对$exp(\delta {\xi }^{\wedge })$进行一阶泰勒展开
• 第二步：约去相同项
• 第三步：将${\xi }^{\wedge }$矩阵形式以及$SE(3)$的指数映射代入替换
• 第四步：指数映射同点坐标（齐次）进行矩阵乘法
• 第五步：分子矩阵乘法，分母写为矩阵形式
• 第六步：计算
• 第七步：定义算符$\odot$表示将一个齐次坐标写为$4\times 6$的矩阵形式

应注意矩阵求导的顺序，若$a、b、x、y$为列向量，则有：
$$\frac{d\left[\begin{array}{c}a\\ b\end{array}\right]}{d\left[\begin{array}{c}x\\ y\end{array}\right]}=(\frac{d{\left[\begin{array}{cc}a& b\end{array}\right]}^T}{d\left[\begin{array}{c}x\\ y\end{array}\right]}{)}^T={\left[\begin{array}{cc}\frac{da}{dx}& \frac{db}{dx}\\ \frac{da}{dy}& \frac{db}{dy}\end{array}\right]}^T=\left[\begin{array}{cc}\frac{da}{dx}& \frac{da}{dy}\\ \frac{db}{dx}& \frac{db}{dy}\end{array}\right]$$