Add SE_2(3) extended pose Lie group manifold to manif

[prashanthr05]

First of all, thank you for this great repository!

I would like to make a feature request for adding an additional Lie group $SE_2(3)$ (extended pose or double direct spatial isometry as introduced in The invariant extended Kalman filter as a stable observer, appendix A2 .
This can be useful in some applications using inertial measurement units. Please see, Associating Uncertainty to Extended Poses for on Lie Group IMU Preintegration with Rotating Earth, where the orientation, position and linear velocity (with the frame located in the origin of the body and the orientation same as the inertial frame) can be packed into a single Lie group.

Similarly. a possible extension could be towards a generic $SE_{N+2}(3)$ Lie group which can be useful for SLAM based applications. Please see Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation.

@artivis Do you think it would be meaningful to have these Lie groups added to the repository?

I've made an initial implementation for $SE_2(3)$ in my fork with prashanthr05@7169899 and prashanthr05@c96f1f4

However, there is a missing implementation for the action of this Lie group on a vector, act() method. It was not clear to me about how this operation might work.

Let me know what you think about this. If you agree, I can maybe open a PR. Thank you!

[artivis]

Hi @prashanthr05,

It would definitely be interesting to have SE_2(3) in manif! I've had my eyes on it for a time as a matter of fact. But I'm busy with a couple (upcoming) redesigns (starting with #150). However since you already implemented it, feel free to open a PR as it is, I will take care of 'porting' it to the new design once that's ready.

I'm not sure either about the act implementation, maybe @joansola has an idea? In case it does not make sense to implement it, we can put there a failing static_assert or such and handle it in the test suite.

While totally optional, it would be great to see a toy example making use of SE_2(3) accompanying the PR in the examples folder. That helps greatly in terms of documentation. Similarly don't hesitate to reference papers (and equations numbers) in the actual code documentation when possible.

To summarize, please do open a PR 👍.

[prashanthr05]

Great! I shall open a PR soon with the suggestions incorporated.

[prashanthr05]

Actually, I forgot to bring this up earlier. One of the important design choices is the serialization for the vector space in the case of semi-direct products with a rotation matrix, which affects the structure of the Adjoint matrix and the Jacobians.

Right now, I notice for $SE(3)$, we use the serialization as $(v, \omega)$. In $SE_2(3)$ implementation, I have used a similar serialization extended as $(v, \omega, a)$. In this case, the Adjoint matrix does not have a special structure.

But, there are cases in which this serialization changes leading to lower-triangular Adjoint matrices (which is actually the case used in the papers linked above where they use $(\omega, a, v)$ for $SE_2(3)$.)

Have you thought of providing the user with the choice of serialization for the vector space?

[joansola]

There is no mandatory act function in a lie group. It depends on what we act onto. Therefore, we can pick up any transformation we find practical to have. As a minimum default for se23.act, we should fold back to se3.act, which is a plain rigid motion. Maybe we want to act on other objects than 3d points. Which one? Only the users of se23 are in position of telling which is the choice that makes more sense. Once implemented, proper explanations like these must be added to the documentation. I think I would not leave it empty with an assert. But I have not thought about it thoroughly. Joan

…

On Mon, Aug 3, 2020, 15:05 Jeremie Deray ***@***.***> wrote: Hi @prashanthr05 <https://github.com/prashanthr05>, It would definitely be interesting to have SE_2(3) in manif! I've had my eyes on it for a time as a matter of fact. But I'm busy with a couple (upcoming) redesigns (starting with #150 <#150>). However since you already implemented it, feel free to open a PR as it is, I will take care of 'porting' it to the new design once that's ready. I'm not sure either about the act implementation, maybe @joansola <https://github.com/joansola> has an idea? In case it does not make sense to implement it, we can put there a failing static_assert or such and handle it in the test suite. While totally optional, it would be great to see a toy example making use of SE_2(3) accompanying the PR in the examples folder. That helps greatly in terms of documentation. Similarly don't hesitate to reference papers (and equations numbers) in the actual code documentation when possible. To summarize, please do open a PR 👍. — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#152 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AAS2LPP6TSIQQSGBCP7M7WLR62YZRANCNFSM4PTECIGQ> .

[artivis]

We encountered this 'issue' as well in the past. It is not really an issue but rather something to pay attention to.

Have you thought of providing the user with the choice of serialization for the vector space?

I'm not sure how this could be implemented, especially since it changes some block-ordering in e.g. the Jacobians. I'm afraid such feature would only bloat the API and complexify the internal implementation - especially since I'm working on giving the user the choice of right/left Jacobians as proposed here. At this point I doubt it is worth the trouble.

[prashanthr05]

I'm not sure how this could be implemented, especially since it changes some block-ordering in e.g. the Jacobians. I'm afraid such feature would only bloat the API and complexify the internal implementation - especially since I'm working on giving the user the choice of right/left Jacobians as proposed here. At this point I doubt it is worth the trouble.

I understand. It would indeed complexify the implementations.

As a minimum default for se23.act, we should fold back to se3.act, which is
a plain rigid motion. Maybe we want to act on other objects than 3d points.
Which one? Only the users of se23 are in position of telling which is the
choice that makes more sense.

In that case then, if we extend towards composite manifolds (with direct products), would a default definition of an act() method make complete sense? Why I bring this up is the definition of act() in the LieGroupBase class.

[joansola]

In that case then, if we extend towards composite manifolds, would a default definition of an act() method make complete sense? I'm not sure if I understand this right. Composite action I believe deserves the same considerations as above. But it has in general poor geometrical meaning. For example, R3xSO3xR3 would be a composite for SE23. What does the action on 3d points mean? Translation plus rotation plus translation? It makes no sense. On Mon, Aug 3, 2020, 15:52 Joan Sola Ortega <joan.sola.ortega@upc.edu> wrote:

…

There is no mandatory act function in a lie group. It depends on what we act onto. Therefore, we can pick up any transformation we find practical to have. As a minimum default for se23.act, we should fold back to se3.act, which is a plain rigid motion. Maybe we want to act on other objects than 3d points. Which one? Only the users of se23 are in position of telling which is the choice that makes more sense. Once implemented, proper explanations like these must be added to the documentation. I think I would not leave it empty with an assert. But I have not thought about it thoroughly. Joan On Mon, Aug 3, 2020, 15:05 Jeremie Deray ***@***.***> wrote: > Hi @prashanthr05 <https://github.com/prashanthr05>, > > It would definitely be interesting to have SE_2(3) in manif! I've had my > eyes on it for a time as a matter of fact. But I'm busy with a couple > (upcoming) redesigns (starting with #150 > <#150>). However since you already > implemented it, feel free to open a PR as it is, I will take care of > 'porting' it to the new design once that's ready. > > I'm not sure either about the act implementation, maybe @joansola > <https://github.com/joansola> has an idea? In case it does not make > sense to implement it, we can put there a failing static_assert or such > and handle it in the test suite. > > While totally optional, it would be great to see a toy example making use > of SE_2(3) accompanying the PR in the examples folder. That helps > greatly in terms of documentation. Similarly don't hesitate to reference > papers (and equations numbers) in the actual code documentation when > possible. > > To summarize, please do open a PR 👍. > > — > You are receiving this because you were mentioned. > Reply to this email directly, view it on GitHub > <#152 (comment)>, or > unsubscribe > <https://github.com/notifications/unsubscribe-auth/AAS2LPP6TSIQQSGBCP7M7WLR62YZRANCNFSM4PTECIGQ> > . >

[joansola]

Can this be closed following #154 merge?

[prashanthr05]

Can this be closed following #154 merge?

Yes, I think we can close this issue for now.

Maybe if we are interested in the SLAM manifold (SE_{N+2}(3)) later, I can work on opening a PR after @artivis has finished the merge for redesign of the library.
However, does having that Lie group add value and how much value it adds is something to debate on.

What do you think @artivis @joansola?

[joansola]

Yes, open a new PR. The SE_{N+2}(3) manifold is in any case a new thing, therefore a new PR.

[artivis]

I'm not convinced by the relevance of SE_{N+2}(3). N (contact points) sounds robot-specific to me and it will be a 'little' more complicated to implement than SE_2(3), e.g. N would have to be a template parameter with everything that implies.

[prashanthr05]

I'm not convinced by the relevance of SE_{N+2}(3). N (contact points) sounds robot-specific to me and it will be a 'little' more complicated to implement than SE_2(3), e.g. N would have to be a template parameter with everything that implies.

It was introduced for EKF-SLAM (1, 2) for embedding N landmark positions within the state along with the robot pose.
There are also other works in the literature that develop non-linear observers using this manifold for SLAM and VSLAM by exploiting some symmetries of that manifold. Eventually, it was extended for the robot contacts in legged robot state estimation.

However, I agree that implementing it would be more complicated.

In any case, we can rename this issue and close it.

[artivis]

Considering landmarks (thus N dynamic) is even more tricky I think. The library wasn't designed with dynamic states in mind. I'm not saying it isn't doable, but simply that I cannot really foresee the difficulties. If you think that it is a great addition that can only be tackled by adding a new group, feel free to give it a go 👍 .

In the case of N being contact points, is N dynamic too? (Number of points actually being in contact at a given time?) I overlooked the paper and assumed it is fixed in my previous comment :D

[prashanthr05]

Considering landmarks (thus N dynamic) is even more tricky I think. The library wasn't designed with dynamic states in mind. I'm not saying it isn't doable, but simply that I cannot really foresee the difficulties. If you think that it is a great addition that can only be tackled by adding a new group, feel free to give it a go .

In the case of N being contact points, is N dynamic too? (Number of points actually being in contact at a given time?) I overlooked the paper and assumed it is fixed in my previous comment :D

Yes, it's dynamic. The author of the paper (one with contacts) has a minimal implementation of the Lie group here.

I am not certain at the moment if it would be a good addition to the library or worth the effort/time. However, manif's Lie group base generic operators are always a plus.

But if I ever end up giving it a try, I will definitely open a PR.

[prashanthr05]

Having said that we can close this issue.