Fix performance issue in likelihood map creation

Signed-off-by: Gerardo Puga <glpuga@gmail.com>
Ekumen-OS · Feb 12, 2025 · bcbfb92 · bcbfb92
1 parent b6d5ee3
commit bcbfb92
Show file tree

Hide file tree

Showing 4 changed files with 59 additions and 43 deletions.
diff --git a/beluga/include/beluga/algorithm/distance_map.hpp b/beluga/include/beluga/algorithm/distance_map.hpp
@@ -48,20 +48,22 @@ namespace beluga {
  * \param neighbors_function Given the index i of one cell in the map,
  *  neighbors_function(i) returns the cell indexes of neighbor cells in the
  *  obstacle map.
+ * \param max_distance_value The maximum distance anywhere in the map. Values over this will be truncated to this value.
  * \return A map where each cell value is the distance to the nearest object.
  */
 template <class Range, class DistanceFunction, class NeighborsFunction>
 auto nearest_obstacle_distance_map(
     Range&& obstacle_mask,
     DistanceFunction&& distance_function,
-    NeighborsFunction&& neighbors_function) {
+    NeighborsFunction&& neighbors_function,
+    std::invoke_result_t<DistanceFunction, std::size_t, std::size_t> max_distance_value) {
   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_mask));
+  auto distance_map = std::vector<DistanceType>(ranges::size(obstacle_mask), max_distance_value);
   auto visited = std::vector<bool>(ranges::size(obstacle_mask), false);
 
   auto compare = [&distance_map](const IndexPair& first, const IndexPair& second) {
@@ -83,8 +85,11 @@ auto nearest_obstacle_distance_map(
     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});
+        const auto distance = distance_function(parent.nearest_obstacle_index, index);
+        if (distance < max_distance_value) {
+          distance_map[index] = distance;
+          queue.push(IndexPair{parent.nearest_obstacle_index, index});
+        }
       }
     }
   }

diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp
@@ -160,11 +160,6 @@ class LikelihoodFieldModel {
       return static_cast<float>((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm());
     };
 
-    const auto squared_max_distance = static_cast<float>(params.max_obstacle_distance * params.max_obstacle_distance);
-    const auto truncate_to_max_distance = [squared_max_distance](auto squared_distance) {
-      return std::min(squared_distance, squared_max_distance);
-    };
-
     /// Pre-computed variables
     const double two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit;
     assert(two_squared_sigma > 0.0);
@@ -180,19 +175,23 @@ class LikelihoodFieldModel {
 
     const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); };
 
+    const auto squared_max_distance = static_cast<float>(params.max_obstacle_distance * params.max_obstacle_distance);
+
     // determine distances to obstacles and calculate likelihood values in-place
     // to minimize memory usage when dealing with large maps
-    auto distance_map = nearest_obstacle_distance_map(grid.obstacle_mask(), squared_distance, neighborhood);
+    auto distance_map = nearest_obstacle_distance_map(
+        grid.obstacle_mask(), squared_distance, neighborhood, static_cast<float>(squared_max_distance));
 
     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);
+      const auto inverse_max_distance = 1 / params.max_laser_distance;
+      const auto squared_background_distance =
+          -two_squared_sigma * std::log((inverse_max_distance - offset) / amplitude);
 
-      distance_map |= beluga::actions::overlay(grid.unknown_mask(), background_distance);
+      distance_map |= beluga::actions::overlay(
+          grid.unknown_mask(), std::min(squared_max_distance, static_cast<float>(squared_background_distance)));
     }
 
-    auto likelihood_values = std::move(distance_map) |                               //
-                             ranges::actions::transform(truncate_to_max_distance) |  //
+    auto likelihood_values = std::move(distance_map) |  //
                              ranges::actions::transform(to_likelihood);
 
     return ValueGrid2<float>{std::move(likelihood_values), grid.width(), grid.resolution()};

diff --git a/beluga/test/beluga/algorithm/test_distance_map.cpp b/beluga/test/beluga/algorithm/test_distance_map.cpp
@@ -44,38 +44,44 @@ auto make_neighbors_function(std::size_t size) {
 
 TEST(DistanceMap, None) {
   auto map = std::vector<bool>{};
-  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6), 1);
   ASSERT_EQ(distance_map.size(), 0);
 }
 
