Addressing first pass

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

beluga/include/beluga/algorithm/distance_map.hpp

@@ -18,6 +18,7 @@
 #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>
@@ -113,14 +114,11 @@ auto nearest_obstacle_unknown_distance_map(
   };
   auto queue = std::priority_queue<IndexPair, std::vector<IndexPair>, decltype(compare)>{compare};
 
-  for (auto [index, tuple_data] : ranges::views::enumerate(obstacle_map)) {
-    if (std::get<0>(tuple_data)) {
+  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
-    } else if (std::get<1>(tuple_data)) {
-      visited[index] = true;
-      distance_map[index] = unknown_distance_map;
     }
   }
 
@@ -136,6 +134,12 @@ auto nearest_obstacle_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,6 +34,9 @@
 
 namespace beluga {
 
+/// General value traits
+enum class GridStatus : std::uint8_t { kFree, kUnknown, kOccupied };
+
 /**
  * \page OccupancyGrid2Page Beluga named requirements: OccupancyGrid2d
  *
@@ -184,9 +187,8 @@ class BaseOccupancyGrid2 : public BaseLinearGrid2<Derived> {
 
   [[nodiscard]] auto unknown_obstacle_data() const {
     return this->self().data() |
-           ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) {
-             return std::make_tuple(value_traits.is_occupied(value), value_traits.is_unknown(value));
-           });
+           ranges::views::transform(
+               [value_traits = this->self().value_traits()](const auto& value) { return value_traits.status(value); });
   }
 };
 

beluga/include/beluga/sensor/likelihood_field_model.hpp

@@ -59,9 +59,10 @@ struct LikelihoodFieldModelParam {
    */
   double sigma_hit = 0.2;
   /// Pre-computed distance map for unkwnown spaces
-  double unknown_distance_map =
-      -2 * sigma_hit * sigma_hit *
-      std::log(std::sqrt(2 * Sophus::Constants<double>::pi()) * (1 - z_random) / max_laser_distance / z_hit);
+  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.
@@ -171,10 +172,8 @@ class LikelihoodFieldModel {
         params.unknown_distance_map);  // nearest_obstacle_distance_map(grid.obstacle_data(), squared_distance,
                                        // neighborhood);
 
-    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) {
+    const auto to_likelihood = [amplitude = params.amplitude, two_squared_sigma = params.two_squared_sigma,
+                                offset = params.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_ros/include/beluga_ros/occupancy_grid.hpp

@@ -66,6 +66,16 @@ 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 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.