From 64026d92fbbb40731200b1cb6069b64fde361fbd Mon Sep 17 00:00:00 2001 From: Diego Palma <69125793+DPR00@users.noreply.github.com> Date: Sat, 5 Oct 2024 14:05:32 -0500 Subject: [PATCH] Extending likelihood field to model unexplored spaces (#430) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ### Proposed changes The following changes are proposed in order to enhance the Likelihood Field Model based on the suggestions described on #55: - First, a new function `unknown_obstacle_data()` is created to ideally replace the `obstacle_data()` function in `beluga/sensor/data/OccupancyGrid.hpp`. This new function will return a `srd::tuple` containing the values which are occupied and unknown. - Then, this new function will be passed to calculate the distance map using a new function `nearest_obstacle_unknown_distance_map`. This new function will ideally replace the `nearest_obstacle_distance_map` function. - The value of the distance map for unknown values is assigned with a pre-computed value inside the `nearest_obstacle_unknown_distance_map` function. Based on this computed value, the likelihood will be $\frac{1}{z_{max}}$. Some comments: - It passed the compilation steps and the beluga example was tested. It would be nice to make some maps testing the draft. How could I perform that? #### Type of change - [ ] 🐛 Bugfix (change which fixes an issue) - [x] 🚀 Feature (change which adds functionality) - [ ] 📚 Documentation (change which fixes or extends documentation) 💥 **Breaking change!** _Explain why a non-backwards compatible change is necessary or remove this line entirely if not applicable._ ### Checklist _Put an `x` in the boxes that apply. This is simply a reminder of what we will require before merging your code._ - [x] Lint and unit tests (if any) pass locally with my changes - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added necessary documentation (if appropriate) - [x] All commits have been signed for [DCO](https://developercertificate.org/) ### Additional comments --------- Signed-off-by: Diego Palma Co-authored-by: Diego Palma --- beluga/include/beluga/actions/overlay.hpp | 134 +++++++ .../include/beluga/algorithm/distance_map.hpp | 10 +- .../beluga/sensor/data/occupancy_grid.hpp | 15 +- .../beluga/sensor/likelihood_field_model.hpp | 36 +- beluga/test/beluga/CMakeLists.txt | 2 + beluga/test/beluga/actions/test_overlay.cpp | 146 ++++++++ .../beluga/test/static_occupancy_grid.hpp | 37 +- .../sensor/data/test_occupancy_grid.cpp | 17 +- .../sensor/test_lfm_with_unknown_space.cpp | 326 ++++++++++++++++++ .../test/benchmark/benchmark_raycasting.cpp | 20 +- 10 files changed, 699 insertions(+), 44 deletions(-) create mode 100644 beluga/include/beluga/actions/overlay.hpp create mode 100644 beluga/test/beluga/actions/test_overlay.cpp create mode 100644 beluga/test/beluga/sensor/test_lfm_with_unknown_space.cpp diff --git a/beluga/include/beluga/actions/overlay.hpp b/beluga/include/beluga/actions/overlay.hpp new file mode 100644 index 000000000..45574d6af --- /dev/null +++ b/beluga/include/beluga/actions/overlay.hpp @@ -0,0 +1,134 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ACTIONS_OVERLAY_HPP +#define BELUGA_ACTIONS_OVERLAY_HPP + +#include +#include + +#include +#include +#include + +/** + * \file + * \brief Implementation of the overlay range adaptor object + */ +namespace beluga::actions { + +namespace detail { + +/// Implementation detail for an overlay range adaptor object. +struct overlay_base_fn { + /// Overload that implements an overlay of a value in a range. + /** + * \tparam ExecutionPolicy An [execution policy](https://en.cppreference.com/w/cpp/algorithm/execution_policy_tag_t). + * \tparam Range An [input range](https://en.cppreference.com/w/cpp/ranges/input_range). + * \tparam MaskRange An [input range](https://en.cppreference.com/w/cpp/ranges/input_range). + * \param policy The execution policy to use. + * \param range An existing range to apply this action to. + * \param mask The mask where the values will be overlaid. + * \param mask_value The value to be overlaid. + */ + template < + class ExecutionPolicy, + class Range, + class MaskRange, + class Mask, + std::enable_if_t>, int> = 0, + std::enable_if_t, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(ExecutionPolicy&& policy, Range& range, MaskRange&& mask, Mask&& mask_value) const + -> Range& { + auto map = range | ranges::views::common; + const auto converted_mask_value = static_cast>(mask_value); + + std::transform( + policy, // + std::begin(map), // + std::end(map), // + std::begin(mask), // + std::begin(map), // + [&converted_mask_value](const auto& base_value, bool flag) { + return flag ? converted_mask_value : base_value; + }); + + return range; + } + + /// Overload that re-orders arguments from an action closure. + template < + class ExecutionPolicy, + class Range, + class MaskRange, + class Mask, + std::enable_if_t, int> = 0, + std::enable_if_t, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(Range&& range, MaskRange&& mask, Mask&& mask_value, ExecutionPolicy policy) const + -> Range& { + return (*this)( + std::move(policy), std::forward(range), std::forward(mask), std::forward(mask_value)); + } + + /// Overload that returns an action closure to compose with other actions. + template < + class ExecutionPolicy, + class MaskRange, + class Mask, + std::enable_if_t, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(ExecutionPolicy policy, MaskRange&& mask, Mask&& mask_value) const { + return ranges::make_action_closure(ranges::bind_back( + overlay_base_fn{}, std::forward(mask), std::forward(mask_value), std::move(policy))); + } +}; + +/// Implementation detail for an overlay range adaptor object with a default execution policy. +struct overlay_fn : public overlay_base_fn { + using overlay_base_fn::operator(); + + /// Overload that defines a default execution policy. + template < + class Range, + class MaskRange, + class Mask, + std::enable_if_t, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(Range&& range, MaskRange&& mask, Mask&& mask_value) const -> Range& { + return (*this)( + std::execution::seq, std::forward(range), std::forward(mask), std::forward(mask_value)); + } + + /// Overload that returns an action closure to compose with other actions. + template , int> = 0> + constexpr auto operator()(MaskRange&& mask, Mask&& mask_value) const { + return ranges::make_action_closure( + ranges::bind_back(overlay_fn{}, std::forward(mask), std::forward(mask_value))); + } +}; + +} // namespace detail + +/// [Range adaptor object](https://en.cppreference.com/w/cpp/named_req/RangeAdaptorObject) that +/// can overlay a range of values (or a range of particles). +/** + * The `overlay` range adaptor allows to overlay the values of the range that match a mask. + * All the values are overlaid for a given value. + */ +inline constexpr ranges::actions::action_closure overlay; +} // namespace beluga::actions + +#endif diff --git a/beluga/include/beluga/algorithm/distance_map.hpp b/beluga/include/beluga/algorithm/distance_map.hpp index 544558a35..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; diff --git a/beluga/include/beluga/sensor/data/occupancy_grid.hpp b/beluga/include/beluga/sensor/data/occupancy_grid.hpp index 8150caffa..e4f7a71c3 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,13 +175,21 @@ 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 over occupied cells 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 a mask over unknown cells 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); + }); + } }; } // namespace beluga diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp index dd7276a32..87c748149 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model.hpp @@ -20,9 +20,11 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -58,6 +60,8 @@ struct LikelihoodFieldModelParam { * Used to calculate the probability of the obstacle being hit. */ double sigma_hit = 0.2; + /// Whether to model unknown space or assume it free. + bool model_unknown_space = false; }; /// Likelihood field sensor model for range finders. @@ -161,12 +165,16 @@ class LikelihoodFieldModel { return std::min(squared_distance, squared_max_distance); }; - const auto to_likelihood = [amplitude = - params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi())), - two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit, - offset = params.z_random / params.max_laser_distance](double squared_distance) { - assert(two_squared_sigma > 0.0); - assert(amplitude > 0.0); + /// 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 auto to_likelihood = [amplitude, two_squared_sigma, offset](double squared_distance) { return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset; }; @@ -174,10 +182,18 @@ class LikelihoodFieldModel { // determine distances to obstacles and calculate likelihood values in-place // to minimize memory usage when dealing with large maps - auto likelihood_values = nearest_obstacle_distance_map(grid.obstacle_data(), squared_distance, neighborhood); - std::transform( - likelihood_values.begin(), likelihood_values.end(), likelihood_values.begin(), truncate_to_max_distance); - std::transform(likelihood_values.begin(), likelihood_values.end(), likelihood_values.begin(), to_likelihood); + 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); + } + + auto likelihood_values = std::move(distance_map) | // + ranges::actions::transform(truncate_to_max_distance) | // + ranges::actions::transform(to_likelihood); return ValueGrid2{std::move(likelihood_values), grid.width(), grid.resolution()}; } diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 237fb2ecb..ece4b8a69 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -18,6 +18,7 @@ add_executable( test_beluga actions/test_assign.cpp actions/test_normalize.cpp + actions/test_overlay.cpp actions/test_propagate.cpp actions/test_reweight.cpp algorithm/raycasting/test_bresenham.cpp @@ -51,6 +52,7 @@ add_executable( sensor/test_beam_model.cpp sensor/test_bearing_sensor_model.cpp sensor/test_landmark_sensor_model.cpp + sensor/test_lfm_with_unknown_space.cpp sensor/test_likelihood_field_model.cpp sensor/test_ndt_model.cpp test_3d_embedding.cpp diff --git a/beluga/test/beluga/actions/test_overlay.cpp b/beluga/test/beluga/actions/test_overlay.cpp new file mode 100644 index 000000000..7a91c40cf --- /dev/null +++ b/beluga/test/beluga/actions/test_overlay.cpp @@ -0,0 +1,146 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include +#include + +#include "beluga/actions/overlay.hpp" +#include "beluga/primitives.hpp" + +namespace { + +TEST(OverlayAction, DefaultExecutionPolicyAndBoolInput) { + auto input = std::vector{false, false, false, false, false, false, false, false, false}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const bool mask_value = true; + + input |= beluga::actions::overlay(mask, mask_value); + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), mask)); +} + +TEST(OverlayAction, DefaultExecutionPolicyAndIntInput) { + auto input = std::vector{1, 2, 3, 4, 5, 6, 7, 8, 9}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const int mask_value = 5; + + input |= beluga::actions::overlay(mask, mask_value); + + const auto expected_output = std::vector{1, 2, 3, 5, 5, 5, 7, 8, 9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, DefaultExecutionPolicyAndStringInput) { + auto input = std::vector{"b", "e", "l", "u", "g", "a", "c", "p", "p"}; + const auto mask = std::vector{false, false, false, false, false, false, false, true, true}; + const std::string mask_value = "+"; + + input |= beluga::actions::overlay(mask, mask_value); + + const auto expected_output = std::vector{"b", "e", "l", "u", "g", "a", "c", "+", "+"}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, DefaultExecutionPolicyAndDoubleInput) { + auto input = std::vector{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const double mask_value = 5.5; + + input |= beluga::actions::overlay(mask, mask_value); + + const auto expected_output = std::vector{1.1, 2.2, 3.3, 5.5, 5.5, 5.5, 7.7, 8.8, 9.9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, DefaultExecutionPolicyAndAllowConvertible) { + auto input = std::vector{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const int mask_value = 5; + + input |= beluga::actions::overlay(mask, mask_value); + + const auto expected_output = std::vector{1.1, 2.2, 3.3, 5.0, 5.5, 5.0, 7.7, 8.8, 9.9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, DefaultExecutionPolicyWithoutClosure) { + auto input = std::vector{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const double mask_value = 5.5; + + input = beluga::actions::overlay(input, mask, mask_value); + + const auto expected_output = std::vector{1.1, 2.2, 3.3, 5.5, 5.5, 5.5, 7.7, 8.8, 9.9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, SeqExecutionPolicyWithoutClosure) { + auto input = std::vector{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const double mask_value = 5.5; + + input = beluga::actions::overlay(std::execution::seq, input, mask, mask_value); + + const auto expected_output = std::vector{1.1, 2.2, 3.3, 5.5, 5.5, 5.5, 7.7, 8.8, 9.9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, SeqExecutionPolicy) { + auto input = std::vector{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const double mask_value = 5.5; + + input |= beluga::actions::overlay(std::execution::seq, mask, mask_value); + + const auto expected_output = std::vector{1.1, 2.2, 3.3, 5.5, 5.5, 5.5, 7.7, 8.8, 9.9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, ParallelExecutionPolicy) { + auto input = std::vector{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + const auto mask = std::vector{false, false, false, true, false, true, false, false, false}; + const double mask_value = 5.5; + + input |= beluga::actions::overlay(std::execution::par, mask, mask_value); + + const auto expected_output = std::vector{1.1, 2.2, 3.3, 5.5, 5.5, 5.5, 7.7, 8.8, 9.9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +TEST(OverlayAction, Temporary) { + auto input = std::vector{1, 2, 3, 4, 5, 6, 7, 8, 9}; + auto action = beluga::actions::overlay(std::vector{false, false, false, true, false, true, false, false, false}, 5); + + input |= action; + + const auto expected_output = std::vector{1, 2, 3, 5, 5, 5, 7, 8, 9}; + + ASSERT_THAT(input, ::testing::Pointwise(::testing::Eq(), expected_output)); +} + +} // namespace diff --git a/beluga/test/beluga/include/beluga/test/static_occupancy_grid.hpp b/beluga/test/beluga/include/beluga/test/static_occupancy_grid.hpp index fd0ccaec2..dfbb9f5ae 100644 --- a/beluga/test/beluga/include/beluga/test/static_occupancy_grid.hpp +++ b/beluga/test/beluga/include/beluga/test/static_occupancy_grid.hpp @@ -24,17 +24,32 @@ namespace beluga::testing { -template -class StaticOccupancyGrid : public BaseOccupancyGrid2> { - public: - struct ValueTraits { - [[nodiscard]] bool is_free(bool value) const { return !value; } - [[nodiscard]] bool is_unknown(bool) const { return false; } - [[nodiscard]] bool is_occupied(bool value) const { return value; } - }; +template +struct ValueTraits; + +template <> +struct ValueTraits { + [[nodiscard]] static bool is_free(bool value) { return !value; } + [[nodiscard]] static bool is_unknown(bool) { return false; } + [[nodiscard]] static bool is_occupied(bool value) { return value; } +}; + +template <> +struct ValueTraits { + static constexpr std::int8_t kFreeValue = 0; + static constexpr std::int8_t kUnknownValue = -1; + static constexpr std::int8_t kOccupiedValue = 100; + [[nodiscard]] static bool is_free(std::int8_t value) { return value == kFreeValue; } + [[nodiscard]] static bool is_unknown(std::int8_t value) { return value == kUnknownValue; } + [[nodiscard]] static bool is_occupied(std::int8_t value) { return value == kOccupiedValue; } +}; + +template +class StaticOccupancyGrid : public BaseOccupancyGrid2> { + public: explicit StaticOccupancyGrid( - std::array array, + std::array array, double resolution = 1.0, const Sophus::SE2d& origin = Sophus::SE2d{}) : grid_{array}, origin_(origin), resolution_{resolution} {} @@ -49,10 +64,10 @@ class StaticOccupancyGrid : public BaseOccupancyGrid2{}; } private: - std::array grid_; + std::array grid_; Sophus::SE2d origin_; double resolution_; }; diff --git a/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp b/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp index 5bc272b77..85164c1ff 100644 --- a/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp +++ b/beluga/test/beluga/sensor/data/test_occupancy_grid.cpp @@ -79,7 +79,7 @@ TEST(OccupancyGrid2, AllFreeCells) { ASSERT_THAT(grid.free_cells() | ranges::to, testing::Pointwise(testing::Eq(), expected_free_cells)); } -TEST(OccupancyGrid2, ObstacleData) { +TEST(OccupancyGrid2, ObstacleMask) { const auto grid = StaticOccupancyGrid<5, 2>{{false, false, false, false, true, false, false, false, true, false}}; // See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=111118 @@ -87,7 +87,20 @@ 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 +} + +TEST(OccupancyGrid2, UnknownMask) { + const auto grid = StaticOccupancyGrid<5, 2, std::int8_t>{{-1, -1, -1, -1, 0, 0, 0, 0, 100, 100}}; + + const auto expected_unknown_mask = std::vector{true, true, true, true, false, false, false, false, false, false}; +// See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=111118 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" + // Data and unkwnown data are equivalent in this case + ASSERT_THAT(grid.unknown_mask() | ranges::to, testing::Pointwise(testing::Eq(), expected_unknown_mask)); #pragma GCC diagnostic pop } diff --git a/beluga/test/beluga/sensor/test_lfm_with_unknown_space.cpp b/beluga/test/beluga/sensor/test_lfm_with_unknown_space.cpp new file mode 100644 index 000000000..9daa52db5 --- /dev/null +++ b/beluga/test/beluga/sensor/test_lfm_with_unknown_space.cpp @@ -0,0 +1,326 @@ +// Copyright 2022-2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +#include +#include +#include + +#include "beluga/sensor/likelihood_field_model.hpp" +#include "beluga/test/static_occupancy_grid.hpp" + +namespace { + +using beluga::testing::StaticOccupancyGrid; + +using UUT = beluga::LikelihoodFieldModel>; + +TEST(LikelihoodFieldModelUnknownSpace, LikelihoodField) { + constexpr double kResolution = 0.5; + // clang-format off + const auto grid = StaticOccupancyGrid<5, 5, std::int8_t>{{ + -1 , -1 , -1 , 100, 100, + -1 , 0 , 0 , 0 , 100, + -1 , 0 , 0 , 0 , 100, + 100, 0 , 0 , 0 , 100, + 100, 100, 100, 100, 100}, + kResolution}; + // clang-format on + + constexpr auto kMaxObstacleDistance = 2.0; + constexpr auto kParamMaxLaserDistance = 20.0; + constexpr auto kLaserHitProbability = 0.5; + constexpr auto kWightForRandomNoise = 0.5; + constexpr auto kStdForObstaclesBeingHit = 0.2; + constexpr auto kModelUnknownSpace = true; + constexpr auto kUnknownSpaceLikelihood = 1 / kParamMaxLaserDistance; + + auto params = beluga::LikelihoodFieldModelParam{kMaxObstacleDistance, kParamMaxLaserDistance, kLaserHitProbability, + kWightForRandomNoise, kStdForObstaclesBeingHit, kModelUnknownSpace}; + auto sensor_model = UUT{params, grid}; + + // clang-format off + const double expected_likelihood_field[] = { // NOLINT(modernize-avoid-c-arrays) + kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, 1.022, 1.022, + kUnknownSpaceLikelihood, 0.025 , 0.027 , 0.069, 1.022, + kUnknownSpaceLikelihood, 0.027 , 0.025 , 0.069, 1.022, + 1.022 , 0.069 , 0.069 , 0.069, 1.022, + 1.022 , 1.022 , 1.022 , 1.022, 1.022}; + // clang-format on + + ASSERT_THAT( + sensor_model.likelihood_field().data(), + testing::Pointwise(testing::DoubleNear(0.003), expected_likelihood_field)); +} + +TEST(LikelihoodFieldModelUnknownSpace, LikelihoodField2) { + constexpr double kResolution = 0.5; + // clang-format off + const auto grid = StaticOccupancyGrid<5, 5, std::int8_t>{{ + -1, -1, -1, 100, 100, + -1, -1, -1, 0 , 0 , + -1, -1, -1, 0 , 0 , + -1, -1, -1, 0 , 0 , + -1, -1, -1, 100, 100}, + kResolution}; + // clang-format on + + constexpr auto kMaxObstacleDistance = 2.0; + constexpr auto kParamMaxLaserDistance = 100.0; + constexpr auto kLaserHitProbability = 0.5; + constexpr auto kWightForRandomNoise = 0.5; + constexpr auto kStdForObstaclesBeingHit = 0.2; + constexpr auto kModelUnknownSpace = true; + constexpr auto kUnknownSpaceLikelihood = 1 / kParamMaxLaserDistance; + + auto params = beluga::LikelihoodFieldModelParam{kMaxObstacleDistance, kParamMaxLaserDistance, kLaserHitProbability, + kWightForRandomNoise, kStdForObstaclesBeingHit, kModelUnknownSpace}; + auto sensor_model = UUT{params, grid}; + + // clang-format off + const double expected_likelihood_field[] = { // NOLINT(modernize-avoid-c-arrays) + kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, 1.002, 1.002, + kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, 0.049, 0.049, + kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, 0.005, 0.005, + kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, 0.049, 0.049, + kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, kUnknownSpaceLikelihood, 1.002, 1.002, + }; + // clang-format on + + ASSERT_THAT( + sensor_model.likelihood_field().data(), + testing::Pointwise(testing::DoubleNear(0.003), expected_likelihood_field)); +} + +TEST(LikelihoodFieldModelUnknownSpace, ImportanceWeight) { + constexpr double kResolution = 0.5; + // clang-format off + const auto grid = StaticOccupancyGrid<5, 5, std::int8_t>{{ + -1, -1, -1 , 0 , 0 , + -1, 0 , 0 , 0 , 0 , + -1, 0 , 100, 0 , -1, + 0 , 0 , 0 , 0 , -1, + 0 , 0 , -1 , -1, -1}, + kResolution}; + // clang-format on + + constexpr auto kMaxObstacleDistance = 2.0; + constexpr auto kParamMaxLaserDistance = 20.0; + constexpr auto kLaserHitProbability = 0.5; + constexpr auto kWightForRandomNoise = 0.5; + constexpr auto kStdForObstaclesBeingHit = 0.2; + constexpr auto kModelUnknownSpace = true; + constexpr auto kUnknownSpaceLikelihood = 1 / kParamMaxLaserDistance; + + auto params = beluga::LikelihoodFieldModelParam{kMaxObstacleDistance, kParamMaxLaserDistance, kLaserHitProbability, + kWightForRandomNoise, kStdForObstaclesBeingHit, kModelUnknownSpace}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{0, 0}}); + ASSERT_NEAR( + 1.000 + kUnknownSpaceLikelihood * kUnknownSpaceLikelihood * kUnknownSpaceLikelihood, + state_weighting_function(grid.origin()), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{1.25, 1.25}}); + ASSERT_NEAR(2.068, state_weighting_function(grid.origin()), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{2.25, 2.25}}); + ASSERT_NEAR( + 1.000 + kUnknownSpaceLikelihood * kUnknownSpaceLikelihood * kUnknownSpaceLikelihood, + state_weighting_function(grid.origin()), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{-50.0, 50.0}}); + ASSERT_NEAR(1.000, state_weighting_function(grid.origin()), 0.003); + } + + { + auto state_weighting_function = + sensor_model(std::vector>{{1.20, 1.20}, {1.25, 1.25}, {1.30, 1.30}}); + ASSERT_NEAR(4.205, state_weighting_function(grid.origin()), 0.01); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{0.0, 0.0}}); + ASSERT_NEAR(2.068, state_weighting_function(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{1.25, 1.25}}), 0.003); + } +} + +TEST(LikelihoodFieldModelUnknownSpace, GridWithOffset) { + constexpr double kResolution = 2.0; + // clang-format off + const 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, 100}, + kResolution, + Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{-5, -5}}}; + // clang-format on + + constexpr auto kMaxObstacleDistance = 2.0; + constexpr auto kParamMaxLaserDistance = 20.0; + constexpr auto kLaserHitProbability = 0.5; + constexpr auto kWightForRandomNoise = 0.5; + constexpr auto kStdForObstaclesBeingHit = 0.2; + constexpr auto kModelUnknownSpace = true; + + auto params = beluga::LikelihoodFieldModelParam{kMaxObstacleDistance, kParamMaxLaserDistance, kLaserHitProbability, + kWightForRandomNoise, kStdForObstaclesBeingHit, kModelUnknownSpace}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{4.5, 4.5}}); + ASSERT_NEAR(2.068, state_weighting_function(Sophus::SE2d{}), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{9.5, 9.5}}); + ASSERT_NEAR(2.068, state_weighting_function(grid.origin()), 0.003); + } +} + +TEST(LikelihoodFieldModelUnknownSpace, GridWithRotation) { + constexpr double kResolution = 2.0; + // clang-format off + const 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, 100}, + kResolution, + Sophus::SE2d{Sophus::SO2d{Sophus::Constants::pi() / 2}, Eigen::Vector2d{0.0, 0.0}}}; + // clang-format on + + constexpr auto kMaxObstacleDistance = 2.0; + constexpr auto kParamMaxLaserDistance = 20.0; + constexpr auto kLaserHitProbability = 0.5; + constexpr auto kWightForRandomNoise = 0.5; + constexpr auto kStdForObstaclesBeingHit = 0.2; + constexpr auto kModelUnknownSpace = true; + + auto params = beluga::LikelihoodFieldModelParam{kMaxObstacleDistance, kParamMaxLaserDistance, kLaserHitProbability, + kWightForRandomNoise, kStdForObstaclesBeingHit, kModelUnknownSpace}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{-9.5, 9.5}}); + ASSERT_NEAR(2.068, state_weighting_function(Sophus::SE2d{}), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{9.5, 9.5}}); + ASSERT_NEAR(2.068, state_weighting_function(grid.origin()), 0.003); + } +} + +TEST(LikelihoodFieldModelUnknownSpace, GridWithRotationAndOffset) { + constexpr double kResolution = 2.0; + // clang-format off + const auto origin_rotation = Sophus::SO2d{Sophus::Constants::pi() / 2}; + const auto origin = Sophus::SE2d{origin_rotation, origin_rotation * Eigen::Vector2d{-5, -5}}; + + const 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, 100}, + kResolution, + origin}; + // clang-format on + + constexpr auto kMaxObstacleDistance = 2.0; + constexpr auto kParamMaxLaserDistance = 20.0; + constexpr auto kLaserHitProbability = 0.5; + constexpr auto kWightForRandomNoise = 0.5; + constexpr auto kStdForObstaclesBeingHit = 0.2; + constexpr auto kModelUnknownSpace = true; + + auto params = beluga::LikelihoodFieldModelParam{kMaxObstacleDistance, kParamMaxLaserDistance, kLaserHitProbability, + kWightForRandomNoise, kStdForObstaclesBeingHit, kModelUnknownSpace}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{-4.5, 4.5}}); + ASSERT_NEAR(2.068, state_weighting_function(Sophus::SE2d{}), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{9.5, 9.5}}); + ASSERT_NEAR(2.068, state_weighting_function(grid.origin()), 0.003); + } +} + +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 + + constexpr auto kMaxObstacleDistance = 2.0; + constexpr auto kParamMaxLaserDistance = 20.0; + constexpr auto kLaserHitProbability = 0.5; + constexpr auto kWightForRandomNoise = 0.5; + constexpr auto kStdForObstaclesBeingHit = 0.2; + constexpr auto kModelUnknownSpace = true; + + auto params = beluga::LikelihoodFieldModelParam{kMaxObstacleDistance, kParamMaxLaserDistance, kLaserHitProbability, + kWightForRandomNoise, kStdForObstaclesBeingHit, kModelUnknownSpace}; + auto sensor_model = UUT{params, std::move(grid)}; + + { + auto state_weighting_function = sensor_model(std::vector>{{1., 1.}}); + EXPECT_NEAR(2.068577607986223, 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 + 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); + } +} + +} // namespace diff --git a/beluga/test/benchmark/benchmark_raycasting.cpp b/beluga/test/benchmark/benchmark_raycasting.cpp index d2a8266b3..db3d176e5 100644 --- a/beluga/test/benchmark/benchmark_raycasting.cpp +++ b/beluga/test/benchmark/benchmark_raycasting.cpp @@ -56,16 +56,10 @@ BENCHMARK_CAPTURE(BM_Bresenham2i, Modified, beluga::Bresenham2i::kModified) using beluga::testing::StaticOccupancyGrid; -template -class BaselineGrid : public beluga::BaseOccupancyGrid2> { +template +class BaselineGrid : public beluga::BaseOccupancyGrid2> { public: - struct ValueTraits { - [[nodiscard]] bool is_free(bool value) const { return !value; } - [[nodiscard]] bool is_unknown(bool) const { return false; } - [[nodiscard]] bool is_occupied(bool value) const { return value; } - }; - - explicit BaselineGrid(std::initializer_list, double resolution) : resolution_{resolution} { + explicit BaselineGrid(std::initializer_list, double resolution) : resolution_{resolution} { std::fill(std::begin(data()), std::end(data()), false); } @@ -79,12 +73,12 @@ class BaselineGrid : public beluga::BaseOccupancyGrid2> [[nodiscard]] std::size_t height() const { return Rows; } [[nodiscard]] double resolution() const { return resolution_; } - [[nodiscard]] auto value_traits() const { return ValueTraits{}; } + [[nodiscard]] auto value_traits() const { return beluga::testing::ValueTraits{}; } private: double resolution_; Sophus::SE2d origin_; - std::array grid_; + std::array grid_; static constexpr std::size_t kWidth = Cols; static constexpr std::size_t kHeight = Rows; }; @@ -101,7 +95,7 @@ const auto kBearingLabels = std::array{ "Diagonal", }; -template