search

MRPT / mrpt

:zap: The Mobile Robot Programming Toolkit (MRPT)
https://docs.mrpt.org/reference/latest/
BSD 3-Clause "New" or "Revised" License
1.89k stars 627 forks source link

gtsam_wrappers.h: covariance matrix transformation between MRPT and GTSAM #1229

Closed miguelcastillon closed 2 years ago

miguelcastillon commented 2 years ago

Hello,

I'm trying to use MRPT and GTSAM to build a SLAM package. I have recently discovered the existence of gtsam_wrappers.h. However, I have a doubt about the transformation of the covariance matrix. As far as I understand, MRPT uses the yaw-pitch-roll parametrization for 3D rotations. However, GTSAM works in SE(3), with the 3 rotational degrees of freedom being the exponential of the SO(3) rotation matrix (Rodrigues vector).

We have checked GTSAM derivatives of the SE(3) group and they are done with respect to the exponential coordinates, using the same Jacobians as in [1]. We understand that when manipulating them, all the covariance matrices and jacobians should share the same representation: YPR or SO(3) exponential map. In the code of gtsam_wrappers.h, I see that the only operation to transform the covariance matrix from YPR to Rodrigues is the reordering of rows and columns. Shouldn't this transformation also include a multiplication with the jacobian matrix that relates YPR and SO(3) exponential map? Or is there something that we are missing?

Thank you very much! Miguel

[1] Solà, J., Deray, J., & Atchuthan, D. (2018). A micro Lie theory for state estimation in robotics. Retrieved from http://arxiv.org/abs/1812.01537

jlblancoc commented 2 years ago

Hi Miguel: You are exactly right. Those functions in gtsam_wrappers.h were done in a hurry for a context where all covariances were isotropic.

If you are now focused on this topic, please, feel free of proposing a PR improving the current function (perhaps we could keep the current ones, adding to its docs that they are only for isotropic noise models, and create new functions accepting the pose together with the covariance, for example?), including some minimal unit test (I could add it if you provide some example numbers).

I think the Jacobian you need is not already "as is" exposed by any single MRPT function, but we have: 1) https://github.com/MRPT/mrpt/blob/develop/libs/math/include/mrpt/math/CQuaternion.h#L451 2) chapters 2 & 10 of: https://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf 3) The implementation of most formulas in 2) above are in here

By chaining the relevant Jacobians, I think we should be able to go back and forth both conventions... Let me know if there's some missing intermediate step.

In case you are interested in visualization, as you probably know, 3D translation uncertainty can be represented in mrpt::opengl with CEllipsoid3D. About SO(3) uncertainty... there's no obvious way to "draw" it, but we have a generic ellipsoid base class which was used in the past to represent inverse depth distributions and such, so perhaps it could be used for that aim too.

Cheers,

miguelcastillon commented 2 years ago

Thank you José Luis,

OK, I'll try to implement this Jacobian for the project I'm currently working on and when I'm confident it's working decently I'll make a PR to the repository :)

I'm quite new to optimization problems on SE(3) and I'm a bit confused with the notation and equivalences. For my graph SLAM problem, I need to pass GTSAM the covariance matrix of my odometry displacements, which I calculate out of pairs of absolute poses. I was following Solà's paper referenced above, so I was going to use its equations (82, 83) [with their terms defined in (179, 180)] to calculate the Jacobian of the displacement. However, now I see that this jacobian is already defined in MRPT (as explained in section 10.3.10 of the technical report). I wonder now if both jacobians (Solà's and MRPT's) turn out to be the same, because they look wildly different to me. Do you have any insight about their equivalence? I'm just wondering out of curiosity, I don't want to waste your time (I can implement the operations and see if they give the same result), but I thought you may have already encountered a similar situation before.

Thanks again! Miguel

EDIT: I've realized that I was comparing two different things, my bad. As I understand it now, the jacobians in section 10.3.10 of the technical report are to be used by the back-end when minimizing the error of the BetweenFactor.