From 401416e57567fa900818e1ca4613b2d5442d0426 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Sun, 6 Nov 2022 19:45:35 -0300 Subject: [PATCH 1/8] Add sampling algorithms --- beluga/CMakeLists.txt | 2 +- beluga/core/CMakeLists.txt | 5 +- beluga/core/benchmark/sampling_benchmark.cpp | 105 ++++++++++++++++ beluga/core/include/beluga/algorithm.h | 3 + .../core/include/beluga/algorithm/sampling.h | 114 ++++++++++++++++++ .../beluga/type_traits/particle_traits.h | 9 ++ beluga/core/test/sampling_test.cpp | 94 +++++++++++++++ 7 files changed, 330 insertions(+), 2 deletions(-) create mode 100644 beluga/core/benchmark/sampling_benchmark.cpp create mode 100644 beluga/core/include/beluga/algorithm.h create mode 100644 beluga/core/include/beluga/algorithm/sampling.h create mode 100644 beluga/core/test/sampling_test.cpp diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 0036f8313..06e14cb98 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -4,7 +4,7 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_BINARY_DIR}) list(APPEND CMAKE_PREFIX_PATH ${CMAKE_CURRENT_BINARY_DIR}) conan_cmake_configure( - REQUIRES range-v3/0.12.0 + REQUIRES abseil/20220623.0 range-v3/0.12.0 GENERATORS cmake_find_package) conan_cmake_autodetect(settings) diff --git a/beluga/core/CMakeLists.txt b/beluga/core/CMakeLists.txt index e368300bd..6d32c4cb2 100644 --- a/beluga/core/CMakeLists.txt +++ b/beluga/core/CMakeLists.txt @@ -1,12 +1,14 @@ find_package(range-v3) +find_package(absl) add_library(core INTERFACE) target_include_directories(core INTERFACE include) -target_link_libraries(core INTERFACE range-v3::range-v3) +target_link_libraries(core INTERFACE absl::absl range-v3::range-v3) target_compile_features(core INTERFACE cxx_std_17) add_executable(core_benchmark) target_sources(core_benchmark PRIVATE + benchmark/sampling_benchmark.cpp benchmark/tuple_vector_benchmark.cpp benchmark/spatial_hash_benchmark.cpp) target_link_libraries(core_benchmark PRIVATE benchmark_main core) @@ -15,6 +17,7 @@ target_compile_options(core_benchmark PRIVATE -Wall -Werror) add_executable(core_test) target_sources(core_test PRIVATE test/particle_filter_test.cpp + test/sampling_test.cpp test/tuple_vector_test.cpp test/spatial_hash_test.cpp) target_link_libraries(core_test PRIVATE gmock_main core) diff --git a/beluga/core/benchmark/sampling_benchmark.cpp b/beluga/core/benchmark/sampling_benchmark.cpp new file mode 100644 index 000000000..7759b237f --- /dev/null +++ b/beluga/core/benchmark/sampling_benchmark.cpp @@ -0,0 +1,105 @@ +#include + +#include +#include + +#include +#include +#include + +namespace { + +struct State { + double x = 0.; + double y = 0.; + double theta = 0.; +}; + +} // namespace + +template <> +struct beluga::voxel_hash { + std::size_t operator()(const State& state, double voxel_size = 1.) const { + const auto pair = std::make_tuple(state.x, state.y); + return beluga::voxel_hash>{}(pair, voxel_size); + } +}; + +namespace { + +using Particle = std::tuple; +using Container = beluga::TupleVector; + +void BM_FixedResample(benchmark::State& state) { + const std::size_t particle_count = state.range(0); + state.SetComplexityN(particle_count); + + auto container = Container{particle_count}; + auto new_container = Container{particle_count}; + auto generator = std::mt19937{std::random_device()()}; + + for (auto&& [state, weight, cluster] : beluga::views::all(container)) { + weight = 1.; + } + + struct Params { + std::size_t min_samples; + }; + + auto parameters = Params{.min_samples = static_cast(particle_count)}; + + for (auto _ : state) { + auto&& samples = beluga::views::fixed_resample(container, generator, parameters) | ranges::views::common; + auto first = std::begin(beluga::views::all(new_container)); + auto last = std::copy(std::begin(samples), std::end(samples), first); + state.counters["SampleSize"] = std::distance(first, last); + } +} + +BENCHMARK(BM_FixedResample)->MinWarmUpTime(1)->RangeMultiplier(2)->Range(128, 1'000'000)->Complexity(); + +void BM_AdaptiveResample(benchmark::State& state) { + const std::size_t particle_count = state.range(0); + state.SetComplexityN(particle_count); + + auto container = Container{particle_count}; + auto new_container = Container{particle_count}; + auto generator = std::mt19937{std::random_device()()}; + + double step = 0; + int i = 0; + for (auto&& [state, weight, cluster] : beluga::views::all(container)) { + weight = 1.; + state.x = step; + if (i++ % 2 == 0) { + step += 0.05; + } + } + + struct Params { + std::size_t min_samples; + std::size_t max_samples; + double voxel_size; + double kld_epsilon; + double kld_upper_quantile; + }; + + auto parameters = Params{ + .min_samples = 0, + .max_samples = particle_count, + .voxel_size = 1., + .kld_epsilon = 0.05, + .kld_upper_quantile = 0.95}; + + for (auto _ : state) { + auto&& samples = beluga::views::adaptive_resample(container, generator, parameters) | ranges::views::common; + auto first = std::begin(beluga::views::all(new_container)); + auto last = std::copy(std::begin(samples), std::end(samples), first); + state.counters["SampleSize"] = std::distance(first, last); + state.counters["Percentage"] = static_cast(std::distance(first, last)) / particle_count; + } +} + +BENCHMARK(BM_AdaptiveResample)->MinWarmUpTime(1)->RangeMultiplier(2)->Range(128, 1'000'000)->Complexity(); + +} // namespace diff --git a/beluga/core/include/beluga/algorithm.h b/beluga/core/include/beluga/algorithm.h new file mode 100644 index 000000000..833b1041f --- /dev/null +++ b/beluga/core/include/beluga/algorithm.h @@ -0,0 +1,3 @@ +#pragma once + +#include diff --git a/beluga/core/include/beluga/algorithm/sampling.h b/beluga/core/include/beluga/algorithm/sampling.h new file mode 100644 index 000000000..572ba090f --- /dev/null +++ b/beluga/core/include/beluga/algorithm/sampling.h @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include +#include + +namespace beluga { + +template +auto random_select(Function1&& first, Function2&& second, RandomNumberGenerator& generator, double probability) { + return [&, probability, distribution = std::uniform_real_distribution{}]() mutable { + return distribution(generator) < probability ? first() : second(); + }; +} + +template +auto random_sample(const Range& samples, const Weights& weights, RandomNumberGenerator& generator) { + return [&generator, first = std::begin(samples), + distribution = absl::discrete_distribution{std::begin(weights), std::end(weights)}]() mutable { + return *(first + distribution(generator)); + }; +} + +namespace detail { + +inline auto set_cluster(double voxel_size) { + return [voxel_size](auto&& particle) { + using state_type = std::decay_t; + auto new_particle = particle; + cluster(new_particle) = voxel_hash{}(state(particle), voxel_size); + return new_particle; + }; +} + +inline auto kld_condition(std::size_t min, double epsilon = 0.05, double upper_quantile = 0.95) { + // Compute minimum number of samples based on a Kullback-Leibler distance epsilon + // between the maximum likelihood estimate and the true distribution. + auto target_size = [upper_quantile, epsilon](std::size_t k) { + double common = 2. / (9 * (k - 1)); + double base = 1. - common - std::sqrt(common) * upper_quantile; + double result = ((k - 1) / epsilon) * base * base * base; + return static_cast(std::ceil(result)); + }; + + return [=, count = 0ULL, buckets = std::unordered_set{}](std::size_t hash) mutable { + count++; + buckets.insert(hash); + auto cluster_count = buckets.size(); + bool target_not_reached = cluster_count <= 2U ? true : count < target_size(cluster_count); + return count < min || target_not_reached; + }; +} + +} // namespace detail + +namespace views { + +template +auto fixed_resample(Container&& particles, RandomNumberGenerator& random_number_generator, const Params& params) { + return ranges::views::generate(random_sample(all(particles), weights(particles), random_number_generator)) | + ranges::views::take_exactly(params.min_samples); +} + +template +auto fixed_resample( + Container&& particles, + RandomSampleGenerator& random_sample_generator, + RandomNumberGenerator& random_number_generator, + const Params& params) { + using particle_type = typename std::decay_t::value_type; + return ranges::views::generate(random_select( + random_sample_generator, random_sample(states(particles), weights(particles), random_number_generator), + random_number_generator, params.random_sample_probability)) | + ranges::views::transform(make_from_state) | ranges::views::take_exactly(params.min_samples); +} + +template +auto adaptive_resample(Container&& particles, RandomNumberGenerator& random_number_generator, const Params& params) { + using particle_type = typename std::decay_t::value_type; + return ranges::views::generate(random_sample(all(particles), weights(particles), random_number_generator)) | + ranges::views::transform(detail::set_cluster(params.voxel_size)) | + ranges::views::take_while( + detail::kld_condition(params.min_samples, params.kld_epsilon, params.kld_upper_quantile), + cluster) | + ranges::views::take(params.max_samples); +} + +template +auto adaptive_resample( + Container&& particles, + RandomSampleGenerator& random_sample_generator, + RandomNumberGenerator& random_number_generator, + const Params& params) { + using particle_type = typename std::decay_t::value_type; + return ranges::views::generate(random_select( + random_sample_generator, random_sample(states(particles), weights(particles), random_number_generator), + random_number_generator, params.random_sample_probability)) | + ranges::views::transform(make_from_state) | + ranges::views::transform(detail::set_cluster(params.voxel_size)) | + ranges::views::take_while( + detail::kld_condition(params.min_samples, params.kld_epsilon, params.kld_upper_quantile), + cluster) | + ranges::views::take(params.max_samples); +} + +} // namespace views + +} // namespace beluga diff --git a/beluga/core/include/beluga/type_traits/particle_traits.h b/beluga/core/include/beluga/type_traits/particle_traits.h index de2f4029c..fb9bb42f2 100644 --- a/beluga/core/include/beluga/type_traits/particle_traits.h +++ b/beluga/core/include/beluga/type_traits/particle_traits.h @@ -34,6 +34,7 @@ struct particle_traits {}; template