From 528a981713a44926f4deef8327dd4cde66edd9d5 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Sat, 27 Jan 2024 18:57:33 -0300 Subject: [PATCH] Add normalize action (#297) Related to #279. Signed-off-by: Nahuel Espinosa --- beluga/include/beluga/actions.hpp | 1 + beluga/include/beluga/actions/normalize.hpp | 161 ++++++++++++++++++ beluga/test/beluga/CMakeLists.txt | 1 + beluga/test/beluga/actions/test_normalize.cpp | 93 ++++++++++ beluga_system_tests/test/test_system_new.cpp | 10 +- 5 files changed, 258 insertions(+), 8 deletions(-) create mode 100644 beluga/include/beluga/actions/normalize.hpp create mode 100644 beluga/test/beluga/actions/test_normalize.cpp diff --git a/beluga/include/beluga/actions.hpp b/beluga/include/beluga/actions.hpp index 4a0440f85..b44da1f34 100644 --- a/beluga/include/beluga/actions.hpp +++ b/beluga/include/beluga/actions.hpp @@ -16,6 +16,7 @@ #define BELUGA_ACTIONS_HPP #include +#include #include #include diff --git a/beluga/include/beluga/actions/normalize.hpp b/beluga/include/beluga/actions/normalize.hpp new file mode 100644 index 000000000..747b91bb8 --- /dev/null +++ b/beluga/include/beluga/actions/normalize.hpp @@ -0,0 +1,161 @@ +// 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_NORMALIZE_HPP +#define BELUGA_ACTIONS_NORMALIZE_HPP + +#include +#include + +#include +#include + +#include +#include +#include + +namespace beluga::actions { + +namespace detail { + +/// Implementation detail for a normalize range adaptor object. +struct normalize_base_fn { + /// Overload that implements the normalize algorithm. + /** + * \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). + * \param policy The execution policy to use. + * \param range An existing range to apply this action to. + * \param factor The normalization factor. + */ + template < + class ExecutionPolicy, + class Range, + std::enable_if_t>, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(ExecutionPolicy&& policy, Range& range, double factor) const -> Range& { + if (std::abs(factor - 1.0) < std::numeric_limits::epsilon()) { + return range; // No change. + } + + auto weights = [&range]() { + if constexpr (beluga::is_particle_range_v) { + return range | beluga::views::weights | ranges::views::common; + } else { + return range | ranges::views::common; + } + }(); + + std::transform( + policy, // + std::begin(weights), // + std::end(weights), // + std::begin(weights), // + [factor](const auto w) { return w / factor; }); + return range; + } + + /// Overload that uses a default normalization factor. + /** + * The default normalization factor is the total sum of weights. + */ + template < + class ExecutionPolicy, + class Range, + std::enable_if_t>, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(ExecutionPolicy&& policy, Range& range) const -> Range& { + auto weights = [&range]() { + if constexpr (beluga::is_particle_range_v) { + return range | beluga::views::weights | ranges::views::common; + } else { + return range | ranges::views::common; + } + }(); + + const double total_weight = ranges::accumulate(weights, 0.0); + return (*this)(std::forward(policy), range, total_weight); + } + + /// Overload that re-orders arguments from an action closure. + template < + class Range, + class ExecutionPolicy, + std::enable_if_t, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(Range&& range, double factor, ExecutionPolicy policy) const -> Range& { + return (*this)(std::move(policy), std::forward(range), factor); + } + + /// Overload that re-orders arguments from an action closure. + template < + class Range, + class ExecutionPolicy, + std::enable_if_t, int> = 0, + std::enable_if_t, int> = 0> + constexpr auto operator()(Range&& range, ExecutionPolicy policy) const -> Range& { + return (*this)(std::move(policy), std::forward(range)); + } + + /// Overload that returns an action closure to compose with other actions. + template , int> = 0> + constexpr auto operator()(ExecutionPolicy policy, double factor) const { + return ranges::make_action_closure(ranges::bind_back(normalize_base_fn{}, factor, std::move(policy))); + } + + /// Overload that returns an action closure to compose with other actions. + template , int> = 0> + constexpr auto operator()(ExecutionPolicy policy) const { + return ranges::make_action_closure(ranges::bind_back(normalize_base_fn{}, std::move(policy))); + } +}; + +/// Implementation detail for a normalize range adaptor object with a default execution policy. +struct normalize_fn : public normalize_base_fn { + using normalize_base_fn::operator(); + + /// Overload that defines a default execution policy. + template , int> = 0> + constexpr auto operator()(Range&& range, double factor) const -> Range& { + return (*this)(std::execution::seq, std::forward(range), factor); + } + + /// Overload that defines a default execution policy. + template , int> = 0> + constexpr auto operator()(Range&& range) const -> Range& { + return (*this)(std::execution::seq, std::forward(range)); + } + + /// Overload that returns an action closure to compose with other actions. + constexpr auto operator()(double factor) const { + return ranges::make_action_closure(ranges::bind_back(normalize_fn{}, factor)); + } +}; + +} // namespace detail + +/// [Range adaptor object](https://en.cppreference.com/w/cpp/named_req/RangeAdaptorObject) that +/// can normalize a range of values (or a range of particles). +/** + * The `normalize` range adaptor allows users to normalize the weights of a range + * (or a range of particles) by dividing each weight by a specified normalization factor. + * + * If none is specified, the default normalization factor corresponds to the total sum of weights + * in the given range. + */ +inline constexpr ranges::actions::action_closure normalize; + +} // namespace beluga::actions + +#endif diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index bdec36ca7..38416c002 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -15,6 +15,7 @@ add_executable( test_beluga actions/test_assign.cpp + actions/test_normalize.cpp actions/test_propagate.cpp actions/test_reweight.cpp algorithm/raycasting/test_bresenham.cpp diff --git a/beluga/test/beluga/actions/test_normalize.cpp b/beluga/test/beluga/actions/test_normalize.cpp new file mode 100644 index 000000000..6b9cd14b7 --- /dev/null +++ b/beluga/test/beluga/actions/test_normalize.cpp @@ -0,0 +1,93 @@ +// 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 + +namespace { + +TEST(NormalizeAction, DefaultExecutionPolicy) { + auto input = std::vector{std::make_tuple(5, beluga::Weight(4.0))}; + input |= beluga::actions::normalize(2.0); + ASSERT_EQ(input.front(), std::make_tuple(5, 2.0)); +} + +TEST(NormalizeAction, SequencedExecutionPolicy) { + auto input = std::vector{std::make_tuple(5, beluga::Weight(4.0))}; + input |= beluga::actions::normalize(std::execution::seq, 2.0); + ASSERT_EQ(input.front(), std::make_tuple(5, 2.0)); +} + +TEST(NormalizeAction, ParallelExecutionPolicy) { + auto input = std::vector{std::make_tuple(5, beluga::Weight(4.0))}; + input |= beluga::actions::normalize(std::execution::par, 2.0); + ASSERT_EQ(input.front(), std::make_tuple(5, 2.0)); +} + +TEST(NormalizeAction, DefaultFactor) { + auto input = std::vector{std::make_tuple(5, beluga::Weight(4.0))}; + input |= beluga::actions::normalize(std::execution::seq); + ASSERT_EQ(input.front(), std::make_tuple(5, 1.0)); +} + +TEST(NormalizeAction, DefaultFactorAndExecutionPolicy) { + auto input = std::vector{std::make_tuple(5, beluga::Weight(4.0))}; + input |= beluga::actions::normalize; + ASSERT_EQ(input.front(), std::make_tuple(5, 1.0)); +} + +TEST(NormalizeAction, EmptyInputRange) { + auto input = std::vector>{}; + input |= beluga::actions::normalize(2.0); + ASSERT_TRUE(input.empty()); +} + +TEST(NormalizeAction, MultipleParticles) { + auto input = std::vector{ + std::make_tuple(5, beluga::Weight(4.0)), // + std::make_tuple(8, beluga::Weight(2.0)), // + std::make_tuple(3, beluga::Weight(6.0))}; + input |= beluga::actions::normalize(2.0); + ASSERT_EQ(input.size(), 3); + ASSERT_EQ(input[0], std::make_tuple(5, 2.0)); + ASSERT_EQ(input[1], std::make_tuple(8, 1.0)); + ASSERT_EQ(input[2], std::make_tuple(3, 3.0)); +} + +TEST(NormalizeAction, MultipleElements) { + auto input = std::vector{4.0, 2.0, 6.0}; + input |= beluga::actions::normalize(2.0); + ASSERT_EQ(input.size(), 3); + ASSERT_EQ(input[0], 2.0); + ASSERT_EQ(input[1], 1.0); + ASSERT_EQ(input[2], 3.0); +} + +TEST(NormalizeAction, ZeroFactor) { + auto input = std::vector{std::make_tuple(5, beluga::Weight(4.0))}; + input |= beluga::actions::normalize(0.0); + ASSERT_TRUE(std::isinf(beluga::weight(input.front()))); +} + +TEST(NormalizeAction, NegativeFactor) { + auto input = std::vector{std::make_tuple(5, beluga::Weight(4.0))}; + input |= beluga::actions::normalize(-2.0); + ASSERT_EQ(input.front(), std::make_tuple(5, beluga::Weight(-2.0))); +} + +} // namespace diff --git a/beluga_system_tests/test/test_system_new.cpp b/beluga_system_tests/test/test_system_new.cpp index d234a47e3..537a064fc 100644 --- a/beluga_system_tests/test/test_system_new.cpp +++ b/beluga_system_tests/test/test_system_new.cpp @@ -172,14 +172,8 @@ auto particle_filter_test( return motion.apply_motion(state, engine); }) | beluga::actions::reweight( - std::execution::par, [&sensor](const auto& state) { return sensor.importance_weight(state); }); - - // TODO(nahuel): Implement a `normalize` action closure that normalizes over the total weight. - /** - * particles |= beluga::actions::normalize; - */ - const double total_weight = ranges::accumulate(beluga::views::weights(particles), 0.0); - particles |= beluga::actions::reweight([total_weight](auto) { return 1.0 / total_weight; }); // HACK + std::execution::par, [&sensor](const auto& state) { return sensor.importance_weight(state); }) | + beluga::actions::normalize(std::execution::par_unseq); const double random_state_probability = probability_estimator(particles);