Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Handle measurement models which return group, but not the Eigen vector. #10

Open
slovak194 opened this issue Dec 10, 2022 · 7 comments
Open

Comments

@slovak194
Copy link

Dear @artivis ,

I follow your code here:

yj.col(i) = h( MapTangent(xis.col(i).data()) + x );

I see so far that the result from the measurement model is expected to be a vector. Are there any thoughts regarding the support of measurement models which return group?
Particularly, when UEKF is used to fuse several high level sources of orientation expressed as SO3 members.

Here are my thoughts so far:

  1. Use a container to store sigma points instead of columns in the matrix (std vector should be fine).
  2. Use proper averaging and weighting methods to compute mean and covariance on manifold (average_biinvariant?).

I think I'll be able to contribute with a little help from your side.

Best regards,
Alex

@artivis
Copy link
Owner

artivis commented Dec 11, 2022

Hi @slovak194,

Indeed kalmanif doesn't support measurements which are in the group. That's not only true for UEKF but also for the other filters as they expect the update to live in the tangent space.

I have difficulties picturing what you are trying to do and what such measurement model would look like. Do you mind developping a little bit?

This being said, I wouldn't use an std vector for the job. At the moment, the filters do not use any dynamic memory allocation - being all template and such (unlike the smoothers). This is a feature I would like to maintain.

@slovak194
Copy link
Author

Hi, @artivis,

I wouldn't use an std vector for the job
Sure, this is just to draw a difference from storing the sigmas as columns.

Do you mind developping a little bit?

I come to the idea from the real use case trying to use manif/kalmanif in my practice but I also found confirmation and a good explanation of the concept in this paper: Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds page #12.

image
image

But, if I correctly understand, the inconsistency between the mean and covariance mentioned above happens only if the covariance is expressed in local tangent space (right invariant) but I might be wrong.

Regarding the real use cases, I do have several now that I have to implement soon.

  1. UEKF is used to fuse several high-level sources with the data already expressed as a group member or manifold member. For example, the positioning system sends SE3 which has to be fused with visual odometry (SE3) and with 2 IMUs (SO3).
  2. Measurements come in form of unit directions which live on S2 (sphere) manifold in 3 space. This is not a group, but a manifold and has to be handled appropriately with box+/box- operators while propagating sigmas and calculating mean/covariance.

Best regards,
Alex

@joansola
Copy link
Collaborator

joansola commented Dec 12, 2022 via email

@slovak194
Copy link
Author

Dear @joansola ,

Thank you very much for such a detailed answer. I actually looked into that topic earlier, end even created the issue here Question: manif Bundle<> or other ways to support composite/compound state. #9

But after playing with Bundle and reading the Micro Lie Theory I came to the conclusion that Bundle is perfectly suitable for my needs. But now I'm not sure.

  1. For some bundles, e.g. SO3xR3, we want the composition to work as in SE3

I find this very confusing, as it breaks the statement written here:
image

I'd expect (as maybe others do) that composition will be performed on per element basis by default as it is written in the paper. And if someone needs a custom composition (like for the IMU above) - then he may create an overload (or follow the steps you have proposed above to create a custom group), but not vice versa.

I may misunderstand the real purpose of the Bundle then. Will it be used for groups only? Some states may live on a manifold (like unit vector S2) but do not form a group. Is it planned to have support for such objects in a Bundle (composite manifold)?

Thank you for your answer! Shall we move here?

Best regards,
Alex

@artivis
Copy link
Owner

artivis commented Dec 12, 2022

manif implements the Bundle as "the concatenation of M non-interacting manifolds". If I refer to example 7 in the paper (SE(n) vs T(n)xSO(n) vs <Rn, SO(n)>) that would be <Rn, SO(n)> (mind the notation <X,Y ...>).

Correct me if I'm wrong but what @joansola is pointing out is that such Bundle may be fine to represent some states (a composite of non-interacting sub-states) but doesn't make sens for others - e.g. representing a robot pose as <R3, SO(3)> or an IMU as <SO3, R3, R3>. That really depends on what you are trying to model.

In case you missed it, kalmanif implements the UKF-M described in Unscented Kalman Filter on Manifolds (UKF-M).

@joansola
Copy link
Collaborator

@artivis is right. Bundle is defined exactly as the Composites in the paper. And what I say is that the SO3xR3 bundle is not equivalent to SE3 because of the difference in the composition operator. And that is you want to use SO3xR3 to represent 3D pose, then "compose" is not going to do what you expect.

Why would someone use SO3xR3 instead of SE3 to represent a 3D pose? Well, because some systems are better off split up into small components. For example, imagine you have a SO3xR3xR3 system representing IMU pose and velocity. And you also have a camera, and that the pose of the camera wrt the IMU is a SO3xR3 object. Now: how do you compute the pose of the camera wrt the inertial frame? You have to "compose" the pose part of the former, with the latter, and this composition needs to be done as in the SE3 case.

Then, why not using SE_2(3) and SE(3) instead? Because maybe your solver somewhere prefers to deal with smaller objects, for which Jacobians and Covariances are easier to compute.

So if you want to play around with these mixed ways of representing the states, then Bundle will not provide you with the appropriate geometric operations, that is, composition.

If you @slovak194 already understand this and build a specific "compose" function for your bundles, then you are already in the right track. What I say is that manif is now not doing that for you, and in some cases one could consider it interesting that manif did so. But it does not. Manif is treating the bundle exactly as "non interacting" blocks, as described in the paper.

@slovak194
Copy link
Author

Dear @joansola, thank you for the clarification, I misunderstood your previous message. This is exactly what I expect and get from the bundle.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants