Add mh_amcl demo

[JesusSilvaUtrera]

Demo using the multi-hypotheses AMCL with Beluga, based on the paper, an example implementation

[JesusSilvaUtrera]

State update now: multi-hypotheses working, tried different configurations of parameters and the localization looks less flaky (still some unwanted jumps, but end up recovering):

testing_mh_amcl.mp4

[hidmic]

@JesusSilvaUtrera hmm, video looks great 💪 (though that's a scary jump) but I must say that I fail to see where we use Beluga APIs in this patch.

[hidmic]

@JesusSilvaUtrera is it? beluga/primitives.hpp are sophisticated enough to pick up well-known members in custom particle types.

[JesusSilvaUtrera]

Mmm, okay I will try again, but I remember having problems compiling because the traits couldn't be found, maybe I was just doing it wrong somewhere else.

[hidmic]

@JesusSilvaUtrera meta: using lifecycle nodes for a demo seems a bit overboard. It makes for a more complex codebase.

[JesusSilvaUtrera]

So you suggest to go back to a simple node? I thought a lifecycle node was appropiate since I am using a bond and it helps setting up the timers and rest of necessary elements.

[hidmic]

@JesusSilvaUtrera meta: a lot of these things beluga already has. See beluga/algorithm/estimation.hpp and beluga_ros/occupancy_grid.hpp for example.

[JesusSilvaUtrera]

The Occupancy grid I am already using it in the code, but I was having problems with some of these utilities, so I decided to make my own implementation. Maybe I was messing up some of its methods or the type of the arguments weren't adequate, I will check

[JesusSilvaUtrera]

@JesusSilvaUtrera hmm, video looks great 💪 (though that's a scary jump) but I must say that I fail to see where we use Beluga APIs in this patch.

@hidmic That is indeed a scary jump jajaja. About using Beluga API, what else did you have in mind, apart from the comments you left? I think I am missing some context about Beluga's API, so I am not sure what else to add (I already added the occupancy grid, and the particle cloud, apart from using the particle traits (which I will review and remove if they are actually not necessary)).

[hidmic]

@JesusSilvaUtrera give https://github.com/Ekumen-OS/beluga-demos/blob/main/localization/beluga_demo_fiducial_localization/src/beluga_lmcl_demo.cpp a look. Mainly the general structure of the filter class. You can't implement MH-AMCL with just that for sure, but I would expect it to build on top of core APIs at least.

[JesusSilvaUtrera]

After the content seen on Beluga's technical sessions, I have included Beluga's API into the "init", "predict" and "correct" steps, and also for the estimations, deprecating some utilities. For the "reseed" step, I would have liked to use Beluga's API more, but I couldn't find a better way to rewrite the custom logic with it.

Some other changes regarding formatting, typos and improving the hypotheses management were added, and now the code seems more robust, but it still gets stuck sometimes (the complexity here is that it doesn't happen all times, it happens at punctual scenarios, so I am not sure yet what is causing this).

There are still more changes that can possibly be done to use more of Beluga's API.

[JesusSilvaUtrera]

I have reviewed deeply the current implementation and all the steps, trying to make the code more performant (it is now) and also spotting some places where Beluga's API or way of work might come handy:

• For the "predict" step, I am using the "propagate" action with my custom motion model (it is quite simple, so I didn't have the necessity to use the "DifferentialDriveModel" for example, as it will result in more code in my opinion, and the way to add noise is not the default one exactly).
• For the "correct" step, after analyzing it I saw that I needed a custom action to update both weights and hits for each particle, so I took a look to Beluga's actions and created my own one, making this step much clearer. I tried to see if I could use Beluga's LikelihoodFieldSensor model to replace some functionalities, but I didn't succeed in that.
• For the "reseed" step, the procedure differs a lot from the usual approach in a particle filter, having the concept of quality and also introducing parameters as the "reseed_percentage_winners", so I kept the original idea but I introduced some of Beluga's code into it.
• For the utilities, I could deprecate some of them, but for example for changing from map to world coordinates (or viceversa) I couldn't find anything in Beluga's OccupancyGrid code, so I had to keep those utilities.
• I also improved a bit how hypotheses are managed, and also now when a new initial pose is given, what it does is to introduce a new hypothesis (if this new initial pose results to be true, this new hypothesis will end up being chosen, if it's false information, it will fall back to the previous hypothesis).
• In execution, the solution is much better now that the code is more performant, and I have tweaked a bit the parameters to improve it even more.

This is all I could get right now without modifying the behavior or worsening the performance, @hidmic if you can give it a look and provide some feedback about what else would you include or any suggestion you might have. I want to give this a push and, if possible, finally close the Frodo before ending this month (or maybe October, depending on what's left).

We can have a short meeting next week as well for discussing the scope, because I fell like there is room for many experiments and tests here, but I need to scope it because I don't want to keep this for long still (but I also want it to provide some value, or at least serve as first implementation for someone to continue).

[JesusSilvaUtrera]

@hidmic I addressed some of the things we discussed yesterday and moved the demo to a new 'experimental' directory. I left a couple of minor TODOs for the future (not addressing them now given the structure will change a lot in the future, so they are just informative).

It should be ready now for a final review and, if everything is okay, approval for merging.

Thank you in advance!

EDIT: I will add a README with the paper and some information now, JIC.

[hidmic]

As discussed f2f, this is a very good starting point to think about proper integration with Beluga. I'll draft some tickets on the core so we can get to work on this.

For the time being, and for this PoC, I'm satisfied.