Trying to understand the kalman update implementation

[patripfr]

Hi, thanks for open sourcing this great work! I'm trying to understand how the code relates to the formulas provided in the paper, but I'm struggling to understand the code in the filterUpdate. Especially what's going on in the following lines is unclear to me: https://github.com/lastflowers/envio/blob/9c20e67761b02907980ff0d33276041ab97aed8d/src/core/sl_iekf.cpp#L349-L357

Additionally, I'm trying to find out how formula 39 from the paper is realized in the code. If you could point me to the respective formulas in the paper or provide some insights on this, that would be highly appreciated!

[lastflowers]

Hi, thanks for your interest in our work :)

The current commit is slightly different from the original implementation in the paper. But still, you can relate formulas in the paper to codes.

For the first question, the covariance update part (line 349 ~ 359) implements (P^+)^{-1} = H^T R^{-1} H + (P^-)^{-1}, where it is done block by block. Note that "Hp" stands for the pose part of the Jacobian, while "h" indicates the depth part of the Jacobian.

For the second one, eq (39) is implemented by this line. https://github.com/lastflowers/envio/blob/9c20e67761b02907980ff0d33276041ab97aed8d/src/core/sl_iekf.cpp#L360 I omitted the 3rd part (H zeta^bar) since it is hopefully not remarkable when compared to the residual part.

I hope this would help you.

[patripfr]

Thanks for pointing this out, it makes sense to me now :)

[patripfr]

Hi, Sorry for bothering you again, but I have another question regarding the update. Could you point me to a derivation how the first component in formula 31 is calculated? I'm trying to calculate it by myself, but I end up with a different result, so I'm curious what's the right way to get there. Thanks, Patrick

[lastflowers]

Hi, Patrick. Starting from the equation (25), and the error definitions in (20), you would get to the equation (31). Take a look at the derivation if you want more information:

[patripfr]

Thanks for showing the derivation. What's still confusing to me is that the result depends on $(p_j^g)^{\wedge}$, which means that it depends on the global position of the feature. This seems confusing to me, as it basically means that a feature in global position (1000, 1000, 0) would have a much higher gradient than a feature at (10,10,0), independent of the camera position. Shouldn't the gradient depend on the relative position between camera and feature, instead of the global position of the feature?

[lastflowers]

Hi, that's a very good point. I agree with you that the Jacobian should depend on the camera-feature relative position. If I define state error as the conventional SO(3), then I reach the Jacobian expressed by the relative position.

But, the point is that the Jacobian with right-invariant error is no longer expressed in the camera-centered frame. This is the beginning of the mismatch between equations and our intuition. For example, you can find a similar result in p.17 of the invariant EKF paper that the Jacobian matrix H_n does depend on the global feature position as our derivation.

[patripfr]

I think I have to familiarize myself better with the concept of the right invariant error. Thanks for your help and the link!