diff --git a/beluga/include/beluga/algorithm/distance_map.hpp b/beluga/include/beluga/algorithm/distance_map.hpp index 6b15ac0f4..b2323ee99 100644 --- a/beluga/include/beluga/algorithm/distance_map.hpp +++ b/beluga/include/beluga/algorithm/distance_map.hpp @@ -41,7 +41,7 @@ namespace beluga { * (std::size_t) -> NeighborsT, where NeighborsT is a * [Range](https://en.cppreference.com/w/cpp/ranges/range) * with value type std::size_t. - * \param obstacle_map A map that represents obstacles in an environment. + * \param obstacle_mask A mask that represents obstacles in an environment. * If the value of a cell is True, the cell has an obstacle. * \param distance_function Given the indexes of two cells in the map i and j, * obstacle_map(i, j) must return the distance between the two cells. @@ -52,7 +52,7 @@ namespace beluga { */ template auto nearest_obstacle_distance_map( - Range&& obstacle_map, + Range&& obstacle_mask, DistanceFunction&& distance_function, NeighborsFunction&& neighbors_function) { struct IndexPair { @@ -61,15 +61,15 @@ auto nearest_obstacle_distance_map( }; using DistanceType = std::invoke_result_t; - auto distance_map = std::vector(ranges::size(obstacle_map)); - auto visited = std::vector(ranges::size(obstacle_map), false); + auto distance_map = std::vector(ranges::size(obstacle_mask)); + auto visited = std::vector(ranges::size(obstacle_mask), 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, decltype(compare)>{compare}; - for (auto [index, is_obstacle] : ranges::views::enumerate(obstacle_map)) { + for (auto [index, is_obstacle] : ranges::views::enumerate(obstacle_mask)) { if (is_obstacle) { visited[index] = true; distance_map[index] = 0; @@ -92,31 +92,6 @@ auto nearest_obstacle_distance_map( return distance_map; } -/// A distance map function considering unknown values. -template -auto nearest_distance_map( - const OccupancyGrid& grid, - DistanceFunction&& distance_function, - NeighborsFunction&& neighbors_function, - const double& unknown_distance_map, - const bool& explore_unknown) { - auto obstacle_map = grid.obstacle_data(); - - auto distance_map = nearest_obstacle_distance_map(obstacle_map, distance_function, neighbors_function); - - if (explore_unknown) { - auto uknown_map = grid.unknown_data(); - - for (auto [index, is_unknown] : ranges::views::enumerate(uknown_map)) { - if (is_unknown) { - distance_map[index] = unknown_distance_map; - } - } - } - - return distance_map; -} - } // namespace beluga #endif diff --git a/beluga/include/beluga/sensor/data/occupancy_grid.hpp b/beluga/include/beluga/sensor/data/occupancy_grid.hpp index 74d1b8b8d..526574f86 100644 --- a/beluga/include/beluga/sensor/data/occupancy_grid.hpp +++ b/beluga/include/beluga/sensor/data/occupancy_grid.hpp @@ -68,7 +68,8 @@ namespace beluga { * `g.coordinates_for(r, f)` returns a range of embedding space coordinates in the * corresponding frame as `Eigen::Vector2d` values; * - `g.free_cells()` returns a range of `std::size_t` indices to free grid cells; - * - `g.obstacle_data()` returns a range of `bool` values, representing grid cell occupancy; + * - `g.obstacle_mask()` returns a range of `bool` values, representing grid cell occupancy; + * - `g.unknown_mask()` returns a range of `bool` values, representing the unkwnown space of the grid cells; */ /// Occupancy 2D grid base type. @@ -174,16 +175,16 @@ class BaseOccupancyGrid2 : public BaseLinearGrid2 { ranges::views::transform([](const auto& tuple) { return std::get<0>(tuple); }); } - /// Retrieves grid data using true booleans for obstacles. - [[nodiscard]] auto obstacle_data() const { + /// Retrieves a mask of true booleans to focus on obstacles data in the grid. + [[nodiscard]] auto obstacle_mask() const { return this->self().data() | ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) { return value_traits.is_occupied(value); }); } - /// Retrieves grid data using true booleans for unknowns. - [[nodiscard]] auto unknown_data() const { + /// Retrieves a mask of true booleans to focus on unkwnown data in the grid. + [[nodiscard]] auto unknown_mask() const { return this->self().data() | ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) { return value_traits.is_unknown(value); diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp index dd926d704..af9616eb9 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -58,6 +59,8 @@ struct LikelihoodFieldModelParam { * Used to calculate the probability of the obstacle being hit. */ double sigma_hit = 0.2; + /// Bool parameter to decide if unknown spaces should be explored + bool model_unknown_space = false; }; /// Likelihood field sensor model for range finders. @@ -95,13 +98,11 @@ 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, const bool& explore_unknown = false) + explicit LikelihoodFieldModel(const param_type& params, const map_type& grid) : params_{params}, - likelihood_field_{make_likelihood_field(params, grid, explore_unknown)}, - world_to_likelihood_field_transform_{grid.origin().inverse()}, - explore_unknown_{explore_unknown} {} + likelihood_field_{make_likelihood_field(params, grid)}, + world_to_likelihood_field_transform_{grid.origin().inverse()} {} /// Returns the likelihood field, constructed from the provided map. [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_; } @@ -146,7 +147,7 @@ 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, explore_unknown_); + likelihood_field_ = make_likelihood_field(params_, grid); world_to_likelihood_field_transform_ = grid.origin().inverse(); } @@ -154,12 +155,8 @@ class LikelihoodFieldModel { param_type params_; ValueGrid2 likelihood_field_; Sophus::SE2d world_to_likelihood_field_transform_; - bool explore_unknown_; - static ValueGrid2 make_likelihood_field( - const LikelihoodFieldModelParam& params, - const OccupancyGrid& grid, - const bool& explore_unknown = false) { + static ValueGrid2 make_likelihood_field(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid) { const auto squared_distance = [&grid, squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance]( std::size_t first, std::size_t second) { @@ -168,22 +165,25 @@ class LikelihoodFieldModel { /// Pre-computed variables const double two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit; + assert(two_squared_sigma > 0.0); + const double amplitude = params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi())); + assert(amplitude > 0.0); + 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_distance_map( - grid, squared_distance, neighborhood, unknown_distance_map, - explore_unknown); // nearest_obstacle_distance_map(grid.obstacle_data(), squared_distance, - // neighborhood); + auto distance_map = nearest_obstacle_distance_map(grid.obstacle_mask(), squared_distance, neighborhood); + + 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); + + distance_map |= beluga::actions::overlay(grid.unknown_mask(), background_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); + const auto to_likelihood = [=](double squared_distance) { return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset; }; diff --git a/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp b/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp index 5bc272b77..6d8e9eeb5 100644 --- a/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp +++ b/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp @@ -87,7 +87,7 @@ TEST(OccupancyGrid2, ObstacleData) { #pragma GCC diagnostic ignored "-Warray-bounds" #pragma GCC diagnostic ignored "-Wstringop-overflow" // Data and obstacle data are equivalent in this case - ASSERT_THAT(grid.obstacle_data() | ranges::to, testing::Pointwise(testing::Eq(), grid.data())); + ASSERT_THAT(grid.obstacle_mask() | ranges::to, testing::Pointwise(testing::Eq(), grid.data())); #pragma GCC diagnostic pop }