Add support for map updates in laser localization filters (#189)

Signed-off-by: Michel Hidalgo <[email protected]>

beluga/include/beluga/localization.hpp

@@ -52,12 +52,16 @@ namespace beluga {
  */
 
 /// Pure abstract class representing an interface for laser-based localization algorithms.
+/**
+ * \tparam Map Environment representation type.
+ */
+template <class Map>
 using LaserLocalizationInterface2d = beluga::mixin::compose_interfaces<
     BaseParticleFilterInterface,
     StorageInterface<Sophus::SE2d, beluga::Weight>,
     EstimationInterface2d,
     OdometryMotionModelInterface2d,
-    LaserSensorModelInterface2d>;
+    LaserSensorModelInterface2d<Map>>;
 
 /// Commonly used resampling policies combined in a single mixin.
 using CombinedResamplingPolicy = ciabatta::
@@ -70,8 +74,9 @@ using CombinedResamplingPolicy = ciabatta::
  *
  * \tparam MotionDescriptor A descriptor of a mixin that implements \ref MotionModelPage.
  * \tparam SensorDescriptor A descriptor of a mixin that implements \ref SensorModelPage.
+ * \tparam Map Environment representation type consistent with the sensor descriptor.
  */
-template <class MotionDescriptor, class SensorDescriptor>
+template <class MotionDescriptor, class SensorDescriptor, class Map>
 using MonteCarloLocalization2d = ciabatta::mixin<
     BootstrapParticleFilter,
     ciabatta::curry<StructureOfArrays, Sophus::SE2d, beluga::Weight>::mixin,
@@ -82,7 +87,7 @@ using MonteCarloLocalization2d = ciabatta::mixin<
     CombinedResamplingPolicy::template mixin,
     MotionDescriptor::template mixin,
     SensorDescriptor::template mixin,
-    ciabatta::provides<LaserLocalizationInterface2d>::mixin>;
+    ciabatta::provides<LaserLocalizationInterface2d<Map>>::template mixin>;
 
 /// An implementation of Adaptive Monte Carlo Localization.
 /**
@@ -91,8 +96,9 @@ using MonteCarloLocalization2d = ciabatta::mixin<
  *
  * \tparam MotionDescriptor A descriptor of a mixin that implements \ref MotionModelPage.
  * \tparam SensorDescriptor A descriptor of a mixin that implements \ref SensorModelPage.
+ * \tparam Map Environment representation type consistent with the sensor descriptor.
  */
-template <class MotionDescriptor, class SensorDescriptor>
+template <class MotionDescriptor, class SensorDescriptor, class Map>
 using AdaptiveMonteCarloLocalization2d = ciabatta::mixin<
     BootstrapParticleFilter,
     ciabatta::curry<StructureOfArrays, Sophus::SE2d, beluga::Weight, beluga::Cluster>::mixin,
@@ -103,7 +109,7 @@ using AdaptiveMonteCarloLocalization2d = ciabatta::mixin<
     CombinedResamplingPolicy::template mixin,
     MotionDescriptor::template mixin,
     SensorDescriptor::template mixin,
-    ciabatta::provides<LaserLocalizationInterface2d>::mixin>;
+    ciabatta::provides<LaserLocalizationInterface2d<Map>>::template mixin>;
 
 }  // namespace beluga
 

beluga/include/beluga/sensor.hpp

@@ -54,6 +54,10 @@
 namespace beluga {
 
 /// Pure abstract class representing the laser sensor model interface.
+/**
+ * \tparam Map Environment representation type.
+ */
+template <class Map>
 struct LaserSensorModelInterface2d {
   /// Measurement type of the sensor: a point cloud for the range finder.
   using measurement_type = std::vector<std::pair<double, double>>;
@@ -72,6 +76,16 @@ struct LaserSensorModelInterface2d {
    * \param points The range finder points in the reference frame of the particle.
    */
   virtual void update_sensor(measurement_type&& points) = 0;
+
+  /// Update the sensor model with a new map.
+  /**
+   * This method updates the sensor model with a new map,
+   * i.e. a representation of the environment, that it needs
+   * to compute the weight of each particle.
+   *
+   * \param map The range finder map.
+   */
+  virtual void update_map(Map&& map) = 0;
 };
 
 }  // namespace beluga

beluga/include/beluga/sensor/beam_model.hpp

@@ -93,9 +93,7 @@ class BeamSensorModel : public Mixin {
       : Mixin(std::forward<Args>(rest)...),
         params_{params},
         grid_{std::move(grid)},
-        free_states_{
-            grid_.coordinates_for(grid_.free_cells(), OccupancyGrid::Frame::kGlobal) |
-            ranges::to<std::vector<Eigen::Vector2d>>} {}
+        free_states_{compute_free_states(grid_)} {}
 
   // TODO(ivanpauno): is sensor model the best place for this?
   // Maybe the map could be provided by a different part of the mixin,
@@ -165,11 +163,22 @@ class BeamSensorModel : public Mixin {
   /// \copydoc LaserSensorModelInterface2d::update_sensor(measurement_type&& points)
   void update_sensor(measurement_type&& points) final { points_ = std::move(points); }
 
+  /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map)
+  void update_map(OccupancyGrid&& map) final {
+    grid_ = std::move(map);
+    free_states_ = compute_free_states(grid_);
+  }
+
  private:
   param_type params_;
   OccupancyGrid grid_;
   std::vector<Eigen::Vector2d> free_states_;
   std::vector<std::pair<double, double>> points_;
+
+  static std::vector<Eigen::Vector2d> compute_free_states(const OccupancyGrid& grid) {
+    constexpr auto kFrame = OccupancyGrid::Frame::kGlobal;
+    return grid.coordinates_for(grid.free_cells(), kFrame) | ranges::to<std::vector>;
+  }
 };
 
 }  // namespace beluga

beluga/include/beluga/sensor/likelihood_field_model.hpp

@@ -97,9 +97,7 @@ class LikelihoodFieldModel : public Mixin {
       : Mixin(std::forward<Args>(rest)...),
         params_{params},
         grid_{std::move(grid)},
-        free_states_{
-            grid_.coordinates_for(grid_.free_cells(), OccupancyGrid::Frame::kGlobal) |
-            ranges::to<std::vector<Eigen::Vector2d>>},
+        free_states_{compute_free_states(grid_)},
         likelihood_field_{make_likelihood_field(params, grid_)} {}
 
   /// Returns the likelihood field, constructed from the provided map.
@@ -152,13 +150,25 @@ class LikelihoodFieldModel : public Mixin {
   /// \copydoc LaserSensorModelInterface2d::update_sensor(measurement_type&& points)
   void update_sensor(measurement_type&& points) final { points_ = std::move(points); }
 
+  /// \copydoc LaserSensorModelInterface2d::update_map(Map&& map)
+  void update_map(OccupancyGrid&& map) final {
+    grid_ = std::move(map);
+    free_states_ = compute_free_states(grid_);
+    likelihood_field_ = make_likelihood_field(params_, grid_);
+  }
+
  private:
   param_type params_;
   OccupancyGrid grid_;
   std::vector<Eigen::Vector2d> free_states_;
   ValueGrid2<double> likelihood_field_;
   std::vector<std::pair<double, double>> points_;
 
+  static std::vector<Eigen::Vector2d> compute_free_states(const OccupancyGrid& grid) {
+    constexpr auto kFrame = OccupancyGrid::Frame::kGlobal;
+    return grid.coordinates_for(grid.free_cells(), kFrame) | ranges::to<std::vector>;
+  }
+
   static ValueGrid2<double> make_likelihood_field(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid) {
     const auto squared_distance = [&grid,
                                    squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance](

beluga/test/beluga/sensor/test_beam_model.cpp

@@ -28,7 +28,7 @@ using beluga::testing::StaticOccupancyGrid;
 
 using UUT = ciabatta::mixin<
     ciabatta::curry<beluga::BeamSensorModel, StaticOccupancyGrid<5, 5>>::mixin,
-    ciabatta::provides<beluga::LaserSensorModelInterface2d>::mixin>;
+    ciabatta::provides<beluga::LaserSensorModelInterface2d<StaticOccupancyGrid<5, 5>>>::mixin>;
 
 BeamModelParam GetParams() {
   BeamModelParam ret;
@@ -74,4 +74,41 @@ TEST(BeamSensorModel, ImportanceWeight) {
   mixin.update_sensor(std::vector<std::pair<double, double>>{{params.beam_max_range, params.beam_max_range}});
   EXPECT_NEAR(0.00012500000000000003, mixin.importance_weight(grid.origin()), 1e-6);
 }
+
+TEST(BeamSensorModel, GridUpdates) {
+  const auto origin = Sophus::SE2d{};
+
+  constexpr double kResolution = 0.5;
+  // clang-format off
+  auto grid = StaticOccupancyGrid<5, 5>{{
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, true , false, false,
+    false, false, false, false, false,
+    false, false, false, false, false},
+    kResolution, origin};
+  // clang-format on
+
+  const auto params = GetParams();
+  auto mixin = UUT{params, std::move(grid)};
+
+  mixin.update_sensor(std::vector<std::pair<double, double>>{{1., 1.}});
+  EXPECT_NEAR(1.0171643824743635, mixin.importance_weight(origin), 1e-6);
+
+  // clang-format off
+  grid = StaticOccupancyGrid<5, 5>{{
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, false, false, false},
+    kResolution, origin};
+  // clang-format on
+
+  mixin.update_map(std::move(grid));
+
+  mixin.update_sensor(std::vector<std::pair<double, double>>{{1., 1.}});
+  EXPECT_NEAR(0.0, mixin.importance_weight(origin), 1e-3);
+}
+
 }  // namespace beluga

beluga/test/beluga/sensor/test_likelihood_field_model.cpp

@@ -26,7 +26,7 @@ using beluga::testing::StaticOccupancyGrid;
 
 using UUT = ciabatta::mixin<
     ciabatta::curry<beluga::LikelihoodFieldModel, StaticOccupancyGrid<5, 5>>::mixin,
-    ciabatta::provides<beluga::LaserSensorModelInterface2d>::mixin>;
+    ciabatta::provides<beluga::LaserSensorModelInterface2d<StaticOccupancyGrid<5, 5>>>::mixin>;
 
 TEST(LikelihoodFieldModel, LikelihoodField) {
   constexpr double kResolution = 0.5;
@@ -157,4 +157,39 @@ TEST(LikelihoodFieldModel, GridWithRotationAndOffset) {
   ASSERT_NEAR(2.068, mixin.importance_weight(grid.origin()), 0.003);
 }
 
+TEST(LikelihoodFieldModel, GridUpdates) {
+  const auto origin = Sophus::SE2d{};
+
+  constexpr double kResolution = 0.5;
+  // clang-format off
+  auto grid = StaticOccupancyGrid<5, 5>{{
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, true , false, false,
+    false, false, false, false, false,
+    false, false, false, false, false},
+    kResolution, origin};
+  // clang-format on
+
+  const auto params = beluga::LikelihoodFieldModelParam{2.0, 20.0, 0.5, 0.5, 0.2};
+  auto mixin = UUT{params, std::move(grid)};
+
+  mixin.update_sensor(std::vector<std::pair<double, double>>{{1., 1.}});
+  EXPECT_NEAR(2.068577607986223, mixin.importance_weight(origin), 1e-6);
+
+  // clang-format off
+  grid = StaticOccupancyGrid<5, 5>{{
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, false, false, true},
+    kResolution, origin};
+  // clang-format on
+  mixin.update_map(std::move(grid));
+
+  mixin.update_sensor(std::vector<std::pair<double, double>>{{1., 1.}});
+  EXPECT_NEAR(1.0, mixin.importance_weight(origin), 1e-3);
+}
+
 }  // namespace

beluga_amcl/include/beluga_amcl/private/amcl_node.hpp

@@ -33,11 +33,15 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <std_srvs/srv/empty.hpp>
 
+#include "beluga_amcl/occupancy_grid.hpp"
 #include "beluga_amcl/private/execution_policy.hpp"
 
 namespace beluga_amcl
 {
 
+using LaserLocalizationInterface2d =
+  beluga::LaserLocalizationInterface2d<OccupancyGrid>;
+
 class AmclNode : public rclcpp_lifecycle::LifecycleNode
 {
 public:
@@ -54,6 +58,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
   CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override;
 
   void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr);
+  std::unique_ptr<LaserLocalizationInterface2d>
+  make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr);
   void timer_callback();
   template<typename ExecutionPolicy>
   void laser_callback(
@@ -66,7 +72,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
   void reinitialize_with_pose(const Sophus::SE2d & pose, const Eigen::Matrix3d & covariance);
 
-  std::unique_ptr<beluga::LaserLocalizationInterface2d> particle_filter_;
+  std::unique_ptr<LaserLocalizationInterface2d> particle_filter_;
   execution::Policy execution_policy_;
 
   std::unique_ptr<bond::Bond> bond_;