rpng / open_vins

An open source platform for visual-inertial navigation research.
https://docs.openvins.com
GNU General Public License v3.0
2.16k stars 636 forks source link

Question on dynamic initialization jacobian #447

Open precyon opened 4 months ago

precyon commented 4 months ago

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{^IG{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{^IG{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.