From bcbfb9285cfeaf2a74fc45ff67fda901f390ba89 Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Mon, 10 Feb 2025 11:37:14 -0300 Subject: [PATCH] Fix performance issue in likelihood map creation Signed-off-by: Gerardo Puga --- .../include/beluga/algorithm/distance_map.hpp | 13 ++++-- .../beluga/sensor/likelihood_field_model.hpp | 21 ++++----- .../beluga/algorithm/test_distance_map.cpp | 22 +++++---- .../sensor/test_lfm_with_unknown_space.cpp | 46 +++++++++++-------- 4 files changed, 59 insertions(+), 43 deletions(-) diff --git a/beluga/include/beluga/algorithm/distance_map.hpp b/beluga/include/beluga/algorithm/distance_map.hpp index b2323ee99..b6d348d06 100644 --- 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 auto nearest_obstacle_distance_map( Range&& obstacle_mask, DistanceFunction&& distance_function, - NeighborsFunction&& neighbors_function) { + NeighborsFunction&& neighbors_function, + std::invoke_result_t max_distance_value) { struct IndexPair { std::size_t nearest_obstacle_index; std::size_t index; }; using DistanceType = std::invoke_result_t; - auto distance_map = std::vector(ranges::size(obstacle_mask)); + auto distance_map = std::vector(ranges::size(obstacle_mask), max_distance_value); auto visited = std::vector(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 index 87c748149..da096198a 100644 --- 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((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm()); }; - const auto squared_max_distance = static_cast(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(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(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(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{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 index ea03e1d4a..304e21557 100644 --- 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{}; - 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{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{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{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{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{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{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 index 9daa52db5..ddc1d93d9 100644 --- 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>{{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>{{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); } }