Addressing second pass

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

beluga/include/beluga/algorithm/distance_map.hpp

@@ -18,7 +18,6 @@
 #include <queue>
 #include <vector>
 
-#include <beluga/sensor/data/occupancy_grid.hpp>
 #include <range/v3/range/access.hpp>
 #include <range/v3/range/primitives.hpp>
 #include <range/v3/view/enumerate.hpp>
@@ -94,52 +93,27 @@ auto nearest_obstacle_distance_map(
 }
 
 /// A distance map function considering unknown values.
-template <class Range, class DistanceFunction, class NeighborsFunction>
-auto nearest_obstacle_unknown_distance_map(
-    Range&& obstacle_map,
+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) {
-  struct IndexPair {
-    std::size_t nearest_obstacle_index;
-    std::size_t index;
-  };
+    const double& unknown_distance_map,
+    const bool& explore_unknown) {
+  auto obstacle_map = grid.obstacle_data();
 
-  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 = nearest_obstacle_distance_map(obstacle_map, distance_function, neighbors_function);
 
-  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, enum_data] : ranges::views::enumerate(obstacle_map)) {
-    if (enum_data == beluga::GridStatus::kOccupied) {
-      visited[index] = true;
-      distance_map[index] = 0;
-      queue.push(IndexPair{index, index});  // The nearest obstacle is itself
-    }
-  }
+  if (explore_unknown) {
+    auto uknown_map = grid.unknown_data();
 
-  while (!queue.empty()) {
-    auto parent = queue.top();
-    queue.pop();
-    for (const std::size_t index : neighbors_function(parent.index)) {
-      if (!visited[index]) {
-        visited[index] = true;
-        distance_map[index] = distance_function(parent.nearest_obstacle_index, index);
-        queue.push(IndexPair{parent.nearest_obstacle_index, index});
+    for (auto [index, is_unknown] : ranges::views::enumerate(uknown_map)) {
+      if (is_unknown) {
+        distance_map[index] = unknown_distance_map;
       }
     }
   }
 
-  for (auto [index, enum_data] : ranges::views::enumerate(obstacle_map)) {
-    if (enum_data == beluga::GridStatus::kUnknown) {
-      distance_map[index] = unknown_distance_map;
-    }
-  }
-
   return distance_map;
 }
 

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

@@ -34,9 +34,6 @@
 
 namespace beluga {
 
-/// General value traits
-enum class GridStatus : std::uint8_t { kFree, kUnknown, kOccupied };
-
 /**
  * \page OccupancyGrid2Page Beluga named requirements: OccupancyGrid2d
  *
@@ -185,10 +182,11 @@ class BaseOccupancyGrid2 : public BaseLinearGrid2<Derived> {
            });
   }
 
-  [[nodiscard]] auto unknown_obstacle_data() const {
+  /// Retrieves grid data using true booleans for unknowns.
+  [[nodiscard]] auto unknown_data() const {
     return this->self().data() |
            ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) {
-             return value_traits.get_status(value);
+             return value_traits.is_unknown(value);
            });
   }
 };

beluga/include/beluga/sensor/likelihood_field_model.hpp

@@ -58,11 +58,6 @@ struct LikelihoodFieldModelParam {
    * Used to calculate the probability of the obstacle being hit.
    */
   double sigma_hit = 0.2;
-  /// Pre-computed distance map for unkwnown spaces
-  double two_squared_sigma = 2 * sigma_hit * sigma_hit;
-  double amplitude = z_hit / (sigma_hit * std::sqrt(2 * Sophus::Constants<double>::pi()));
-  double offset = z_random / max_laser_distance;
-  double unknown_distance_map = -two_squared_sigma * std::log((1 / max_laser_distance - offset) / z_hit / amplitude);
 };
 
 /// Likelihood field sensor model for range finders.
@@ -100,11 +95,13 @@ 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)
+  explicit LikelihoodFieldModel(const param_type& params, const map_type& grid, const bool& explore_unknown = false)
       : params_{params},
-        likelihood_field_{make_likelihood_field(params, grid)},
-        world_to_likelihood_field_transform_{grid.origin().inverse()} {}
+        likelihood_field_{make_likelihood_field(params, grid, explore_unknown)},
+        world_to_likelihood_field_transform_{grid.origin().inverse()},
+        explore_unknown_{explore_unknown} {}
 
   /// Returns the likelihood field, constructed from the provided map.
   [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_; }
@@ -149,31 +146,42 @@ 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);
+    likelihood_field_ = make_likelihood_field(params_, grid, explore_unknown_);
     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) {
+  static ValueGrid2<double> make_likelihood_field(
+      const LikelihoodFieldModelParam& params,
+      const OccupancyGrid& grid,
+      const bool& explore_unknown = false) {
     const auto squared_distance = [&grid,
                                    squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance](
                                       std::size_t first, std::size_t second) {
       return std::min((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm(), squared_max_distance);
     };
 
+    /// Pre-computed variables
+    const double two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit;
+    const double amplitude = params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants<double>::pi()));
+    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_obstacle_unknown_distance_map(
-        grid.unknown_obstacle_data(), squared_distance, neighborhood,
-        params.unknown_distance_map);  // nearest_obstacle_distance_map(grid.obstacle_data(), squared_distance,
-                                       // neighborhood);
+    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);
 
-    const auto to_likelihood = [amplitude = params.amplitude, two_squared_sigma = params.two_squared_sigma,
-                                offset = params.offset](double squared_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);
       return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;

beluga/test/beluga/include/beluga/test/static_occupancy_grid.hpp

@@ -31,15 +31,6 @@ class StaticOccupancyGrid : public BaseOccupancyGrid2<StaticOccupancyGrid<Rows,
     [[nodiscard]] bool is_free(bool value) const { return !value; }
     [[nodiscard]] bool is_unknown(bool) const { return false; }
     [[nodiscard]] bool is_occupied(bool value) const { return value; }
-    [[nodiscard]] beluga::GridStatus get_status(bool value) const {
-      if (is_unknown(value)) {
-        return beluga::GridStatus::kUnknown;
-      }
-      if (is_occupied(value)) {
-        return beluga::GridStatus::kOccupied;
-      }
-      return beluga::GridStatus::kFree;
-    }
   };
 
   explicit StaticOccupancyGrid(

beluga_ros/include/beluga_ros/occupancy_grid.hpp

@@ -66,16 +66,6 @@ class OccupancyGrid : public beluga::BaseOccupancyGrid2<OccupancyGrid> {
 
     /// Check if the given `value` corresponds to that of an occupied cell.
     [[nodiscard]] static bool is_occupied(std::int8_t value) { return value == kOccupiedValue; }
-
-    [[nodiscard]] static beluga::GridStatus get_status(std::int8_t value) {
-      if (is_unknown(value)) {
-        return beluga::GridStatus::kUnknown;
-      }
-      if (is_occupied(value)) {
-        return beluga::GridStatus::kOccupied;
-      }
-      return beluga::GridStatus::kFree;
-    }
   };
 
   /// Constructor.