左右扰动问题

左右扰动问题

​ 左右绕动都是函数在线性化点处的展开，因为姿态数据是不符合向量直接的加法的，所有从左边增加的扰动量和从右边加的扰动量结果会不一致

左右绕动关系

结论:

​ $Exp(左)\ast X=X\ast Exp(右)$

​ $Exp(左)=X\ast Exp(右)\ast X^{-1}$

​ $左 = Adj(X) * 右 $

关系验证

$err=R_{12}^T{R}_1{R}_2$

左右扰动的雅可比矩阵

对$R_1$的左雅可比矩阵:

$\frac{\partial err}{\partial R_1}=\frac{ln(R_{12}^{T}Exp(\delta \theta )R_1{R}_2)-ln(R_{12}^T{R}_1{R}_2)}{\delta \theta }$

$=\frac{ln(R_{12}^T{R}_1{R}_{2}Exp((R_1{R}_2{)}^T\delta \theta ))-ln(R_{12}^T{R}_1{R}_2)}{\delta \theta }$

$=J_r^{-1}(R_{12}^T{R}_1{R}_2)(R_1{R}_2{)}^T$

对$R_1$的右雅可比矩阵:

$\frac{\partial err}{\partial R_1}=\frac{ln(R_{12}^T{R}_{1}Exp(\delta \theta )R_2)-ln(R_{12}^T{R}_1{R}_2)}{\delta \theta }$

$=\frac{ln(R_{12}^T{R}_1{R}_{2}Exp(R_2^T\delta \theta ))-ln(R_{12}^T{R}_1{R}_2)}{\delta \theta }$

$=J_r^{-1}(R_{12}^T{R}_1{R}_2)R_2^T$

左右扰动雅可比矩阵关系验证

关系验证:

​ 根据第一小结得到的左扰动模型和右扰动模型关系

​ $Exp(左)\ast X=X\ast Exp(右)$ 得到的$左=Adj右$ 对于旋转来说$Adj=X$

​ 所以观察对$R_1$ 的左右绕动，正好差一个$Adj(R_1)$

右雅可比矩阵用于左乘更新

​ 目的:为了手动求解雅可比矩阵更见简单！

​ 个人感觉采用左雅可比矩阵和右雅可比矩阵都是可以的，只要统一就行了(左绕动左乘更新、右绕动右乘更新)，涉及到不统一的地方就需要做转换。比如在$err$ 对$R_2$的雅可比矩阵推倒中，明显右绕动更见简单，但如果我系统设置的整体是一个左绕动更新，那就需要转换了。

转换1从jacobian入手

​ 虽然推倒出了右绕动，但是可以利用左右绕动之间的关系进行转换就可以直接对Jacobian转换成左绕动的Jacobian

​ $\frac{\partial err}{\partial {\theta }_l}=\frac{\partial err}{\partial {\theta }_r}\ast \frac{\partial {\theta }_r}{\partial {\theta }_l}$ 其中$\frac{\partial {\theta }_r}{\partial {\theta }_l}=Ad{j}^{-1}(X)$

转换2从$\delta x$入手

​ 用右绕动雅可比求解出来的$\delta x$是右绕动增量，通过$左=Adj(X)右$可以把右绕动增量转换为左绕动增量，再送到定义的广义的+函数里面

从求解式子入手

​ 在GN方法中，求解 $H\Delta x=b$ 将$右=Ad{j}^{-1}\ast 左$ 带入

​ $HAd{j}^{-1}\delta x_l=b$ 左右均添加一个系数 $(Ad{j}^{-1}{)}^{T}HAd{j}^{-1}\delta x_l=(Ad{j}^{-1}{)}^{T}b$ (这里加一个系数是为了保证新H的对称)

​ 同时再展开一下$(Ad{j}^{-1}{)}^T{J}^{T}JAd{j}^{-1}\delta x_l=(Ad{j}^{-1}{)}^T{J}^T\ast res$ 可以看出 其实是对雅可比矩阵改变是等效的！