Added: beam-skipping to likelihood-field-prob

beluga/include/beluga/sensor/likelihood_field_model_base.hpp

@@ -121,18 +121,26 @@ class LikelihoodFieldModelBase {
   Sophus::SE2d world_to_likelihood_field_transform_; /*!< Transformation from world coordinates to the likelihood field
                                                         coordinate system. */
 
-  /// Creates a likelihood field from an occupancy grid.
+  /// Gaussian likelihood profile: maps a squared distance-to-obstacle to a likelihood value.
   /**
-   * \param params Parameters to configure the likelihood field.
-   * \param grid Occupancy grid representing the static map.
-   * \return Likelihood field computed from the occupancy grid.
+   * Holds the gaussian coefficients derived from the model parameters so they are computed once
+   * and reused. Shared between `make_likelihood_field()` (applied over every grid cell) and
+   * derived models that need the likelihood value at a specific distance (see
+   * beluga::LikelihoodFieldProbModel).
    */
-  static ValueGrid2<float> make_likelihood_field(const param_type& params, const OccupancyGrid& grid) {
-    const auto squared_distance = [&grid](std::size_t first, std::size_t second) {
-      return static_cast<float>((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm());
-    };
+  struct LikelihoodProfile {
+    double amplitude;          ///< Peak contribution of the obstacle-hit gaussian.
+    double two_squared_sigma;  ///< 2 * sigma_hit^2, the gaussian denominator.
+    double offset;             ///< Constant contribution of random perception noise.
+
+    /// Returns the likelihood value for a given squared distance to the nearest obstacle.
+    [[nodiscard]] double operator()(double squared_distance) const {
+      return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;
+    }
+  };
 
-    /// Pre-computed variables
+  /// Builds the likelihood profile (gaussian coefficients) from the model parameters.
+  [[nodiscard]] static LikelihoodProfile make_likelihood_profile(const param_type& params) {
     const double two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit;
     assert(two_squared_sigma > 0.0);
 
@@ -141,10 +149,22 @@ class LikelihoodFieldModelBase {
 
     const double offset = params.z_random / params.max_laser_distance;
 
-    const auto to_likelihood = [amplitude, two_squared_sigma, offset](double squared_distance) {
-      return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;
+    return LikelihoodProfile{amplitude, two_squared_sigma, offset};
+  }
+
+  /// Creates a likelihood field from an occupancy grid.
+  /**
+   * \param params Parameters to configure the likelihood field.
+   * \param grid Occupancy grid representing the static map.
+   * \return Likelihood field computed from the occupancy grid.
+   */
+  static ValueGrid2<float> make_likelihood_field(const param_type& params, const OccupancyGrid& grid) {
+    const auto squared_distance = [&grid](std::size_t first, std::size_t second) {
+      return static_cast<float>((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm());
     };
 
+    const auto profile = make_likelihood_profile(params);
+
     const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); };
 
     const auto squared_max_distance = static_cast<float>(params.max_obstacle_distance * params.max_obstacle_distance);
@@ -160,7 +180,7 @@ class LikelihoodFieldModelBase {
     if (params.model_unknown_space) {
       const auto inverse_max_distance = 1 / params.max_laser_distance;
       const auto squared_background_distance =
-          -two_squared_sigma * std::log((inverse_max_distance - offset) / amplitude);
+          -profile.two_squared_sigma * std::log((inverse_max_distance - profile.offset) / profile.amplitude);
 
       const auto get_effective_unknown_value = [only_obstacle_boundaries =
                                                     params.only_obstacle_boundaries](auto&& tuple) {
@@ -179,7 +199,7 @@ class LikelihoodFieldModelBase {
     }
 
     auto likelihood_values = std::move(distance_map) |  //
-                             ranges::actions::transform(to_likelihood);
+                             ranges::actions::transform(profile);
 
     return ValueGrid2<float>{std::move(likelihood_values), grid.width(), grid.resolution()};
   }

beluga/include/beluga/sensor/likelihood_field_prob_model.hpp

@@ -17,22 +17,51 @@
 
 #include <algorithm>
 #include <beluga/sensor/likelihood_field_model_base.hpp>
+#include <cassert>
 #include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <functional>
 #include <random>
+#include <range/v3/numeric/accumulate.hpp>
+#include <range/v3/view/zip.hpp>
 #include <vector>
 
+#include <sophus/se2.hpp>
+
 /**
  * \file
  * \brief Implementation of a likelihood field prob sensor model for range finders.
  */
 
 namespace beluga {
 
-/// Parameters used to construct a LikelihoodFieldProbModelParam instance.
+/// Parameters used to construct a LikelihoodFieldProbModel instance.
 /**
  * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3.
+ *
+ * Inherits the likelihood field parameters from beluga::LikelihoodFieldModelBaseParam and
+ * adds the fields configuring the optional beam skipping heuristic, which detects beams that
+ * disagree with the map across a large fraction of the particle set (e.g. caused by
+ * unmapped/dynamic obstacles) and excludes them from the weight computation. Beam skipping is
+ * disabled by default; see https://github.com/Ekumen-OS/beluga/issues/187 for context.
  */
-using LikelihoodFieldProbModelParam = LikelihoodFieldModelBaseParam;
+struct LikelihoodFieldProbModelParam : public LikelihoodFieldModelBaseParam {
+  /// Whether to enable the beam skipping heuristic.
+  bool do_beamskip = false;
+  /// Distance to the nearest obstacle below which a beam is considered to agree with the map.
+  /**
+   * Must not be greater than `max_obstacle_distance` when `do_beamskip` is enabled. The likelihood
+   * field saturates at `max_obstacle_distance`, so beyond that distance it is flat and the agreement
+   * test (evaluated on the likelihood field, see beluga::LikelihoodFieldProbModel) stops
+   * discriminating: every beam landing further away would be counted as agreeing with the map.
+   */
+  double beam_skip_distance = 0.5;
+  /// Fraction of particles that must agree on a beam for it to be kept.
+  double beam_skip_threshold = 0.3;
+  /// If the fraction of skipped beams exceeds this value, skipping is disabled for the update.
+  double beam_skip_error_threshold = 0.9;
+};
 
 /// Likelihood field prob sensor model for range finders.
 /**
@@ -54,10 +83,107 @@ class LikelihoodFieldProbModel : public LikelihoodFieldModelBase<OccupancyGrid>
 
   /// Constructs a LikelihoodFieldProbModel instance.
   /**
-   * @copydoc LikelihoodFieldModelBase::LikelihoodFieldModelBase
+   * \param params Parameters to configure this instance.
+   *  See beluga::LikelihoodFieldProbModelParam for details.
+   * \param grid Occupancy grid representing the static map that the sensor model
+   *  uses to compute a likelihood field for lidar hits and compute importance weights
+   *  for particle states.
+   *
+   * When beam skipping is enabled, `params.beam_skip_distance` must not be greater than
+   * `params.max_obstacle_distance`. See beluga::LikelihoodFieldProbModelParam::beam_skip_distance.
    */
   explicit LikelihoodFieldProbModel(const param_type& params, const map_type& grid)
-      : LikelihoodFieldModelBase<OccupancyGrid>(params, grid) {}
+      : LikelihoodFieldModelBase<OccupancyGrid>(params, grid),
+        do_beamskip_{params.do_beamskip},
+        beam_skip_threshold_{params.beam_skip_threshold},
+        beam_skip_error_threshold_{params.beam_skip_error_threshold},
+        likelihood_threshold_{compute_likelihood_threshold(params)} {}
+
+  /// Precomputes the beam skipping mask from the current particle states.
+  /**
+   * Runs the first pass of the beam skipping heuristic: for every beam, it counts the
+   * fraction of particle states for which the beam endpoint lands close enough to a mapped
+   * obstacle (within `beam_skip_distance`, expressed here as a likelihood threshold). Beams
+   * for which that fraction falls below `beam_skip_threshold` are masked out and ignored by
+   * the subsequent weight computation in `operator()`. If skipping would discard more than
+   * `beam_skip_error_threshold` of the beams, the mask is reset so that all beams are used,
+   * preventing filter divergence. When beam skipping is disabled this only sizes the mask to the
+   * measurement, leaving every beam enabled.
+   *
+   * Must be called once per update, after motion propagation and before reweighting.
+   *
+   * \tparam StateRange A range of particle states (Sophus::SE2d).
+   * \param points 2D lidar hit points in the reference frame of particle states.
+   * \param states Range with the (propagated) particle states for the current update.
+   */
+  template <class StateRange>
+  void prepare(const measurement_type& points, StateRange&& states) {
+    // The mask is always sized to match the measurement, so that `operator()` can consume it
+    // unconditionally: when beam skipping is disabled every beam stays enabled.
+    const std::size_t num_beams = points.size();
+    beam_mask_.assign(num_beams, std::uint8_t{1});
+
+    if (!do_beamskip_ || num_beams == 0) {
+      return;
+    }
+
+    const auto unknown_space_occupancy_prob = static_cast<float>(1. / this->params_.max_laser_distance);
+    std::vector<std::size_t> obs_count(num_beams, 0);
+    std::size_t num_states = 0;
+
+    for (const auto& state : states) {
+      ++num_states;
+      const auto transform = this->world_to_likelihood_field_transform_ * state;
+      const auto x_offset = transform.translation().x();
+      const auto y_offset = transform.translation().y();
+      const auto cos_theta = transform.so2().unit_complex().x();
+      const auto sin_theta = transform.so2().unit_complex().y();
+      for (std::size_t i = 0; i < num_beams; ++i) {
+        const auto& point = points[i];
+        // Transform the end point of the laser to the grid local coordinate system.
+        // Not using Eigen/Sophus because they make the routine x10 slower.
+        // See `benchmark_likelihood_field_model.cpp` for reference.
+        const auto x = point.first * cos_theta - point.second * sin_theta + x_offset;
+        const auto y = point.first * sin_theta + point.second * cos_theta + y_offset;
+        const auto pz = this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob);
+        // The likelihood field is monotonically decreasing in the distance to the nearest
+        // obstacle, so "distance < beam_skip_distance" is equivalent to "pz > threshold".
+        if (pz > likelihood_threshold_) {
+          ++obs_count[i];
+        }
+      }
+    }
+
+    if (num_states == 0) {
+      return;
+    }
+
+    std::size_t skipped = 0;
+    for (std::size_t i = 0; i < num_beams; ++i) {
+      const double ratio = static_cast<double>(obs_count[i]) / static_cast<double>(num_states);
+      beam_mask_[i] = static_cast<std::uint8_t>(ratio > beam_skip_threshold_);
+      if (!beam_mask_[i]) {
+        ++skipped;
+      }
+    }
+
+    // Safety fallback: if too many beams would be skipped, integrate all of them instead.
+    const double skipped_ratio = static_cast<double>(skipped) / static_cast<double>(num_beams);
+    if (skipped_ratio > beam_skip_error_threshold_) {
+      std::fill(beam_mask_.begin(), beam_mask_.end(), std::uint8_t{1});
+    }
+  }
+
+  /// Returns the current beam skipping mask (one flag per beam, non-zero means the beam is used).
+  /**
+   * The mask is populated by `prepare()`. It is empty until the first call, which `operator()`
+   * treats as using every beam. Mainly useful for introspection and testing.
+   *
+   * A `std::vector<std::uint8_t>` is used instead of `std::vector<bool>` to avoid the bit-packed
+   * specialization: the mask is read once per beam per particle in the reweight hot path, where a
+   * plain byte load is faster and more cache friendly than the bit-masking `std::vector<bool>` does.
+   */
+  [[nodiscard]] const std::vector<std::uint8_t>& beam_mask() const { return beam_mask_; }
 
   /// Returns a state weighting function conditioned on 2D lidar hits.
   /**
@@ -66,28 +192,66 @@ class LikelihoodFieldProbModel : public LikelihoodFieldModelBase<OccupancyGrid>
    *  and borrowing a reference to this sensor model (and thus their lifetime are bound).
    */
   [[nodiscard]] auto operator()(measurement_type&& points) const {
-    return [this, points = std::move(points)](const state_type& state) -> weight_type {
+    // Beams masked out by prepare() contribute nothing to the weight. The mask is resolved once
+    // here, so that the per-particle loop below can zip it against the measurement without any
+    // further bookkeeping. When prepare() was not called for this measurement (e.g. when the model
+    // is used standalone, outside the two pass update) every beam is integrated, reproducing the
+    // plain likelihood field prob behavior.
+    auto mask = beam_mask_.size() == points.size()  //
+                    ? beam_mask_
+                    : std::vector<std::uint8_t>(points.size(), std::uint8_t{1});
+
+    return [this, points = std::move(points), mask = std::move(mask)](const state_type& state) -> weight_type {
       const auto transform = this->world_to_likelihood_field_transform_ * state;
       const auto x_offset = transform.translation().x();
       const auto y_offset = transform.translation().y();
       const auto cos_theta = transform.so2().unit_complex().x();
       const auto sin_theta = transform.so2().unit_complex().y();
       const auto unknown_space_occupancy_prob = static_cast<float>(1. / this->params_.max_laser_distance);
 
-      return std::exp(std::transform_reduce(
-          points.cbegin(), points.cend(), 0.0, std::plus{},
-          [this, x_offset, y_offset, cos_theta, sin_theta, unknown_space_occupancy_prob](const auto& point) {
-            // Transform the end point of the laser to the grid local coordinate system.
-            // Not using Eigen/Sophus because they make the routine x10 slower.
-            // See `benchmark_likelihood_field_model.cpp` for reference.
-            const auto x = point.first * cos_theta - point.second * sin_theta + x_offset;
-            const auto y = point.first * sin_theta + point.second * cos_theta + y_offset;
-            const auto pz =
-                static_cast<double>(this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob));
-            return std::log(pz);
-          }));
+      const auto beam_log_likelihood = [this, x_offset, y_offset, cos_theta, sin_theta,
+                                        unknown_space_occupancy_prob](const auto& tuple) {
+        const auto& [point, is_beam_used] = tuple;
+        if (!is_beam_used) {
+          return 0.0;  // Neutral element of the log-likelihood sum below.
+        }
+        // Transform the end point of the laser to the grid local coordinate system.
+        // Not using Eigen/Sophus because they make the routine x10 slower.
+        // See `benchmark_likelihood_field_model.cpp` for reference.
+        const auto x = point.first * cos_theta - point.second * sin_theta + x_offset;
+        const auto y = point.first * sin_theta + point.second * cos_theta + y_offset;
+        const auto pz =
+            static_cast<double>(this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob));
+        return std::log(pz);
+      };
+
+      return std::exp(ranges::accumulate(ranges::views::zip(points, mask), 0.0, std::plus{}, beam_log_likelihood));
     };
   }
+
+ private:
+  bool do_beamskip_;                     ///< Whether the beam skipping heuristic is enabled.
+  double beam_skip_threshold_;           ///< Fraction of particles that must agree for a beam to be kept.
+  double beam_skip_error_threshold_;     ///< Skipped-beam fraction above which skipping is disabled.
+  float likelihood_threshold_;           ///< Likelihood equivalent of `beam_skip_distance`.
+  std::vector<std::uint8_t> beam_mask_;  ///< Per-beam mask computed by `prepare()` (non-zero means used).
+
+  /// Converts `beam_skip_distance` into the equivalent likelihood field value.
+  /**
+   * Reuses the base class likelihood profile (the same gaussian used to build the likelihood
+   * field), so that the "distance to obstacle < beam_skip_distance" agreement test can be
+   * evaluated directly on the precomputed likelihood field without keeping the distance map around.
+   *
+   * That equivalence only holds within the range the likelihood field actually resolves distances
+   * in, hence `beam_skip_distance <= max_obstacle_distance` is a requirement of the configuration.
+   */
+  static float compute_likelihood_threshold(const param_type& params) {
+    assert(!params.do_beamskip || params.beam_skip_distance <= params.max_obstacle_distance);
+
+    const auto profile = LikelihoodFieldModelBase<OccupancyGrid>::make_likelihood_profile(params);
+    const double squared_distance = params.beam_skip_distance * params.beam_skip_distance;
+    return static_cast<float>(profile(squared_distance));
+  }
 };
 
 }  // namespace beluga

beluga/include/beluga/sensor/primitives.hpp

@@ -15,6 +15,10 @@
 #ifndef BELUGA_SENSOR_PRIMITIVES_HPP
 #define BELUGA_SENSOR_PRIMITIVES_HPP
 
+#include <type_traits>
+#include <utility>
+#include <vector>
+
 // /**
 //  * \file
 //  * \brief Implementation of sensor primitives to abstract member access.
@@ -37,6 +41,26 @@ struct has_likelihood_field<T, std::void_t<decltype(std::declval<T>().likelihood
 template <class T>
 inline constexpr bool has_likelihood_field_v = has_likelihood_field<T>::value;
 
+// Primary template which defaults to `false_type`.
+// A specialization will override this if the method is detected.
+template <class T, class = void>
+struct has_beam_skip : std::false_type {};
+
+// Specialization. Uses SFINAE to detect whether a sensor model exposes a
+// `prepare(measurement, states)` method, used to precompute a beam skipping mask over the
+// whole particle set before per-particle reweighting. The states range is probed with a
+// `std::vector<state_type>`; any range type is accepted by the actual templated method.
+template <class T>
+struct has_beam_skip<
+    T,
+    std::void_t<decltype(std::declval<T&>().prepare(
+        std::declval<const typename T::measurement_type&>(),
+        std::declval<const std::vector<typename T::state_type>&>()))>> : std::true_type {};
+
+/// Trait variable that indicates whether a type `T` supports beam skipping via `prepare()`.
+template <class T>
+inline constexpr bool has_beam_skip_v = has_beam_skip<T>::value;
+
 }  // namespace beluga
 
 #endif  // BELUGA_SENSOR_PRIMITIVES_HPP