《视觉SLAM十四讲》学习笔记-李代数求导与扰动模型的一些重要公式

1. BCH(Baker-Campbell-Hausdorff)公式：
   $$\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$$

当为SO(3)上的李代数$\ln (\exp (\Phi_1^\wedge) \exp (\Phi_2^\wedge) )^\vee$时，BCH变为：

$$\ln (\exp (\Phi_1^\wedge) \exp (\Phi_2^\wedge) )^\vee \approx \begin{cases} \mathbf{J}_l(\Phi_2)^{-1}\Phi_1 + \Phi_2 & if~\Phi_1 ~is ~small,\\ \mathbf{J}_r(\Phi_1)^{-1}\Phi_2 + \Phi_1 & if~\Phi_2 ~is ~small. \end{cases}$$
这其中，左乘BCH近似的Jacobi $\mathbf{J}_l$为：
$$\mathbf{J}_l = \mathbf{J} = \frac{\sin \theta}{\theta}\mathbf{I} + \left( 1-\frac{\sin\theta}{\theta} \right)\vec{a}\vec{a}^\top + \frac{1-\cos\theta}{\theta}\vec{a}^\wedge$$
$\mathbf{J}_l$的逆为
$$\mathbf{J}_l^{-1} = \frac{\theta}{2}\cot\frac{\theta}{2}\mathbf{I} + \left( 1-\frac{\theta}{2}\cot \frac{\theta}{2} \right)\vec{a}\vec{a}^\top -\frac{\theta}{2}\vec{a}^\wedge$$
另外， 右乘Jacobi $\mathbf{J}_r$可对自变量取负号即可得到：
$$\mathbf{J}_r(\Phi) = \mathbf{J}_l(-\Phi)$$

1. BCH近似的意义. 旋转$\mathbf{R}$对应的李代数为$\Phi$, 左乘微小旋转$\triangle\mathbf{R}$所对应的李代数为$\triangle\Phi$.
   则李群上的结果为$\triangle\mathbf{R}\cdot\mathbf{R}$,李代数的结果根据BCH变为$\mathbf{J}_l^{-1}(\Phi)\triangle\Phi + \Phi$:
   

   $$\exp(\triangle\Phi^\wedge)\exp(\Phi^\wedge) = \exp\left( (\Phi+ \mathbf{J}_l^{-1}(\Phi)\triangle\Phi)^\wedge \right)$$
   若在李代数上做加法， $\Phi\rightarrow \Phi + \triangle \Phi$:
   $$\exp \left( (\Phi + \triangle\Phi)^\wedge \right) = \exp\left( (\mathbf{J}_l\triangle\Phi)^\wedge\right)\exp(\Phi^\wedge) = \exp(\Phi^\wedge)\exp\left( (\mathbf{J}_r\triangle\Phi)^\wedge \right)$$

2. SO(3)李代数的求导。两种思路：

   • [x] 用李代数表示姿态，根据李代数加法对李代数求导；
   • [x] 对李群左乘或右乘微小的扰动，对该扰动求导.

3.1 问题：对一个空间点$\vec{p}$进行旋转，得到$\mathbf{R}\vec{p}$,求旋转后的点相对于旋转的导数，即$\frac{\partial\mathbf{R}\vec{p}}{\partial\mathbf{R}}$.
设 $\mathbf{R}$对应的李代数为$\Phi$, 则问题表述为:

$$\frac{\partial(\exp(\Phi^\wedge)\vec{p})}{\partial \Phi}$$

$$\begin{aligned} \frac{\partial(\exp(\Phi^\wedge)\vec{p})}{\partial \Phi} &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{\exp \left( (\Phi+\delta\Phi)^\wedge\right)\vec{p} -\exp(\Phi^\wedge)\vec{p}}{\delta\Phi}\\ &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{ \exp \left( (\mathbf{J}_l\delta\Phi)^\wedge\right)\exp(\Phi^\wedge)\vec{p} -\exp(\Phi^\wedge)\vec{p}}{\delta\Phi} ~~(\text{BCH linear approximation})\\ &\approx \lim\limits_{\delta\Phi\rightarrow 0} \frac{ \mathbf{I} + \left( (\mathbf{J}_l\delta\Phi)^\wedge\right)\exp(\Phi^\wedge)\vec{p} -\exp(\Phi^\wedge)\vec{p}}{\delta\Phi}~~(\text{Taylor's formula}) \\ &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{ (\mathbf{J}_l\delta\Phi)^\wedge \exp(\Phi^\wedge)\vec{p} }{\delta\Phi} \\ &= \lim\limits_{\delta\Phi\rightarrow 0} \frac{ - (\exp(\Phi^\wedge)\vec{p})^\wedge \mathbf{J}_l\delta\Phi }{\delta\Phi}~~(\text{cross product}) \\ &= -(\mathbf{R}\vec{p})^\wedge\mathbf{J}_l. \end{aligned}$$

所以旋转后的点相对于李代数的导数为：

$$\frac{\partial(\mathbf{R}\vec{p})}{\partial\Phi} = (-\mathbf{R}\vec{p})^\wedge\mathbf{J}_l$$

3.2 扰动模型的求导公式推导

问题：对$\mathbf{R}$的一次扰动量为$\triangle\mathbf{R}$。设左扰动$\triangle\mathbf{R}$对应的李代数为$\varphi$，对$\varphi$求导即为问题解：

$$\frac{\partial\mathbf{R\vec{p}}}{\partial\varphi}=\lim\limits_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}.$$
$$\begin{aligned} \frac{\partial\mathbf{R\vec{p}}}{\partial\varphi} &=\lim\limits_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}\\ &\approx \lim\limits_{\varphi\rightarrow 0} \frac{ (1 + \varphi^\wedge) \exp(\Phi^\wedge)\vec{p} - \exp(\Phi^\wedge)\vec{p} }{\varphi}\\ &= \lim\limits_{\varphi\rightarrow 0} \frac{\varphi^\wedge \exp(\Phi^\wedge)\vec{p} }{\varphi} = \lim\limits_{\varphi\rightarrow 0} \frac{\varphi^\wedge \mathbf{R}\vec{p} }{\varphi}\\ &= \lim\limits_{\varphi\rightarrow 0} \frac{ - (\mathbf{R}\vec{p})^\wedge \varphi }{\varphi}=-(\mathbf{R}\vec{p})^\wedge. \end{aligned}$$