Final minor improvements, moved the demo to 'experimental' directory

Signed-off-by: JesusSilvaUtrera <jesus.silva@ekumenlabs.com>

Add README for future information

Signed-off-by: JesusSilvaUtrera <jesus.silva@ekumenlabs.com>

Author: JesusSilvaUtrera

…ation/beluga_demo_mh_amcl/CMakeLists.txt …ental/beluga_demo_mh_amcl/CMakeLists.txtlocalization/beluga_demo_mh_amcl/CMakeLists.txt renamed to experimental/beluga_demo_mh_amcl/CMakeLists.txt localization/beluga_demo_mh_amcl/CMakeLists.txt renamed to experimental/beluga_demo_mh_amcl/CMakeLists.txt

@@ -0,0 +1,33 @@
+# Beluga Multi-Hypotheses AMCL demo
+
+## How the algorithm work
+
+Traditional AMCL is extended by managing multiple populations of particles (each population is a hypothesis). Particles are created, destroyed, or merged based on their consistency with sensory data, choosing the best fitting hypothesis in each iteration.
+
+The observation model is based on geometric transformations and projections (using the tf2 library to compute the map → odom → base_link → base_scan transformations), supporting both 2D (cells in a costmap) and 3D environments (voxels from an octree for example, or elevation costmaps, no evidence about this functionality).
+
+Instead of relying on traditional uncertainty measures from the covariance matrix, the authors propose a new metric (Novel metric) that compares sensory measurements with expected values to assess consistency.
+
+The particle update process is divided into prediction, correction, and reseeding, optimizing computational cost. The prediction phase happens frequently (100Hz), correction less so (10Hz), and reseeding rarely (0.3Hz):
+- The prediction phase remains the same as the typical particle filter.
+- The correction phase applies the new observation model to update the weights of the particles, using the ‘hits field’ (h) instead of the covariance matrix to compute the quality of the hypothesis.
+- In the reseed phase particles with less weight (losers) are replaced by particles close to those with more weight (winners). If the distribution’s covariance is high, the number of particles is increased. If it’s low, it’s decreased to save computational cost.
+
+A quality value is computed (value between 0 and 1) for every set of particles (every hypothesis) and a weight is computed for every particle (this remains the same). Some hypotheses can be merged if they converge to the same pose.
+
+The output of the localization system is still a mean value and a covariance calculated from the highest quality particle set.
+
+The number of hypotheses is parametrized, along with other new concepts like the quality thresholds or the reseeding percentage for winners.
+
+The map-matching is done with 4 levels of resolution to improve computational cost, because the next level is only computed on those cells that are candidates from the previous level. The result is a list with the candidates sorted by quality.
+
+### Steps for managing the hypotheses
+
+1. Start: At the start of the robot operation, P(0,t0) (first set of particles, hypothesis) is started at the robot’s initial position, if known.
+2. Creation: Periodically, a cascade map matching algorithm is used to determine which map positions the latest sensory readings could be obtained. If a position with a high match is found (high hit field), a new P(k,t) is started at this position.
+3. Destruction: If Quality(P(k,t)) is less than some threshold and |Pt| > 0, then P(k,t) is considered to be wrong, and it is removed.
+4. Merge: Even if two P(i,t) and P(j,t), start at different positions, they could end up converging to the same position. In this case, they are mixed in a P(k,t) containing the particles with more weight.
+
+## Useful links
+
+[Original Paper](https://arxiv.org/pdf/2209.07586)

experimental/beluga_demo_mh_amcl/README.md

@@ -92,12 +92,6 @@ namespace mh_amcl
     std::vector<std::shared_ptr<beluga_ros::OccupancyGrid>> costmaps_;
   };
 
-  /**
-   * @brief wrapper to transform the costmap to ROS Occupancy grid message
-   */
-  nav_msgs::msg::OccupancyGrid
-  toMsg(std::shared_ptr<beluga_ros::OccupancyGrid> costmap);
-
 } // namespace mh_amcl
 
 #endif // MH_AMCL__MAPMATCHER_HPP_

…_demo_mh_amcl/config/mh_amcl_params.yaml …_demo_mh_amcl/config/mh_amcl_params.yamllocalization/beluga_demo_mh_amcl/config/mh_amcl_params.yaml renamed to experimental/beluga_demo_mh_amcl/config/mh_amcl_params.yaml localization/beluga_demo_mh_amcl/config/mh_amcl_params.yaml renamed to experimental/beluga_demo_mh_amcl/config/mh_amcl_params.yaml

@@ -76,7 +76,7 @@ namespace mh_amcl
      * the particles. It's only done if laser data and costmap information are
      * present.
      */
-    void correct_once(const sensor_msgs::msg::LaserScan &scan);
+    void correct(const sensor_msgs::msg::LaserScan &scan);
 
     /**
      * @brief Particles are ordered by probability, winners and losers are

…lude/beluga_demo_mh_amcl/map_matcher.hpp …lude/beluga_demo_mh_amcl/map_matcher.hpplocalization/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/map_matcher.hpp renamed to experimental/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/map_matcher.hpp localization/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/map_matcher.hpp renamed to experimental/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/map_matcher.hpp

@@ -73,6 +73,7 @@ namespace mh_amcl::actions
                 if (valid_beams > 0)
                 {
                     particle.weight += total_prob;
+                    // TODO: Review if hits should be computed like this
                     particle.hits = total_prob / static_cast<float>(scan.ranges.size());
                 }
                 else