VIO学习之公式推导——李代数篇

1、李代数公式基本性质：

1、扰动：$R^{-1}\to (Rexp({\varphi }^{\wedge }){)}^{-1}=exp({\varphi }^{\wedge }{)}^{-1}{R}^{-1}=exp({\varphi }^{\wedge }{)}^T{R}^{-1}=exp(-{\varphi }^{\wedge })R^{-1}$
上面用到了反对称矩阵的性质
2、叉乘交换：$a\times b=-b\times a$
3、右扰动对数近似：$ln(Rexp({\varphi }^{\wedge }){)}^{\vee }\approx ln(R{)}^{\vee }+J_r^{-1}(ln(R{)}^{\vee })\varphi$
其中$$J_r^{-1}(\theta w)=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})w{w}^T+\frac{\theta }{2}{w}^{\wedge }$$
4、左扰动对数近似：$ln(exp({\varphi }^{\wedge })R{)}^{\vee }\approx J_l^{-1}(ln(R{)}^{\vee })\varphi +ln(R{)}^{\vee }$
5、 $R{p}^{\wedge }{R}^T=(Rp{)}^{\wedge }$:证明请参考这里
6、SO(3)伴随性质：$Rexp({\varphi }^{\wedge })R^T=exp((R\varphi {)}^{\wedge })$
7、SE(3)伴随性质：$Texp({\varphi }^{\wedge })T^T=exp((Ad(T)\varphi {)}^{\wedge })$
其中
$$d(T)=\left[\begin{array}{cc}R& t^{\wedge }R\\ 0& R\end{array}\right]$$

2、常见雅可比(以自变量R举例)

1、旋转点的左扰动雅可比：
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial R}& =\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })Rp-Rp}{\partial \phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })Rp-Rp}{\partial \phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\partial \phi }\\ & =\frac{-(Rp{)}^{\wedge }\phi }{\partial \phi }\\ & =-(Rp{)}^{\wedge }\end{array}$$
2、旋转点的右扰动雅可比：
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial R}& =\underset{\phi \to 0}{\lim }\frac{Rexp({\phi }^{\wedge })p-Rp}{\partial \phi }\\ & =\underset{\phi \to 0}{\lim }\frac{R(I+{\phi }^{\wedge })p-Rp}{\partial \phi }\\ & =\underset{\phi \to 0}{\lim }\frac{R{\phi }^{\wedge }p}{\partial \phi }\\ & =-\frac{R{p}^{\wedge }\phi }{\partial \phi }\\ & =-R{p}^{\wedge }\end{array}$$
3、旋转连乘雅可比：
$$\begin{array}{rl}\frac{dln(R_1{R}_2{)}^{\vee }}{d{R}_2}& =\underset{\varphi \to 0}{\lim }\frac{ln(R_1{R}_{2}exp({\varphi }^{\wedge }){)}^{\vee }-ln(R_1{R}_2{)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{ln(R_1{R}_2{)}^{\vee }+J_r^{-1}\varphi -ln(R_1{R}_2{)}^{\vee }}{\varphi }\\ & =J_r^{-1}(ln(R_1{R}_2{)}^{\vee })\end{array}$$
其中用到了性质3

4、旋转连乘的雅可比：
$$\begin{array}{rl}\frac{dln(R_1{R}_2{)}^{\vee }}{d{R}_1}& =\underset{\varphi \to 0}{\lim }\frac{ln(R_{1}exp({\varphi }^{\wedge })R_2{)}^{\vee }-ln(R_1{R}_2{)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{ln(R_1{R}_2{R}_2^{T}exp({\varphi }^{\wedge })R_2{)}^{\vee }-ln(R_1{R}_2{)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{ln(R_1{R}_{2}exp((R_2^T\varphi {)}^{\wedge }){)}^{\vee }-ln(R_1{R}_2{)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{ln(R_1{R}_2{)}^{\vee }+J_r^{-1}{R}_2^T\varphi -ln(R_1{R}_2{)}^{\vee }}{\varphi }\\ & =J_r^{-1}(ln(R_1{R}_2{)}^{\vee })R_2^T\end{array}$$

其中用到了性质3和性质6

5、旋转点逆的右扰动：
$$\begin{array}{rl}\frac{d{R}^{-1}p}{dR}& =\underset{\phi \to 0}{\lim }\frac{(Rexp({\phi }^{\wedge }){)}^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{exp(-{\phi }^{\wedge })R^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(I-{\phi }^{\wedge })R^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-{\phi }^{\wedge }{R}^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(R^{-1}p{)}^{\wedge }\phi }{\phi }\\ & =(R^{-1}p{)}^{\wedge }\end{array}$$

6、连乘旋转点逆的右扰动
$$\begin{array}{rl}\frac{dln(R_1{R}_2^{-1}{)}^{\vee }}{d{R}^2}& =\underset{\phi \to 0}{\lim }\frac{ln(R_1(R_{2}exp({\phi }^{\wedge }){)}^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_{1}exp(-{\phi }^{\wedge })R_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}{R}_{2}exp(-{\phi }^{\wedge })R_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}exp(-(R_2\phi {)}^{\wedge }){)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}{)}^{\vee }-J_r^{-1}{R}_2\phi -ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-J_r^{-1}{R}_2\phi }{\phi }\\ & =-J_r^{-1}{R}_2\end{array}$$
其中用到了性质3和性质6