Addressing comments

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

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;
@@ -92,31 +92,6 @@ auto nearest_obstacle_distance_map(
   return distance_map;
 }
 
-/// A distance map function considering unknown values.
-template <class OccupancyGrid, class DistanceFunction, class NeighborsFunction>
-auto nearest_distance_map(
-    const OccupancyGrid& grid,
-    DistanceFunction&& distance_function,
-    NeighborsFunction&& neighbors_function,
-    const double& unknown_distance_map,
-    const bool& explore_unknown) {
-  auto obstacle_map = grid.obstacle_data();
-
-  auto distance_map = nearest_obstacle_distance_map(obstacle_map, distance_function, neighbors_function);
-
-  if (explore_unknown) {
-    auto uknown_map = grid.unknown_data();
-
-    for (auto [index, is_unknown] : ranges::views::enumerate(uknown_map)) {
-      if (is_unknown) {
-        distance_map[index] = unknown_distance_map;
-      }
-    }
-  }
-
-  return distance_map;
-}
-
 }  // namespace beluga
 
 #endif

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,16 +175,16 @@ 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 of true booleans to focus on obstacles data 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 grid data using true booleans for unknowns.
-  [[nodiscard]] auto unknown_data() const {
+  /// Retrieves a mask of true booleans to focus on unkwnown data 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);

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;
+  /// Bool parameter to decide if unknown spaces should be explored
+  bool model_unknown_space = false;
 };
 
 /// Likelihood field sensor model for range finders.
@@ -95,13 +98,11 @@ class LikelihoodFieldModel {
    * \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.
-   * \param explore_unknown Bool parameter to decide if unknown spaces should be explored
    */
-  explicit LikelihoodFieldModel(const param_type& params, const map_type& grid, const bool& explore_unknown = false)
+  explicit LikelihoodFieldModel(const param_type& params, const map_type& grid)
       : params_{params},
-        likelihood_field_{make_likelihood_field(params, grid, explore_unknown)},
-        world_to_likelihood_field_transform_{grid.origin().inverse()},
-        explore_unknown_{explore_unknown} {}
+        likelihood_field_{make_likelihood_field(params, grid)},
+        world_to_likelihood_field_transform_{grid.origin().inverse()} {}
 
   /// Returns the likelihood field, constructed from the provided map.
   [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_; }
@@ -146,20 +147,16 @@ class LikelihoodFieldModel {
    * \param grid New occupancy grid representing the static map.
    */
   void update_map(const map_type& grid) {
-    likelihood_field_ = make_likelihood_field(params_, grid, explore_unknown_);
+    likelihood_field_ = make_likelihood_field(params_, grid);
     world_to_likelihood_field_transform_ = grid.origin().inverse();
   }
 
  private:
   param_type params_;
   ValueGrid2<double> likelihood_field_;
   Sophus::SE2d world_to_likelihood_field_transform_;
-  bool explore_unknown_;
 
-  static ValueGrid2<double> make_likelihood_field(
-      const LikelihoodFieldModelParam& params,
-      const OccupancyGrid& grid,
-      const bool& explore_unknown = false) {
+  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](
                                       std::size_t first, std::size_t second) {
@@ -168,22 +165,25 @@ class LikelihoodFieldModel {
 
     /// 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 double inverse_max_distance = 1 / params.max_laser_distance;
-    const double unknown_distance_map = -two_squared_sigma * std::log((inverse_max_distance - offset) / amplitude);
 
     const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); };
 
-    const auto distance_map = nearest_distance_map(
-        grid, squared_distance, neighborhood, unknown_distance_map,
-        explore_unknown);  // nearest_obstacle_distance_map(grid.obstacle_data(), squared_distance,
-                           // neighborhood);
+    auto distance_map = 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);
+
+      distance_map |= beluga::actions::overlay(grid.unknown_mask(), background_distance);
+    }
 
-    const auto to_likelihood = [amplitude = amplitude, two_squared_sigma = two_squared_sigma,
-                                offset = offset](double squared_distance) {
-      assert(two_squared_sigma > 0.0);
-      assert(amplitude > 0.0);
+    const auto to_likelihood = [=](double squared_distance) {
       return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;
     };
 

beluga/test/beluga/sensor/data/test_occupancy_grid.cpp

@@ -87,7 +87,7 @@ TEST(OccupancyGrid2, ObstacleData) {
 #pragma GCC diagnostic ignored "-Warray-bounds"
 #pragma GCC diagnostic ignored "-Wstringop-overflow"
   // Data and obstacle data are equivalent in this case
-  ASSERT_THAT(grid.obstacle_data() | ranges::to<std::vector>, testing::Pointwise(testing::Eq(), grid.data()));
+  ASSERT_THAT(grid.obstacle_mask() | ranges::to<std::vector>, testing::Pointwise(testing::Eq(), grid.data()));
 #pragma GCC diagnostic pop
 }