Add mh_amcl demo

docker/files/DOTaliases

@@ -33,3 +33,7 @@ alias nav2_integration_demo='cd ~/ws \
 alias amcl3_localization_demo='cd ~/ws \
   && source install/setup.bash \
   && ros2 launch beluga_demo_amcl3_localization demo_botanic_garden_amcl3_localization.launch.py'
+
+alias mh_amcl_demo='cd ~/ws \
+  && source install/setup.bash \
+  && ros2 launch beluga_demo_mh_amcl bringup.launch.py'

experimental/beluga_demo_mh_amcl/CMakeLists.txt

@@ -0,0 +1,74 @@
+# Copyright 2025 Ekumen, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 3.16)
+project(beluga_demo_mh_amcl)
+
+# Dependencies
+find_package(ament_cmake REQUIRED)
+find_package(beluga REQUIRED)
+find_package(beluga_ros REQUIRED)
+find_package(bondcpp REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+# Add the include directory
+include_directories(include)
+
+# Executable
+add_executable(beluga_mh_amcl_node
+  src/beluga_mh_amcl_node.cpp
+  src/mh_amcl/mh_amcl.cpp
+  src/mh_amcl/map_matcher.cpp
+  src/mh_amcl/particles_distribution.cpp
+  src/mh_amcl/utils.cpp
+)
+ament_target_dependencies(beluga_mh_amcl_node
+  beluga
+  beluga_ros
+  bondcpp
+  Eigen3
+  geometry_msgs
+  lifecycle_msgs
+  message_filters
+  rclcpp
+  rclcpp_components
+  rclcpp_lifecycle
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
+  visualization_msgs
+)
+install(TARGETS beluga_mh_amcl_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install directories
+install(DIRECTORY
+ config
+ launch
+ maps
+ rviz
+ DESTINATION share/${PROJECT_NAME})
+
+ament_package()

experimental/beluga_demo_mh_amcl/config/mh_amcl_params.yaml

@@ -0,0 +1,39 @@
+mh_amcl:
+  ros__parameters:
+    use_sim_time: true
+
+    # Maximum number of hypotheses maintained
+    max_hypotheses: 5
+
+    # Enable or disable multi-hypothesis AMCL
+    multihypothesis: true
+
+    # Minimum weight for a candidate hypothesis to be considered
+    min_candidate_weight: 0.6
+
+    # Minimum distance between candidate hypotheses (in meters)
+    min_candidate_distance: 1.0
+
+    # Minimum angular difference between candidate hypotheses (in radians)
+    min_candidate_angle: 1.047 # Equivalent to 60 degrees
+
+    # Threshold for low-quality hypotheses
+    low_q_hypo_threshold: 0.2
+
+    # Distance threshold for merging similar hypotheses (in meters)
+    hypo_merge_distance: 0.4
+
+    # Angular threshold for merging similar hypotheses (in radians)
+    hypo_merge_angle: 0.523 # Equivalent to 30 degrees
+
+    # Threshold to consider a hypothesis as "good"
+    good_hypo_threshold: 0.5
+
+    # Minimum difference in quality between the best hypothesis and others
+    min_hypo_diff_winner: 0.25
+
+    # Bond timeout
+    bond_timeout: 8.0
+
+    # Standard deviation for the LiDAR (shouldn't be below 0.2 for the correct computation of the qualities)
+    distance_perception_error: 0.5

experimental/beluga_demo_mh_amcl/doc/README.md

@@ -0,0 +1,35 @@
+# Beluga Multi-Hypotheses AMCL demo
+
+**NOTE**: This demo is experimental and a work in progress. It is **not** a complete proof-of-concept or a production-ready integration. Expect bugs, breaking changes, and incomplete features.
+
+## How the algorithm works
+
+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 initial position, if known.
+2. Creation: Periodically, a cascade map matching algorithm is used to determine in which map positions the latest sensor readings could have been obtained. If a position with a high match is found (high hit field), a new P(k,t) is initialized 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/include/beluga_demo_mh_amcl/map_matcher.hpp

@@ -0,0 +1,97 @@
+// Copyright 2025 Ekumen, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MH_AMCL__MAPMATCHER_HPP_
+#define MH_AMCL__MAPMATCHER_HPP_
+
+#include <list>
+#include <memory>
+#include <vector>
+
+#include <beluga_ros/occupancy_grid.hpp>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/transform_datatypes.h>
+
+namespace mh_amcl
+{
+
+  /**
+   * @brief Struct containing a transform and its associated weight
+   */
+  struct TransformWeighted
+  {
+    float weight;
+    tf2::Transform transform;
+  };
+
+  /**
+   * @brief Overload of the operator '<' to be able to compare TransformWeighted
+   * structs
+   */
+  bool operator<(const TransformWeighted &tw1, const TransformWeighted &tw2);
+
+  /**
+   * MapMatcher class
+   *
+   * Performs the multi-resolution map matching process. It receives an occupancy
+   * grid map and laser scan data to find transformations (poses) that align the
+   * scan data with the map. It weights all potential transformations to select
+   * the higher quality candidates.
+   */
+  class MapMatcher
+  {
+  public:
+    explicit MapMatcher(std::shared_ptr<const nav_msgs::msg::OccupancyGrid> map);
+
+    /**
+     * @brief Get the candidate transformations from the laser information and the
+     * map
+     */
+    std::vector<TransformWeighted>
+    get_matches(const sensor_msgs::msg::LaserScan &scan);
+
+  protected:
+    static const int NUM_LEVEL_SCALE_COSTMAP = 4;
+
+    /**
+     * @brief Reduce the resolution of the map to half to perform the
+     * multi-resolution map-matching
+     */
+    std::shared_ptr<beluga_ros::OccupancyGrid>
+    half_scale(std::shared_ptr<beluga_ros::OccupancyGrid> costmap_in);
+
+    /**
+     * @brief Transform the LaserScan message in a vector of 3D points
+     */
+    std::vector<tf2::Vector3>
+    laser2points(const sensor_msgs::msg::LaserScan &scan);
+
+    // Auxiliary methods to get the matches between the laser data and the map
+    std::vector<TransformWeighted>
+    get_matches(int scale, const std::vector<tf2::Vector3> &scan, float min_x,
+                float min_y, float max_y, float max_x);
+    float match(int scale, std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
+                const std::vector<tf2::Vector3> &scan, const tf2::Transform &transform);
+
+    // Current costmaps
+    std::vector<std::shared_ptr<beluga_ros::OccupancyGrid>> costmaps_;
+  };
+
+} // namespace mh_amcl
+
+#endif // MH_AMCL__MAPMATCHER_HPP_