Add a new feature: model unkwnon spaces in likelihood model

Signed-off-by: Diego Palma <dpalma@symbotic.com>

beluga/include/beluga/actions/overlay.hpp

@@ -0,0 +1,126 @@
+// Copyright 2024 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 BELUGA_ACTIONS_OVERLAY_HPP
+#define BELUGA_ACTIONS_OVERLAY_HPP
+
+#include <algorithm>
+#include <execution>
+
+#include <range/v3/action/action.hpp>
+#include <range/v3/view/common.hpp>
+#include <range/v3/view/transform.hpp>
+
+/**
+ * \file
+ * \brief Implementation of the overlay range adaptor object
+ */
+namespace beluga::actions {
+
+namespace detail {
+
+/// Implementation detail for an overlay range adaptor object.
+struct overlay_base_fn {
+  /// Overload that implements an overlay of a value in a range.
+  /**
+   * \tparam ExecutionPolicy An [execution policy](https://en.cppreference.com/w/cpp/algorithm/execution_policy_tag_t).
+   * \tparam Range An [input range](https://en.cppreference.com/w/cpp/ranges/input_range).
+   * \tparam MaskRange An [input range](https://en.cppreference.com/w/cpp/ranges/input_range).
+   * \param policy The execution policy to use.
+   * \param range An existing range to apply this action to.
+   * \param mask The mask where the values will be overlaid.
+   * \param mask_value The value to be overlaid.
+   */
+  template <
+      class ExecutionPolicy,
+      class Range,
+      class MaskRange,
+      class Mask = ranges::range_value_t<Range>,
+      std::enable_if_t<std::is_execution_policy_v<std::decay_t<ExecutionPolicy>>, int> = 0,
+      std::enable_if_t<ranges::range<Range>, int> = 0,
+      std::enable_if_t<ranges::range<MaskRange>, int> = 0>
+  constexpr auto operator()(ExecutionPolicy&& policy, Range& range, MaskRange& mask, Mask mask_value) const -> Range& {
+    auto map = range | ranges::views::common;
+
+    std::transform(
+        policy,            //
+        std::begin(map),   //
+        std::end(map),     //
+        std::begin(mask),  //
+        std::begin(map),   //
+        [mask_value](const auto& base_value, bool flag) { return flag ? mask_value : base_value; });
+
+    return range;
+  }
+
+  /// Overload that re-orders arguments from an action closure.
+  template <
+      class ExecutionPolicy,
+      class Range,
+      class MaskRange,
+      class Mask = ranges::range_value_t<Range>,
+      std::enable_if_t<std::is_execution_policy_v<ExecutionPolicy>, int> = 0,
+      std::enable_if_t<ranges::range<Range>, int> = 0,
+      std::enable_if_t<ranges::range<MaskRange>, int> = 0>
+  constexpr auto operator()(Range&& range, MaskRange& mask, Mask mask_value, ExecutionPolicy policy) const -> Range& {
+    return (*this)(std::move(policy), std::forward<Range>(range), mask, mask_value);
+  }
+
+  /// Overload that returns an action closure to compose with other actions.
+  template <
+      class ExecutionPolicy,
+      class MaskRange,
+      class Mask,
+      std::enable_if_t<std::is_execution_policy_v<ExecutionPolicy>, int> = 0,
+      std::enable_if_t<ranges::range<MaskRange>, int> = 0>
+  constexpr auto operator()(ExecutionPolicy policy, MaskRange&& mask, const Mask& mask_value) const {
+    return ranges::make_action_closure(
+        ranges::bind_back(overlay_base_fn{}, std::forward<MaskRange>(mask), mask_value, std::move(policy)));
+  }
+};
+
+/// Implementation detail for an overlay range adaptor object with a default execution policy.
+struct overlay_fn : public overlay_base_fn {
+  using overlay_base_fn::operator();
+
+  /// Overload that defines a default execution policy.
+  template <
+      class Range,
+      class MaskRange,
+      class Mask = ranges::range_value_t<Range>,
+      std::enable_if_t<ranges::range<Range>, int> = 0,
+      std::enable_if_t<ranges::range<MaskRange>, int> = 0>
+  constexpr auto operator()(Range&& range, MaskRange& mask, Mask mask_value) const -> Range& {
+    return (*this)(std::execution::seq, std::forward<Range>(range), mask, mask_value);
+  }
+
+  /// Overload that returns an action closure to compose with other actions.
+  template <class MaskRange, class Mask, std::enable_if_t<ranges::range<MaskRange>, int> = 0>
+  constexpr auto operator()(MaskRange&& mask, const Mask& mask_value) const {
+    return ranges::make_action_closure(ranges::bind_back(overlay_fn{}, std::forward<MaskRange>(mask), mask_value));
+  }
+};
+
+}  // namespace detail
+
+/// [Range adaptor object](https://en.cppreference.com/w/cpp/named_req/RangeAdaptorObject) that
+/// can overlay a range of values (or a range of particles).
+/**
+ * The `overlay` range adaptor allows to overlay the values of the range that match a mask.
+ * All the values are overlaid for a given value.
+ */
+inline constexpr ranges::actions::action_closure<detail::overlay_fn> overlay;
+}  // namespace beluga::actions
+
+#endif

