Support state estimation on manifolds (i.e. SO(2) or SO(3))

[mherb]

https://arxiv.org/abs/1107.1119

Consider to use https://github.com/artivis/manif

[artivis]

Dear @mherb,

I couldn't help noticing that you recently stared my manif library. While looking for a way to reach out, I came to see the update on this issue mentioning manif and figured this would be the right place to do so.

For your information, some times ago I have been working on the kalman library with the very purpose of integrating it with manif (or the other way around). I have managed at that time to get the EKF working with manif or rather with any custom user-defined 'State' class, thus not inheriting from Kalman::Vector<T, N>. Using 'type traits' allowed for minimal modifications to kalman source code.
See an example of such custom 'State' class here together with its traits specialization here.
I would have to double check the following but as I remember, the custom 'State' class only has for requirements to be default-constructible and implement the + and += operators.
If you ever came to further browse the changes made on this branch, please mind that overall the modifications are a bit messy and do not reflect what a final solution may look like.
Notice also that while EKF is working fine, UKF and both Root versions are not compiling and likely far from being on tracks to also support custom 'State' classes.

All being said, I only would like to let you know that I could offer some (limited?) support to bringing closer kalman and manif libraries, as I believe this would enable some desirable features to the community.

Best.

[mherb]

Dear @artivis,

thank you so much reaching out. I stumbled upon your manif Lib recently and it looks really good, thank you for your efforts regarding this project. While I just had a brief at it so far, I think it also matches perfectly with the kalman lib and I agree with you that integrating both libs would certainly benefit the community a lot as manifold filtering is a common problem in robotics.

As you might be aware, there was unfortunately few progress on kalman in recent times due to me being busy with other projects. Therefore I currently cannot give an exact timeframe when integration will happen (I have lots of ideas, but need some time to refine into a viable concept).

As you pointed out, just overloading add/subtract operators is not enough for more complex filter algorithms such as UKF, which also needs to perform mean and variance computation in manifold space. I noticed that manif also has (limited?) support for such operations including quaternion averaging, which certainly makes integration much easier. Another issue is that the algorithms currently assume that State objects can be treated as Eigen vectors, which might get a bit complicated if State classes contain complex logic (i.e. composite of SE(3) and further state parameters).

I am currently considering to encapsulate the logic for manifold operations in the state class similar to your implementation, i.e. the user has to provide overloaded +/- operators as well as special functions such as average(it begin, it end) that perform the dedicated operations in the manifold state space chosen by the user to be used by the filter algorithms. If the user does not provide overloads, we assume a standard euclidean space. For the problem of composite state spaces, I am still in early thoughts how to handle this properly in a generic way, but suggestions are always welcome. I have also come to notice that you are well aware of this problem (artivis/manif#84).

In any case, I appreciate your efforts on manif a lot and think that both libs can mutually benefit from one another in the future.

Best,
Markus

[artivis]

Dear @mherb,

Thank you for your prompt reply.
I am aware of the difficulty to allocate time and efforts to further develop projects such as the kalman library. I face it myself more than I wish I would.

The difficulties your are pointing out concerning the UKF are, for a part, those I encountered. For an other part, they are design-related and linked to the topic of composite states to which I will come back. manif does offer two averaging methods at the moment. I just realized that some code under development made it to the main branch of the repo, I shall fix this. Allow me to point out the following publication which I believe is helpful to this matter: Unscented Kalman Filtering on Lie Groups.

Concerning the topic of composite states, this is a matter we plan to address in a near futur in manif as we also need such feature in another project. Note that Section IV of the paper that originated the library is dedicated to this topic.

Thank you again for your time, let us keep this communication channel open.

Best.