open_vins icon indicating copy to clipboard operation
open_vins copied to clipboard

Question on dynamic initialization jacobian

Open precyon opened this issue 1 month ago • 0 comments

To the authors and the community,

I have some trouble deriving on of the error jacobians in the dynamic initialization. In particular, the jacobian of the error w.r.t rotation component of the state as coded here. It says,

H_theta = -Jr_inv * R_lin.transpose();

From your technical report, the error is defined as global error $^G {\delta\theta}$. The state to be estimated is $^I_G{R}$. So the perturbations are right-sided. Is that correct?

Now $^G{\delta\theta} = \log(^I_G{R}^T{^I_G{R}_\textrm{lin}})$. So the jacobian w.r.t the state $^I_G{R}$ should be the following through chain-rule.

$\frac{\partial \log{^G{\delta\theta}}}{\partial ^G{\delta\theta}} \times \frac{\partial ^I_G{R}^T{^I_G{R}_\textrm{lin}}}{\partial ^I_G{R}^T} \times \frac{\partial ^I_G{R}^T}{\partial ^I_G{R}}$

Which should be (switching back to code for brevity),

Jr_inv * R_lin.transpose() * (-R_i) 

Where we are using the following for elementary jacobians. While they can be derived through perturbing the corresponding expressions on the right, I will refer to Eqn 140 and 141 in Sola et al.

  1. $J^{QR}_Q = R^T$
  2. $J^{R^T}_{R} = -R$

So finally I have,

H_theta = - Jr_inv * R_lin.transpose() * R_i 

which differs from the code by having an extra R_i on the right.

What am I missing in my understanding? I did find and look at 276 but that didn't quite address it fully.

precyon avatar May 30 '24 05:05 precyon