First draft to address #55

Signed-off-by: Diego Palma <[email protected]>

beluga/include/beluga/algorithm/distance_map.hpp

@@ -92,6 +92,53 @@ auto nearest_obstacle_distance_map(
   return 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,
+    DistanceFunction&& distance_function,
+    NeighborsFunction&& neighbors_function,
+    bool unknown_distance_map) {
+  struct IndexPair {
+    std::size_t nearest_obstacle_index;
+    std::size_t index;
+  };
+
+  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 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, tuple_data] : ranges::views::enumerate(obstacle_map)) {
+    if (std::get<0>(tuple_data)) {
+      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;
+    }
+  }
+
+  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});
+      }
+    }
+  }
+
+  return distance_map;
+}
+
 }  // namespace beluga
 
 #endif

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

@@ -181,6 +181,13 @@ class BaseOccupancyGrid2 : public BaseLinearGrid2<Derived> {
              return value_traits.is_occupied(value);
            });
   }
+
+  [[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));
+           });
+  }
 };
 
 }  // namespace beluga

beluga/include/beluga/sensor/likelihood_field_model.hpp

@@ -58,6 +58,10 @@ struct LikelihoodFieldModelParam {
    * Used to calculate the probability of the obstacle being hit.
    */
   double sigma_hit = 0.2;
+  /// Pre-computed unknown distance map
+  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);
 };
 
 /// Likelihood field sensor model for range finders.
@@ -162,7 +166,10 @@ class LikelihoodFieldModel {
 
     const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); };
 
-    const auto distance_map = nearest_obstacle_distance_map(grid.obstacle_data(), squared_distance, neighborhood);
+    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 to_likelihood = [amplitude =
                                     params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants<double>::pi())),