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

[slovak194]

Dear @artivis ,

I follow your code here:

kalmanif/include/kalmanif/impl/unscented_kalman_filter_manifolds.h

Line 287 in 5e0f1f2

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]

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]

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.

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]

Wow, I was unaware of this paper by Hertzbeg, with the good Udo Frese in it. It actually proposes THE SAME ideas as in our micro-Lie paper regarding plus and minus. I wish I had come across this work earlier, it would have saved me a lot of trouble! Our implementation of Bundle in manif is quite straightforward, as explained in the “composites” section of our paper. However, the practical application of bundles has some tricky corners that are not handled by manif. Let me exxplain the main one. 1. For some bundles, e.g. SO3xR3, we want composition to work as in SE3, but want the plus and minus operators to work on the bundle elements. That is, if T1, T2 belong to SO3xR3, with elements T = [R,p], then we want to compose according to SE3 formula: T1 * T2 = [ R1 * R2 ; p1 + R1 * p2 ] (1) but would like to express the tangent differences block-wise, as in T1 (-) T2 = [ Log(R2.tr * R1) ; p1 - p2 ] (2) This makes the definition of plus and minus inconsistent with the theory of the paper, since now (2) can not be reduced to: T1 (-) T2 = Log ( T2.inv * T1 ) with Log, inv and ‘*' acting on SE3, which is the formula for SE3 given in the paper. If we use the definitions for the bundle, then the composition '*' must be that of the bundle T1 * T2 = [R1*R2, p1+p2] which is then incompatible with (1). I hope you see where the conflict resides. I see three solutions for this. 1.a. Define a function `Bundle<SO3,R3> compose ( const Bundle<SO3,R3>&, const Bundle<SO3,R3>&)` implementing (1). Manif does not need to be altered, as this function lives out of the library. 1.b Overload the appropriate manif functions for each bundle so that the `compose` operator in `manif` performs like the above. 1.c. Define a new group to manif e.g. `Pose3` which acts as SE3 for what relates to geometry (i.e. composition) but as a bundle with regards to plus and minus. I want to recall here that one of the key advantages of bundles, apart from encapsulating heterogeneous states, is to produce errors and derivatives in the tangent spaces of EACH of the components of the bundle, so that we can re-use all the formulas of the bundle constituents. In particular, closed forms for exp(), Jr() and Adjoint() are never trivial and cannot be realistically computed for “any” bundle. That is why, when computing the derivative using formula 41a in our paper, we need a `minus` operator that works on bundle components separately, otherwise we’d need a definition of `Log` in closed form for the bundle. Things get a little mor complicated in some cases. For example, the IMU group can be put as SO3xR3xR3, with elements T = [R, p, v]. Now, composition acts also in a particular way: T1 * T2 = [ R1 * R2, p1 + R1*p2, R1*v2 ] which, again, requires a specific implementation of `compose` if treated as a bundle, or a new definition of plus and minus when treated as a monolythic group (because, otherwise, we need to know closed forms for exp, log, Jr, Jl and Adj) It is true that closed forms exist for the monolythic IMU group called SE_2(3) — see Barrau and Bonnabel. But other implementations of the IMU group, such as ours (see Atchuthan) do not have a closed form. It is clear that if now you go to a complete random bundle, e.g. SO3 x SO2 x R3 x R x SE3, it becomes absolutely impossible to have at the same time: - A definition of compose that satisfies the geometrical properties of our system - Definitions of pllus and minus that repsect the independence of the bundle components, so that we avoid having to know closed forms for exp, log, Jr, Jl, Adj I believe these topics need the proper attention. Joan

…

On 12 Dec 2022, at 09:44, Oleksandr ***@***.***> wrote: Hi, @artivis <https://github.com/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 <https://arxiv.org/pdf/1107.1119.pdf> page #12. <https://user-images.githubusercontent.com/7280672/206990736-7502dc58-5b58-4913-bc8f-b7d81ea892d1.png> <https://user-images.githubusercontent.com/7280672/206998946-0f707613-d3f5-406d-b028-e0a9880235be.png> 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. 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). 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 — Reply to this email directly, view it on GitHub <#10 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AAS2LPP5JD3BCG6M7V3WFPTWM3QVLANCNFSM6AAAAAAS2QN3DM>. You are receiving this because you are subscribed to this thread.

[slovak194]

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:

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]

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]

@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]

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