-TEST(DistanceMap, Empty) {
+TEST(DistanceMap, EmptyNonZeroDistanceMin) {
   auto map = std::array<bool, 6>{false, false, false, false, false, false};
-  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
-  ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0));
+  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6), 10);
+  ASSERT_THAT(distance_map, testing::ElementsAre(10, 10, 10, 10, 10, 10));
 }
 
 TEST(DistanceMap, Full) {
   auto map = std::array<bool, 6>{true, true, true, true, true, true};
-  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6), 10);
   ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0));
 }
 
 TEST(DistanceMap, Case1) {
   auto map = std::array<bool, 6>{false, true, false, false, false, true};
-  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6), 10);
   ASSERT_THAT(distance_map, testing::ElementsAre(1, 0, 1, 2, 1, 0));
 }
 
 TEST(DistanceMap, Case2) {
   auto map = std::array<bool, 6>{true, true, false, false, false, false};
-  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6), 10);
   ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 1, 2, 3, 4));
 }
 
 TEST(DistanceMap, Case3) {
   auto map = std::array<bool, 6>{false, false, false, false, false, true};
-  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6), 10);
   ASSERT_THAT(distance_map, testing::ElementsAre(5, 4, 3, 2, 1, 0));
 }
 
+TEST(DistanceMap, MinDistaceSet) {
+  auto map = std::array<bool, 6>{false, false, false, false, false, true};
+  auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6), 3);
+  ASSERT_THAT(distance_map, testing::ElementsAre(3, 3, 3, 2, 1, 0));
+}
+
 }  // namespace
diff --git a/beluga/test/beluga/sensor/test_lfm_with_unknown_space.cpp b/beluga/test/beluga/sensor/test_lfm_with_unknown_space.cpp
@@ -280,15 +280,17 @@ TEST(LikelihoodFieldModelUnknownSpace, GridUpdates) {
   const auto origin = Sophus::SE2d{};
 
   constexpr double kResolution = 0.5;
-  // clang-format off
-  auto grid = StaticOccupancyGrid<5, 5, std::int8_t>{{
-    -1, -1, -1, 0, 0,
-    -1, 0 , 0 , 0, 0,
-    -1, 0 , 0 , 0, 0,
-    0 , 0 , 0 , 0, 0,
-    0 , 0 , 0 , 0, 0},
-    kResolution, origin};
-  // clang-format on
+
+  auto grid = StaticOccupancyGrid<5, 5, std::int8_t>{
+      {
+          -1, -1, -1, +0, +0,  //
+          -1, +0, +0, +0, +0,  //
+          -1, +0, +0, +0, +0,  //
+          +0, +0, +0, +0, +0,  //
+          +0, +0, +0, +0, +0,  //
+      },
+      kResolution,
+      origin};
 
   constexpr auto kMaxObstacleDistance = 2.0;
   constexpr auto kParamMaxLaserDistance = 20.0;
@@ -303,23 +305,27 @@ TEST(LikelihoodFieldModelUnknownSpace, GridUpdates) {
 
   {
     auto state_weighting_function = sensor_model(std::vector<std::pair<double, double>>{{1., 1.}});
-    EXPECT_NEAR(2.068577607986223, state_weighting_function(origin), 1e-6);
+    const auto expected_sensor_return_value = std::pow(0.025, 3.0) + 1.0;
+    EXPECT_NEAR(expected_sensor_return_value, state_weighting_function(origin), 1e-6);
   }
 
-  // clang-format off
-  grid = StaticOccupancyGrid<5, 5, std::int8_t>{{
-    -1, -1, -1, 0, 0  ,
-    -1, 0 , 0 , 0, 0  ,
-    -1, 0 , 0 , 0, 0  ,
-    0 , 0 , 0 , 0, 0  ,
-    0 , 0 , 0 , 0, 100},
-    kResolution, origin};
-  // clang-format on
+  grid = StaticOccupancyGrid<5, 5, std::int8_t>{
+      {
+          -1, -1, -1, +0, +0,    //
+          -1, +0, +0, +0, +0,    //
+          -1, +0, +0, +0, +0,    //
+          +0, +0, +0, +0, +0,    //
+          +0, +0, +0, +0, +100,  //
+      },
+      kResolution,
+      origin};
+
   sensor_model.update_map(std::move(grid));
 
   {
     auto state_weighting_function = sensor_model(std::vector<std::pair<double, double>>{{1., 1.}});
-    EXPECT_NEAR(1.0, state_weighting_function(origin), 1e-3);
+    const auto expected_sensor_return_value = std::pow(0.025, 3.0) + 1.0;
+    EXPECT_NEAR(expected_sensor_return_value, state_weighting_function(origin), 1e-6);
   }
 }