Multi-Hypothesis AMCL

[nahueespinosa]

Description

Following up on this discussion. As we're looking for targets to show the versatility of beluga, MH-AMCL is probably worth considering. In addition to the multiple hypothesis machinery, they also use a different resampling algorithm.

Preprint: https://arxiv.org/abs/2209.07586

Definition of Done

TBD

[nahueespinosa]

Following up on a discussion about the proposed observation model (contribution 2 of the paper):

• It seems to be inspired by the beam model (see #54) described in Probabilistic Robotics, Chapter 6.3.1 with some tweaks:
  • Instead of modeling the measurements by the linear combination of four different distributions, it uses a single Gaussian assuming a correct range value with local noise.
  • The previous assumption allows for an optimization to only lookup cells near the measurement.
• In addition to the state and weight of particles, it stores a "hits" value that "indicates the likelihood of each particle with the last perception". There is no clear description of how that is calculated, but most likely they count the number of beams that are closer than a certain distance from an obstacle (perhaps $3 \sigma$).
• The quality value for a particle set is computed from particle "hits". Equation (8) is kind of cryptic to me, but maybe they normalize the hits per particle and then average and normalize over the particle set.

Other notes:

• The quality value is calculated per set of particles to select what the localization system output will be, and is also used to eliminate wrong hypotheses when it falls below a threshold.
• The weight value is calculated per particle and, as usual, is used in the resampling step of each of the particle sets. The resampling approach removes the particles with lower weights (which they call "losers").
• The output of the localization system is still a mean value and a covariance calculated from the highest quality particle set.

FYI @serraramiro1 @glpuga

[serraramiro1]

Updating after today's sync

Some really basic outlines for this contribution, in the shape of some development steps in suggested order.

1. Create a new particle trait specialization, introducing the concept of hits and providing accessors. (Related to // TODO (Ramiro) : Link to particle traits issue.
2. Implement the new sensor model, that should leverage the hits concepts to match the preprint's propposal.
3. Implement the reseed version of resampling, TLDR, quoting the preprint:

Reseed: Removes hypotheses x with weights w < threshold, creating new ones near to hypotheses with
high weights

4. Come up with a design for a policy based multi particle filters wrapper, so that we can reason about multi hypotheses estimation, while keeping the logic inside beluga core.
5. Implement number 4
6. Wrap 5. around a ROS2 node as an example that can be used by others.

FYI @hidmic @glpuga @nahueespinosa

[olmerg]

mh_amcl

[glpuga]

mh_amcl

Thanks for posting @olmerg .

That's interesting, at first sight it looks very different from NAV2's amcl node. I had understood from some post that they were going to add MH back into NAV2's AMCL, but from this starting point it looks like it would be a very hard thing to do and it looks like it's more likely that they will just open source this code.

[hidmic]

at first sight it looks very different from NAV2's amcl node.

They gave up :smile:

[olmerg]

Yes the approach it is interesting they have the particles in the container and another timer make the hypothesis algorithm.

[hidmic]

This has been in the backlog for a while. I wonder if there is path (and interest) for the techniques in https://github.com/fmrico/mh_amcl to be recreated in Beluga. I also wonder if there have been any further developments.

Perhaps @fmrico can shed some light here.

[fmrico]

Hi all,

Yes, we continued this research to elevation maps for non-planar environments:

https://github.com/navigation-gridmap/mh_amcl https://onlinelibrary.wiley.com/doi/full/10.1002/rob.22353

I hope you find it interesting Francisco