On implementing a general MCL solution for ROS2

[hidmic]

Monte Carlo Localization (MCL) is a family of non-parametric Bayesian state estimation algorithms, fairly popular in robotics. Over the years, quite a few variations of this idea have been developed: NDT MCL, Dual Timescale NDT MCL, QMCL, and yet only AMCL has seen widespread use and for a very narrow class of measurement and motion models (e.g. the typical diff drive robot with a 2D lidar). Moreover, the natural ease for parallelization that these algorithms afford is rarely leveraged.

Some ROS1 packages have come out from academia (e.g. quickmcl, ndt_localization) and there have been attempts to substitute AMCL in Navigation2 (see ros-navigation/navigation2#1391 and ros-navigation/navigation2#1930), but a production-ready, general MCL package for ROS 2 remains a pipe dream.

There is a room for an implementation to emerge.

[hidmic]

As @nahueespinosa ventures into C++ L4 territory focusing on concurrency and high performance code, and @camistolo signs up for a C++ L3 with an emphasis in robotics (and lots and lots of templates 😃), I (that is we) think we can pursue something like this.

Once we've sorted out the operational details, I'll hoist this discussion in a ticket.

[glpuga]

Leaving this here for inspiration: https://www.youtube.com/watch?v=RmY82ApjCMQ

[nahueespinosa]

Thanks Gera!

[glpuga]

Now that we were discussing potential goals for beluga, MH-AMCL is maybe worth looking at, since it's probably going to be merged into NAV2 AMCL .

The paper appears to be heading towards ICRA2023: https://www.youtube.com/watch?v=LnmQ11Ew01g .

The preprint can already be found at https://arxiv.org/abs/2209.07586

[glpuga]

On the same topic, notice:

• Potential AMCL Improvements (NDT, QMCL, etc) ros-navigation/navigation2#1391
• Localization Framework Initial PR ros-navigation/navigation2#1930, whose status seems to be summarized in this comment: Investigate GPU optimizations ros-navigation/navigation2#1781 (comment)

[nahueespinosa]

Created #52, I think it's a good candidate for an L3 padawan project.

[nahueespinosa]

In general, an MCL algorithm consists of three fundamental components: a particle filter, a motion model and an observation model. Furthermore, for a given particle filter, different sampling variants can be identified. This is an attempt to classify articles to make sense of all this.

• Particle filter variants:
  • MCL: https://www.ri.cmu.edu/pub_files/pub1/dellaert_frank_1999_2/dellaert_frank_1999_2.pdf
  • Mixture-MCL: http://robots.stanford.edu/papers/thrun.robust-mcl.pdf
  • Real time particle filter: https://www.researchgate.net/publication/2986264_Real-Time_Particle_Filters
    • Divide sample sets between all available observations and then represent the state as a mixture of sample sets. Optimize the mixing weights in order to be as close to the true posterior distribution as possible.
  • Delayed state particle filter: https://www.researchgate.net/publication/281199975_Particle_filter_with_one-step_randomly_delayed_measurements_and_unknown_latency_probability
• (Re)Sampling variants
  • Fixed
  • Low variance sampler - Stochastic universal sampling
  • AMCL: https://www.robots.ox.ac.uk/~cvrg/hilary2005/adaptive.pdf
    • Adaptive KLD sampler: https://proceedings.neurips.cc/paper/2001/file/c5b2cebf15b205503560c4e8e6d1ea78-Paper.pdf
• Observation model variants
  • Beam model
  • Likelihood field models (LFM)
  • Conditional observation model (COM): https://www.researchgate.net/publication/326586084_Mobile_Robot_Localization_Considering_Class_of_Sensor_Observations
  • Penalized mixture of exponential power (PMoEP): https://arxiv.org/pdf/1601.01060.pdf
  • NDT-MCL: https://www.researchgate.net/publication/261352911_Normal_distributions_transform_Monte-Carlo_localization_NDT-MCL
    • Uses NDT to represent the map and sensor data.
  • DT-NDT-MCL: https://www.researchgate.net/publication/282017851_Localization_in_highly_dynamic_environments_using_dual-timescale_NDT-MCL
    • Keeps track of a static map and a short term map.
  • Range Image-based LIDAR localization: https://arxiv.org/pdf/2105.12121.pdf
    • Uses range LIDAR images and a mesh-based map representation.
  • Loc-NeRF: https://arxiv.org/pdf/2209.09050.pdf
    • NeRF model is the map of the environment.
• Motion model variants
  • TBD
• Failure detection
  • Reliable MCL: https://arxiv.org/pdf/2205.04769.pdf
• Combinations with other algorithms
  • Moving grid-MCL: https://ieeexplore.ieee.org/document/5420461
  • Rao-Blackwellized particle filter: https://arxiv.org/pdf/1301.3853.pdf
  • MH-MCL: https://arxiv.org/pdf/2209.07586.pdf
    • Independent particle filters to track different hypotheses competing to be selected as the system output.
• Parallelization
  • https://link.springer.com/chapter/10.1007/978-3-030-01535-0_23
  • http://access.ee.ntu.edu.tw/Publications/Conference/CA98_2010.pdf
  • https://ieeexplore.ieee.org/abstract/document/7130310
  • https://ieeexplore.ieee.org/document/7035029
  • https://ieeexplore.ieee.org/document/9158397

FYI @camistolo, feel free to contribute to this list.

[hidmic]

Not strictly related, but there's some conceptual overlap with https://github.com/locusrobotics/fuse.

[nahueespinosa]

Bibliography and resources:

• Particle Filter and Monte Carlo Localization (Cyrill Stachniss): https://www.youtube.com/watch?v=MsYlueVDLI0&t=2759s

Existing implementations:

• https://github.com/ros-planning/navigation2/tree/main/nav2_amcl
• https://github.com/VorpalBlade/quickmcl
• https://github.com/adler-1994/gmcl
• https://gitsvn-nt.oru.se/software/ndt_tools_public/-/tree/master/ndt_localization

[nahueespinosa]

I've been reading several articles and looking at the nav2_amcl implementation of particle filters for localization (and QuickMCL which is based on the former) and the main takeaway is that AMCL is a combination of two techniques to improve the base MCL algorithm:

1. The first one is described in https://www.robots.ox.ac.uk/~cvrg/hilary2005/adaptive.pdf and is meant to improve the computational performance of the filter by adapting the number of particles to the underlying state uncertainty. Once the robot knows where it is, only a small number of samples suffices to accurately track its position.
2. The second one is described in Chapter 8.3.3 of Probabilistic Robotics and is a recovery technique used to solve the kidnapped robot problem by inserting random particles from the map based on an accuracy estimation of the distribution.

Both solutions have a slightly different algorithm definition for the filter:

Algorithm KLD Sampling:
  do
    Resampling: Draw state from previous belief
    Sampling: Predict next state
    Compute importance weight
    Insert sample into sample set
  while Number of samples is less than KL-bound
  Normalize

Algorithm Augmented MCL:
  foreach Particle
    Sampling: Predict next state
    Compute importance weight
  endfor
  foreach Particle
    with probability max(0.0, 1.0 − accuracy_estimate) do
      Insert random pose into sample set
    else
      Resampling: Draw state from previous belief
      Insert sample into sample set
    endwith
  endfor

IMO the combination of the two for a generic implementation could look like:

Algorithm Adaptive MCL:
  foreach Particle
    Sampling: Predict next state
    Compute importance weight
  endfor
  do
    with probability determined by a recovery policy do
      Insert random pose into sample set
    else
      Resampling: Draw state from previous belief
      Insert sample into sample set
    endwith
  while a sample size policy returns true

Where a clear sample-importance-resampling sequence can be identified.

[nahueespinosa]

Some implementation ideas can be found in https://godbolt.org/z/zqsx319Ga.

[hidmic]

@nahueespinosa won't such combination lead to unnecessary particle updates for the pure KLD sampling case?

[nahueespinosa]

@hidmic Sorry, this is me oversimplifying things. The recovery policy described in Probabilistic Robotics depends on the average weight before normalization, so a complete importance weight update is required. It may be worth considering updating the recovery policy from a previous iteration of the filter, I'll think about it.

[olmerg]

Some years ago when I was learning GPU programming I study some form of parallelization of particle filter using beta c++ Thrust (final course project), I made this article using an ackerman robot. The principal problem is in the resampling where some data need to be shared but some improvements can be found in the bibliography. If you think that can give some insight I can review in details what I do.

Garca2018_Chapter_GPU-ImplementationOfASequentia.pdf

[olmerg]

Reviewing how to parallel resampling I found this articles,

• Performance Analysis of Resampling Algorithms of Parallel/Distributed Particle Filters
• [Parallel resampling for particle filters on FPGAs] (https://ieeexplore.ieee.org/abstract/document/7082775)
• [Parallel Resampling in the Particle Filter] (https://arxiv.org/pdf/1301.4019.pdf)