SLAM中求导相关的公式总结

李代数与李群的关系
$\dot{R}{R}^T$ 是一个反对称矩阵，所以这个矩阵可以用一个1×3向量进行反对称来表示
$\dot{R}{R}^T={\Phi }^{\hat{}}$ ，
根据十四讲 4.8 的推导，最后则有 $\dot{R(t)}={\Phi }^{\hat{}}\cdot R(t)$
这个李代数 $\Phi$ 反映了 $R$ 的导数性质，所以李代数是李群 $SO(3)$ 的正切空间，因为这里李群是9维的，所以切向量也是一个空间

李代数由一个方向和夹角构成，$\Phi =\theta a，||\vec{a}||=1$
指数映射，也是罗德里格斯公式
$exp({\Phi }^{\wedge })=exp(\theta a^{\wedge })=cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }=R$
$exp({\Phi }^{\wedge })=R$
意思就是李代数 $\Phi$ 可以由角度 $\theta$ 和方向向量 $\vec{a}$ 表示，通过罗德里格斯公式可以变换成对应的旋转矩阵 $SO(3)$

用李代数表示旋转会有个问题，就是周期性，就是多个李代数可以对应一个旋转矩阵，如果固定旋转角度在±π时就是唯一对应的

由旋转矩阵 $R$ 求李代数 $\Phi$ ，$\theta =arccos\frac{tr(R)-1}{2},Ra=a$，由 $ln(R{)}^{\vee }=\Phi$ 表示

BCH一阶线性近似表达
$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }$

当左边的 ${\varphi }_1$ 为小量时，此时相当于左乘，则
$=>J_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2$
其实就是相当于在 ${\varphi }_2$ 的基础上加上微量 $J_l({\varphi }_2{)}^{-1}{\varphi }_1$

当右边的 ${\varphi }_2$ 为小量时，此时相当于右乘，则
$=>J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1$

SLAM中我们构建了与位姿相关的函数后，需要讨论该函数对位姿的求导，以估计当前值
有两种方法求导
1、用李代数表示位姿，根据李代数加法对李代数求导
2、对李群进行左乘或右乘进行扰动，对扰动求导

由于使用李代数求导要计算 雅可比 $J$ ，这个形式比较复杂，工程中不用这个方法，都是用扰动模型，所以只看扰动模型
$\frac{\partial (Rp)}{\partial \phi }=li{m}_{\phi \to 0}\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }$
$exp({\phi }^{\wedge })$ 是微量，相当于是对旋转的导数，则等于 $(I+{\phi }^{\wedge })$
具体推导如下
左乘扰动求导
这里是旋转 $R$ 对向量 $p$ 进行旋转，不停地左乘扰动来改变向量 $p$ 的方向，左扰动 $\Delta R$ 对应的李代数 $\phi$ ，$Rp$ 的结果也是向量

$\frac{\partial (Rp)}{\partial \phi }=li{m}_{\phi \to 0}\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }$
$=li{m}_{\phi \to 0}\frac{(I+{\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }$
乘进去相减，很明显
$=li{m}_{\phi \to 0}\frac{{\phi }^{\wedge }Rp}{\phi }$

叉乘有一个性质，$\vec{a}\times \vec{b}=-\vec{b}\times \vec{a}$
$\vec{a}\times \vec{b}=a^{\wedge }\cdot b$
$-\vec{b}\times \vec{a}=-b^{\wedge }\cdot a$
$a^{\wedge }\cdot b=-b^{\wedge }\cdot a$

则上式等于
$=li{m}_{\phi \to 0}\frac{-(Rp{)}^{\wedge }\phi }{\phi }$
约掉 $\phi$ 则 $li{m}_{\phi \to 0}=-(Rp{)}^{\wedge }$
这里省去了雅可比 $J$ 的计算
对右乘也是一样的方法

矩阵转置的性质
$(A+B{)}^T=A^T+B^T$
$(\lambda A{)}^T=\lambda A^T$
$(AB{)}^T=B^T{A}^T$

SO(3)的伴随性质
$R^{T}Exp(\varphi )R=Exp(R^T\varphi )$
$\varphi$ 为扰动量 $\Delta R$ 对应的李代数

对复合旋转进行求导

$\frac{\partial Log(R_1{R}_2)}{\partial R_1}$

这里是对两个相乘的旋转矩阵中的其中一个旋转进行求导，上面的左乘扰动例子是对矩阵相乘向量进行求导的，所以可以直接对矩阵进行扰动

但是这里不能直接对矩阵 $SO(3)$ 进行扰动，不能直接说$R_1{R}_2$ 对 $R_1$ 或 $R_2$ 的导数，这样就变成矩阵对向量的求导，因为扰动量是可以用向量表示的，前面的例子 $Rp$ 相乘后也是个向量，所以可以直接对扰动量进行求导，但是这里两个矩阵相乘还是矩阵，所以得用 $Log$ 将矩阵相乘结果变为李代数，这样才符合求导的定义
$Log(R)=log(R{)}^{\vee }$，结果是向量，用 $Log$ 就是为了懒得写 $\vee$
$Exp(w)=exp(w^{\wedge })$，结果是矩阵

对 $R_1$ 进行右扰动
$\frac{\partial Log(R_1{R}_2)}{\partial R_1}=li{m}_{\phi \to 0}\frac{Log(R_{1}Exp(\varphi )R_2)-Log(R_1{R}_2)}{\varphi }$ ①

利用 $SO(3)$ 的伴随性质：
$R^{T}Exp(\varphi )R=Exp(R^T\varphi )$

把 $R^T$ 换到另一边

$=>Exp(\varphi )R=RExp(R^T\varphi )$

所以 $Exp(\varphi )R_2=R_{2}Exp(R_2^T\varphi )$

$\frac{\partial Log(R_1{R}_2)}{\partial R_1}=li{m}_{\phi \to 0}\frac{Log(R_1{R}_{2}Exp(R_2^T\varphi ))-Log(R_1{R}_2)}{\varphi }$ ②

然后利用BCH的一阶线性近似，现在的公式形式相当于小量在最右边，则利用
$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }=J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1$

统一表达式的话就是

$Log(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }))=J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1$

也等于

$=>Log(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }))=J_r(Log({\varphi }_1^{\wedge }){)}^{-1}Log({\varphi }_2^{\wedge })+Log({\varphi }_1^{\wedge })$

所以上面的公式推导为
$Log(R_1{R}_{2}Exp(R_2^T\varphi ))=J_r^{-1}(Log(R_1{R}_2))Log(Exp(R_2^T\varphi ))+Log(R_1{R}_2)$ ③

代入公式②，把 $Log(R_1{R}_2)$ 减掉，把 $\varphi$ 消掉，得：

$=>J_r^{-1}(Log(R_1{R}_2))R_2^T$