beluga/include/beluga/algorithm/distance_map.hpp

@@ -41,7 +41,7 @@ namespace beluga {
  *  (std::size_t) -> NeighborsT, where NeighborsT is a
  *    [Range](https://en.cppreference.com/w/cpp/ranges/range)
  *    with value type std::size_t.
- * \param obstacle_map A map that represents obstacles in an environment.
+ * \param obstacle_mask A mask that represents obstacles in an environment.
  *  If the value of a cell is True, the cell has an obstacle.
  * \param distance_function Given the indexes of two cells in the map i and j,
  *  obstacle_map(i, j) must return the distance between the two cells.
@@ -52,7 +52,7 @@ namespace beluga {
  */
 template <class Range, class DistanceFunction, class NeighborsFunction>
 auto nearest_obstacle_distance_map(
-    Range&& obstacle_map,
+    Range&& obstacle_mask,
     DistanceFunction&& distance_function,
     NeighborsFunction&& neighbors_function) {
   struct IndexPair {
@@ -61,15 +61,15 @@ auto nearest_obstacle_distance_map(
   };
 
   using DistanceType = std::invoke_result_t<DistanceFunction, std::size_t, std::size_t>;
-  auto distance_map = std::vector<DistanceType>(ranges::size(obstacle_map));
-  auto visited = std::vector<bool>(ranges::size(obstacle_map), false);
+  auto distance_map = std::vector<DistanceType>(ranges::size(obstacle_mask));
+  auto visited = std::vector<bool>(ranges::size(obstacle_mask), false);
 
   auto compare = [&distance_map](const IndexPair& first, const IndexPair& second) {
     return distance_map[first.index] > distance_map[second.index];
   };
   auto queue = std::priority_queue<IndexPair, std::vector<IndexPair>, decltype(compare)>{compare};
 
-  for (auto [index, is_obstacle] : ranges::views::enumerate(obstacle_map)) {
+  for (auto [index, is_obstacle] : ranges::views::enumerate(obstacle_mask)) {
     if (is_obstacle) {
       visited[index] = true;
       distance_map[index] = 0;

beluga/include/beluga/sensor/data/occupancy_grid.hpp

@@ -68,7 +68,8 @@ namespace beluga {
  *   `g.coordinates_for(r, f)` returns a range of embedding space coordinates in the
  *   corresponding frame as `Eigen::Vector2d` values;
  * - `g.free_cells()` returns a range of `std::size_t` indices to free grid cells;
- * - `g.obstacle_data()` returns a range of `bool` values, representing grid cell occupancy;
+ * - `g.obstacle_mask()` returns a range of `bool` values, representing grid cell occupancy;
+ * - `g.unknown_mask()` returns a range of `bool` values, representing the unkwnown space of the grid cells;
  */
 
 /// Occupancy 2D grid base type.
@@ -174,13 +175,21 @@ class BaseOccupancyGrid2 : public BaseLinearGrid2<Derived> {
            ranges::views::transform([](const auto& tuple) { return std::get<0>(tuple); });
   }
 
-  /// Retrieves grid data using true booleans for obstacles.
-  [[nodiscard]] auto obstacle_data() const {
+  /// Retrieves a mask over occupied cells in the grid.
+  [[nodiscard]] auto obstacle_mask() const {
     return this->self().data() |
            ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) {
              return value_traits.is_occupied(value);
            });
   }
+
+  /// Retrieves a mask over unknown cells in the grid.
+  [[nodiscard]] auto unknown_mask() const {
+    return this->self().data() |
+           ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) {
+             return value_traits.is_unknown(value);
+           });
+  }
 };
 
 }  // namespace beluga

beluga/include/beluga/sensor/likelihood_field_model.hpp

@@ -20,6 +20,7 @@
 #include <random>
 #include <vector>
 
+#include <beluga/actions/overlay.hpp>
 #include <beluga/algorithm/distance_map.hpp>
 #include <beluga/sensor/data/occupancy_grid.hpp>
 #include <beluga/sensor/data/value_grid.hpp>
@@ -58,6 +59,8 @@ struct LikelihoodFieldModelParam {
    * Used to calculate the probability of the obstacle being hit.
    */
   double sigma_hit = 0.2;
+  /// Whether to model unknown space or assume it free.
+  bool model_unknown_space = false;
 };
 
 /// Likelihood field sensor model for range finders.
@@ -161,20 +164,32 @@ class LikelihoodFieldModel {
       return std::min(squared_distance, squared_max_distance);
     };
 
-    const auto to_likelihood = [amplitude =
-                                    params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants<double>::pi())),
-                                two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit,
-                                offset = params.z_random / params.max_laser_distance](double squared_distance) {
-      assert(two_squared_sigma > 0.0);
-      assert(amplitude > 0.0);
+    /// Pre-computed variables
+    const double two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit;
+    assert(two_squared_sigma > 0.0);
+
+    const double amplitude = params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants<double>::pi()));
+    assert(amplitude > 0.0);
+
+    const double offset = params.z_random / params.max_laser_distance;
+
+    const auto to_likelihood = [=](double squared_distance) {
       return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;
     };
 
     const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); };
 
     // determine distances to obstacles and calculate likelihood values in-place
     // to minimize memory usage when dealing with large maps
