BCH公式

两个李代数指数映射乘积，由BCH公式给出：
$$\ln \left(\exp \left(\mathbf{A}\right)\exp \left(\mathbf{B}\right)\right)=\mathbf{A}+\mathbf{B}+\frac{1}{2}\left[\mathbf{A},\mathbf{B}\right]+\frac{1}{12}\left[\mathbf{A},\left[\mathbf{A},\mathbf{B}\right]\right]-\frac{1}{12}\left[\mathbf{B},\left[\mathbf{A},\mathbf{B}\right]\right]+...$$
其中 [] 为李括号。

考虑 SO(3) 上的李代数 ${\ln \left(\exp \left({\mathbf{\varphi }}_1^{\wedge }\right)\exp \left({\mathbf{\varphi }}_2^{\wedge }\right)\right)}^{\vee }$，当 ${\mathbf{\varphi }}_1$ 或 ${\mathbf{\varphi }}_2$ 为小量时，小量二次以上的项都可以被忽略掉。此时，BCH 拥有线性近似表达：
$${\ln \left(\exp \left({\mathbf{\varphi }}_1^{\wedge }\right)\exp \left({\mathbf{\varphi }}_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{cc}{\mathbf{J}}_l{\left({\mathbf{\varphi }}_2^{}\right)}^{-1}{\mathbf{\varphi }}_1^{}+{\mathbf{\varphi }}_2^{}& 当{\mathbf{\varphi }}_1^{}为小量\\ {\mathbf{J}}_l{\left({\mathbf{\varphi }}_1^{}\right)}^{-1}{\mathbf{\varphi }}_2^{}+{\mathbf{\varphi }}_1^{}& 当{\mathbf{\varphi }}_2^{}为小量\end{array}$$
左乘 BCH 近似雅可比 ${\mathbf{J}}_l$
$${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$
​ 逆为：
$${{\mathbf{J}}_l}^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$
而右乘雅可比仅需要对自变量取负号即可：
$${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$$

BCH近似的意义：

扰动李代数求导

左扰动雅可比

$$\begin{gathered} \frac{\partial \left(\mathbf{Rp}\right)}{\partial \mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{Rp}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{-{\left(\mathbf{Rp}\right)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }} \\ =-{\left(\mathbf{Rp}\right)}^{\wedge } \end{gathered}$$

右扰动雅可比

$$\begin{gathered} \frac{\partial \left(\mathbf{Rp}\right)}{\partial \mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\varphi }}^{\wedge }\right)\exp \left({\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\varphi }}^{\wedge }\right)\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\mathbf{R}{\mathbf{\phi }}^{\wedge }\mathbf{p}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{-\mathbf{R}{\mathbf{p}}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }} \\ =-\mathbf{R}{\mathbf{p}}^{\wedge } \end{gathered}$$

旋转连乘的雅可比

$$\begin{gathered} \frac{d\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }}{d{\mathbf{R}}_2}=\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\ln ({\mathbf{R}}_1{\mathbf{R}}_2\exp ({\mathbf{\varphi }}^{\wedge }){)}^{\vee }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }}{\mathbf{\varphi }} \\ =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }+{\mathbf{J}}_r^{-1}\mathbf{\varphi }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }}{\mathbf{\varphi }} \\ ={\mathbf{J}}_r^{-1}(\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }) \end{gathered}$$

注：${\mathbf{J}}_r^{-1}(\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee })$ 的意思是计算 $\mathbf{\phi }=\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }$，根据 $\mathbf{\phi }$ 来计算 ${\mathbf{J}}_r^{-1}$ 。

其中用到了对 $\forall \mathbf{R}$ :
$$\ln (\mathbf{R}\exp ({\mathbf{\varphi }}^{\wedge }){)}^{\vee }=\ln (\mathbf{R}{)}^{\vee }+{\mathbf{J}}_r^{-1}\mathbf{\varphi }$$
和 ${\mathbf{J}}_r^{-1}$ 为 SO(3) 上的右雅可比：
$${\mathbf{J}}_r^{-1}(\theta \mathbf{\omega })=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\mathbf{\omega }{\mathbf{\omega }}^T+\frac{\theta }{2}{\mathbf{\omega }}^{\wedge }$$

旋转连乘的雅可比

$$\begin{gathered} \frac{d\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }}{d{\mathbf{R}}_1}=\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\ln ({\mathbf{R}}_1\exp ({\mathbf{\varphi }}^{\wedge }){\mathbf{R}}_2{)}^{\vee }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }}{\mathbf{\varphi }} \\ =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\ln \left({\mathbf{R}}_1{\mathbf{R}}_2\exp \right(({\mathbf{R}}_2^T\mathbf{\varphi }{)}^{\wedge }){)}^{\vee }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }}{\mathbf{\varphi }} \\ ={\mathbf{J}}_r^{-1}(\ln ({\mathbf{R}}_1{\mathbf{R}}_2{)}^{\vee }){\mathbf{R}}_2^T \end{gathered}$$

​ 这里用到了 SO(3) 的伴随性质：
$${\mathbf{R}}^T\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{R}=\exp (({\mathbf{R}}^T\mathbf{\varphi }{)}^{\wedge })$$
​ 有关 SE(3)：由于 SE(3) 李代数性质复杂，在 VIO 中，我们通常使用SO(3)和 $\mathbf{t}$ 的形式表达旋转和平移。对平移部分使用矢量更新而非SE(3)上的更新。

SO(3)和SE(3)上伴随矩阵的性质

证明：TODO。