-    auto likelihood_values = nearest_obstacle_distance_map(grid.obstacle_data(), squared_distance, neighborhood);
+    auto likelihood_values = nearest_obstacle_distance_map(grid.obstacle_mask(), squared_distance, neighborhood);
+
+    if (params.model_unknown_space) {
+      const double inverse_max_distance = 1 / params.max_laser_distance;
+      const double background_distance = -two_squared_sigma * std::log((inverse_max_distance - offset) / amplitude);
+
+      likelihood_values |= beluga::actions::overlay(grid.unknown_mask(), background_distance);
+    }
+
     std::transform(
         likelihood_values.begin(), likelihood_values.end(), likelihood_values.begin(), truncate_to_max_distance);
     std::transform(likelihood_values.begin(), likelihood_values.end(), likelihood_values.begin(), to_likelihood);

beluga/test/beluga/CMakeLists.txt

@@ -18,6 +18,7 @@ add_executable(
   test_beluga
   actions/test_assign.cpp
   actions/test_normalize.cpp
+  actions/test_overlay.cpp
   actions/test_propagate.cpp
   actions/test_reweight.cpp
   algorithm/raycasting/test_bresenham.cpp
@@ -51,6 +52,7 @@ add_executable(
   sensor/test_beam_model.cpp
   sensor/test_bearing_sensor_model.cpp
   sensor/test_landmark_sensor_model.cpp
+  sensor/test_lfm_with_unknown_space.cpp
   sensor/test_likelihood_field_model.cpp
   sensor/test_ndt_model.cpp
   test_3d_embedding.cpp