diff --git a/beluga/include/beluga/algorithm.hpp b/beluga/include/beluga/algorithm.hpp index 773b4ec29..9dbd0069d 100644 --- a/beluga/include/beluga/algorithm.hpp +++ b/beluga/include/beluga/algorithm.hpp @@ -20,6 +20,7 @@ * \brief Includes all beluga algorithms. */ +#include #include #include #include diff --git a/beluga/include/beluga/algorithm/cluster_based_estimation.hpp b/beluga/include/beluga/algorithm/cluster_based_estimation.hpp new file mode 100644 index 000000000..2c60b5764 --- /dev/null +++ b/beluga/include/beluga/algorithm/cluster_based_estimation.hpp @@ -0,0 +1,435 @@ +// Copyright 2023-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_ALGORITHM_CLUSTER_BASED_ESTIMATION_HPP +#define BELUGA_ALGORITHM_CLUSTER_BASED_ESTIMATION_HPP + +// standard library +#include +#include +#include +#include +#include +#include + +// external +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// project +#include +#include + +#if RANGE_V3_MAJOR == 0 && RANGE_V3_MINOR < 12 +#include +#else +#include +#endif + +/** + * \file + * \brief Implementation of a cluster-based estimation algorithm. + */ + +namespace beluga { + +namespace clusterizer_detail { + +/// Create a priority queue from a map using a specified projection. +/** + * This function template constructs a priority queue where the elements are + * ordered by a priority value derived from the map's values. + * The elements in the queue will contain a key that belongs to the map and the + * corresponding priority. + * + * \tparam Map The type of the associative container. + * \tparam Proj The type of the projection invocable. + * \param map The map containing the data to be inserted into the priority queue. + * \param proj The projection function used to compute the priority of each element. + * \return A priority queue where elements are ordered by the priority computed + * from the map's values using the projection function. + */ +template +[[nodiscard]] auto make_priority_queue(const Map& map, Proj&& proj) { + struct KeyWithPriority { + double priority; // priority value used to order the queue (higher value first). + typename Map::key_type key; + + bool operator<(const KeyWithPriority& other) const { + return priority < other.priority; // sort in decreasing priority order + } + }; + + const auto make_from_pair = [proj = std::forward(proj)](const auto& pair) { + return KeyWithPriority{std::invoke(proj, pair.second), pair.first}; + }; + + const auto range = map | // + ranges::views::transform(make_from_pair) | // + ranges::views::common; + + return std::priority_queue(range.begin(), range.end()); +} + +/// Calculates the threshold value at a specified percentile from a range. +/** + * Find the value that is greater than the given percentage of the numbers in a range. + * + * \tparam Range The type of the input range containing the values. + * \param range The input range of values from which to calculate the percentile threshold. + * \param percentile The percentile (between 0 and 1) to calculate the threshold for. + * \return The value at the specified percentile in the sorted range. + */ +template +[[nodiscard]] auto calculate_percentile_threshold(Range&& range, double percentile) { + auto values = range | ranges::to; + const auto n = static_cast(static_cast(values.size()) * percentile); + std::nth_element(values.begin(), values.begin() + n, values.end()); + return values[static_cast(n)]; +} + +/// A struct that holds the data of a single cell for the clusterization algorithm. +template +struct ClusterCell { + State representative_state; ///< state of a particle that is within the cell + double weight{0.0}; ///< average weight of the cell + std::size_t num_particles{0}; ///< number of particles in the cell + std::optional cluster_id; ///< cluster id of the cell +}; + +/// A map that holds the sparse data about the particles grouped in cells. +template +using ClusterMap = std::unordered_map>; + +/// Create a cluster map from a range of particles and their corresponding spatial hashes. +/** + * This method will populate all the relevant fields in the map except for the cluster ID, + * which has to be computed with a separate call to `assign_clusters`. + * + * \tparam States The range type for particle states. + * \tparam Weights The range type for particle weights. + * \tparam Hashes The range type for particle spatial hashes. + * \param states A range of particle states. + * \param weights A range of particle weights. + * \param hashes A range of particle spatial hashes. + * \return A map where each unique hash corresponds to a cell with accumulated weight, particle count, and + * representative state. + */ +template +[[nodiscard]] static auto make_cluster_map(States&& states, Weights&& weights, Hashes&& hashes) { + using State = ranges::range_value_t; + ClusterMap map; + + // Preallocate memory with a very rough estimation of the number of cells we might end up with. + map.reserve(states.size() / 5); + + // Calculate the accumulated cell weight and save a single representative state for each cell. + for (const auto& [state, weight, hash] : ranges::views::zip(states, weights, hashes)) { + auto [it, inserted] = map.try_emplace(hash, ClusterCell{}); + auto& [_, entry] = *it; + entry.weight += weight; + entry.num_particles++; + if (inserted) { + entry.representative_state = state; + } + } + + return map; +} + +/// Normalize weights and cap them to a given percentile. +/** + * Given a valid cluster map, normalize the accumulated weight by the number of particles + * in each cell to avoid biasing the clustering algorithm towards cells that randomly end up + * with more particles than others. + * + * Then cap the values to a given percentile to flatten the top of the approximated + * density function and make the clustering algorithm more robust to estimation noise, + * by fusing together any adjacent peaks whose weight is above the threshold. + * + * \tparam State The state type of the cells in the map. + * \param map A reference to the map where cells are stored. + * \param percentile The percentile threshold for capping the weights. + */ +template +static void normalize_and_cap_weights(ClusterMap& map, double percentile) { + auto entries = ranges::views::values(map); + + for (auto& entry : entries) { + assert(entry.num_particles > 0); + entry.weight /= static_cast(entry.num_particles); + } + + const double max_weight = + calculate_percentile_threshold(ranges::views::transform(entries, &ClusterCell::weight), percentile); + + for (auto& entry : entries) { + entry.weight = std::min(entry.weight, max_weight); + } +} + +/// Assign cluster ids to an existing cluster map. +/** + * This function implements a clustering algorithm that assigns cluster IDs to cells in a map + * based on their spatial relationships. + * + * Notice that with this algorithm each cell will go through the priority queue at most twice. + * + * \tparam State The state type of the cells in the map. + * \tparam NeighborsFunction A callable object that, given a state, returns a range of neighboring cell hashes. + * \param map A reference to the map where cells are stored. + * \param neighbors_function A function that returns neighboring cell hashes for a given state. + */ +template +static void assign_clusters(ClusterMap& map, NeighborsFunction&& neighbors_function) { + auto queue = make_priority_queue(map, &ClusterCell::weight); + const auto max_priority = queue.top().priority; + + std::size_t next_cluster_id = 0; + + // Process cells in order of descending weight. + while (!queue.empty()) { + const auto hash = queue.top().key; + queue.pop(); + + // The hash is guaranteed to exist in the map. + auto& cell = map[hash]; + + // If there's no cluster id assigned to the cell, assign a new one. + if (!cell.cluster_id.has_value()) { + cell.cluster_id = next_cluster_id++; + } + + // Process the neighbors of the cell; if they don't have a cluster id already assigned + // then assign them one and add them to the queue with an inflated priority + // to ensure they get processed ASAP before moving on to other local peaks. + // Notice that with this algorithm each cell will go through the priority queue at most twice. + + // Define a lambda to check if a neighboring cell is valid for clustering. + const auto is_valid_neighbor = [&](const auto& neighbor_hash) { + auto it = map.find(neighbor_hash); + return ( + (it != map.end()) && // is within the map + (!it->second.cluster_id.has_value()) && // AND not yet mapped to a cluster + (it->second.weight <= cell.weight)); // AND has lower weight than the current cell + }; + + // Process the neighbors of the current cell. + for (const std::size_t neighbor_hash : neighbors_function(cell.representative_state) | // + ranges::views::cache1 | // + ranges::views::filter(is_valid_neighbor)) { + auto& neighbor = map[neighbor_hash]; + neighbor.cluster_id = cell.cluster_id; + queue.push({max_priority + neighbor.weight, neighbor_hash}); // reintroduce with inflated priority + } + } +} + +} // namespace clusterizer_detail + +/// Parameters used to construct a ParticleClusterizer instance. +struct ParticleClusterizerParam { + double linear_hash_resolution = 0.20; ///< clustering algorithm linear resolution + double angular_hash_resolution = 0.524; ///< clustering algorithm angular resolution + + /// Cluster weight cap threshold. + /** + * This parameters is the upper percentile threshold used to cap the accumulated + * weight of the cells in the grid to flatten the top of the approximated + * density function and make the clustering algorithm more robust to estimation noise, + * by fusing together any adjacent peaks whose weight is above the threshold. + * The range of this parameter is 0.0 to 1.0. The 0.9 default places the + * threshold at approximately 0.5 standard deviation from the mean, which is + * halfway to the inflection of the normal distribution. + * */ + double weight_cap_percentile = 0.90; +}; + +/// Particle clusterizer implementation. +class ParticleClusterizer { + public: + /// Constructor that initializes the ParticleClusterizer with given parameters. + explicit ParticleClusterizer(const ParticleClusterizerParam& parameters) : parameters_{parameters} {} + + /// Computes the neighboring cells for a given pose using the spatial hash function. + /** + * \param pose The pose for which neighboring cells are computed. + * \return A range of spatial hashes corresponding to the neighboring cells. + */ + [[nodiscard]] auto neighbors(const Sophus::SE2d& pose) const { + return adjacent_grid_cells_ | // + ranges::views::transform([&pose](const Sophus::SE2d& neighbor_pose) { return pose * neighbor_pose; }) | + ranges::views::transform(spatial_hash_function_); + } + + /// Clusters the given particles based on their states and weights. + /** + * \tparam States Range type of the states. + * \tparam Weights Range type of the weights. + * \param states Range containing the states of the particles. + * \param weights Range containing the weights of the particles. + * \return A vector of cluster IDs corresponding to the input particles. + */ + template + [[nodiscard]] auto operator()(States&& states, Weights&& weights) { + auto hashes = states | ranges::views::transform(spatial_hash_function_) | ranges::to; + + auto map = clusterizer_detail::make_cluster_map(states, weights, hashes); + clusterizer_detail::normalize_and_cap_weights(map, parameters_.weight_cap_percentile); + clusterizer_detail::assign_clusters(map, [this](const auto& state) { return neighbors(state); }); + + return hashes | // + ranges::views::transform([&map](std::size_t hash) { return map[hash].cluster_id.value(); }) | + ranges::to; + } + + private: + ParticleClusterizerParam parameters_; ///< Parameters for the particle clusterizer. + + beluga::spatial_hash spatial_hash_function_{ + parameters_.linear_hash_resolution, // x + parameters_.linear_hash_resolution, // y + parameters_.angular_hash_resolution // theta + }; + + std::array adjacent_grid_cells_ = { + ///< Adjacent grid cells for neighbor calculation. + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{+parameters_.linear_hash_resolution, 0.0}}, + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{-parameters_.linear_hash_resolution, 0.0}}, + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{0.0, +parameters_.linear_hash_resolution}}, + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{0.0, -parameters_.linear_hash_resolution}}, + Sophus::SE2d{Sophus::SO2d{+parameters_.angular_hash_resolution}, Sophus::Vector2d{0.0, 0.0}}, + Sophus::SE2d{Sophus::SO2d{-parameters_.angular_hash_resolution}, Sophus::Vector2d{0.0, 0.0}}, + }; +}; + +/// For each cluster, estimate the mean and covariance of the states that belong to it. +/** + * \tparam States Range type of the states. + * \tparam Weights Range type of the weights. + * \tparam Hashes Range type of the hashes. + * \param states Range containing the states of the particles. + * \param weights Range containing the weights of the particles. + * \param clusters Cluster ids of the particles. + * \return A vector of elements, containing the weight, mean and covariance of each cluster, in no particular order. + */ +template +[[nodiscard]] auto estimate_clusters(States&& states, Weights&& weights, Clusters&& clusters) { + using State = typename ranges::range_value_t; + using Weight = typename ranges::range_value_t; + using Cluster = typename ranges::range_value_t; + + using EstimateState = std::decay_t(beluga::estimate(states, weights)))>; + using EstimateCovariance = std::decay_t(beluga::estimate(states, weights)))>; + + static_assert(std::is_same_v); + + struct Particle { + State state; + Weight weight; + Cluster cluster; + + /// Convenient factory method to pass to `zip_with`. + static constexpr auto create(const State& s, Weight w, Cluster c) { return Particle{s, w, c}; } + }; + + struct Estimate { + Weight weight; + EstimateState mean; + EstimateCovariance covariance; + }; + + auto particles = ranges::views::zip_with(&Particle::create, states, weights, clusters) | // + ranges::to; + + ranges::sort(particles, std::less{}, &Particle::cluster); + + // For each cluster, estimate the mean and covariance of the states that belong to it. + return particles | +#if RANGE_V3_MAJOR == 0 && RANGE_V3_MINOR < 12 + // Compatibility support for old Range-v3 versions that don't have a `chunk_by` view. + // The difference between the deprecated `group_by` and the standard `chunk_by` is: + // - group_by: The predicate is evaluated between the first element in the group and the current one. + // - chunk_by: The predicate is evaluated between adjacent elements. + // + // See also https://github.com/ericniebler/range-v3/issues/1637 + // + // For this specific application, we can use them interchangeably. + ranges::views::group_by([](const auto& p1, const auto& p2) { return p1.cluster == p2.cluster; }) | // +#else + ranges::views::chunk_by([](const auto& p1, const auto& p2) { return p1.cluster == p2.cluster; }) | // +#endif + ranges::views::cache1 | // + ranges::views::filter([](auto subrange) { +#if RANGE_V3_MAJOR == 0 && RANGE_V3_MINOR < 11 + return ranges::distance(subrange) > 1; +#else + // If there's only one sample in the cluster we can't estimate the covariance. + return subrange.size() > 1; +#endif + }) | + ranges::views::transform([](auto subrange) { + auto states = subrange | ranges::views::transform(&Particle::state); + auto weights = subrange | ranges::views::transform(&Particle::weight); + const auto [mean, covariance] = beluga::estimate(states, weights); + const auto total_weight = ranges::accumulate(weights, 0.0); + return Estimate{total_weight, std::move(mean), std::move(covariance)}; + }) | + ranges::to; +} + +/// Computes a cluster-based estimate from a particle set. +/** + * Particles are grouped into clusters around local maxima. The state mean and covariance + * of the cluster with the highest total weight is returned. If no clusters are found, + * the overall mean and covariance of the particles are calculated and returned. + * + * \tparam States Range type of the states. + * \tparam Weights Range type of the weights. + * \param states Range containing the states of the particles. + * \param weights Range containing the weights of the particles. + * \param parameters Parameters for the particle clusterizer (optional). + * \return A pair consisting of the state mean and covariance of the cluster with the highest total weight. + */ +template +[[nodiscard]] auto cluster_based_estimate( + States&& states, // + Weights&& weights, // + ParticleClusterizerParam parameters = {}) { + const auto clusters = ParticleClusterizer{parameters}(states, weights); + + auto per_cluster_estimates = estimate_clusters(states, weights, clusters); + + if (per_cluster_estimates.empty()) { + // hmmm... maybe the particles are too fragmented? calculate the overall mean and covariance. + return beluga::estimate(states, weights); + } + + const auto [_, mean, covariance] = + *ranges::max_element(per_cluster_estimates, std::less{}, [](const auto& e) { return e.weight; }); + + return std::make_pair(mean, covariance); +} + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/algorithm/estimation.hpp b/beluga/include/beluga/algorithm/estimation.hpp index 9081c1751..3f9f1f089 100644 --- a/beluga/include/beluga/algorithm/estimation.hpp +++ b/beluga/include/beluga/algorithm/estimation.hpp @@ -128,6 +128,7 @@ template < typename = std::enable_if_t>>> std::pair, Sophus::Matrix3> estimate(Poses&& poses, Weights&& weights) { auto translation_view = poses | ranges::views::transform([](const auto& pose) { return pose.translation(); }); + auto poses_view = poses | ranges::views::common; auto weights_view = weights | ranges::views::common; const auto weights_sum = std::accumulate(weights_view.begin(), weights_view.end(), 0.0); auto normalized_weights_view = @@ -144,8 +145,8 @@ std::pair, Sophus::Matrix3> estimate(Poses&& poses, // This is expected and the value will be renormalized after having used the non-normal result to estimate the // orientation autocovariance. const Sophus::Vector4 mean_pose_vector = std::transform_reduce( - poses.begin(), poses.end(), normalized_weights_view.begin(), Sophus::Vector4::Zero().eval(), std::plus{}, - pose_to_weighted_eigen_vector); + poses_view.begin(), poses_view.end(), normalized_weights_view.begin(), Sophus::Vector4::Zero().eval(), + std::plus{}, pose_to_weighted_eigen_vector); // Calculate the weighted pose estimation Sophus::SE2 estimated_pose = Eigen::Map>{mean_pose_vector.data()}; diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 9490fcf44..8b7c930db 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -22,6 +22,7 @@ add_executable( actions/test_reweight.cpp algorithm/raycasting/test_bresenham.cpp algorithm/test_amcl_core.cpp + algorithm/test_cluster_based_estimation.cpp algorithm/test_distance_map.cpp algorithm/test_effective_sample_size.cpp algorithm/test_estimation.cpp diff --git a/beluga/test/beluga/algorithm/test_cluster_based_estimation.cpp b/beluga/test/beluga/algorithm/test_cluster_based_estimation.cpp new file mode 100644 index 000000000..64ab2c499 --- /dev/null +++ b/beluga/test/beluga/algorithm/test_cluster_based_estimation.cpp @@ -0,0 +1,420 @@ +// Copyright 2023-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 +#include + +#include + +namespace beluga { +namespace { + +using testing::SE2Near; +using testing::Vector3Near; + +using Eigen::Vector2d; +using Sophus::Matrix3d; +using Sophus::SE2d; +using Sophus::SO2d; + +template +[[nodiscard]] auto precalculate_particle_hashes(Range&& states, const Hasher& spatial_hash_function_) { + return states | ranges::views::transform(spatial_hash_function_) | ranges::to>(); +} + +struct ClusterBasedEstimationDetailTesting : public ::testing::Test { + double kSpatialHashResolution = 1.0; + double kAngularHashResolution = Sophus::Constants::pi() / 2.0; // 90 degrees + double kTolerance = 1e-6; + + // spatial hash function used to group particles in cells + beluga::spatial_hash spatial_hash_function{ + kSpatialHashResolution, // x + kSpatialHashResolution, // y + kAngularHashResolution // theta + }; + + [[nodiscard]] auto generate_test_grid_cell_data_map() const { + constexpr auto kUpperLimit = 30.0; + + typename clusterizer_detail::ClusterMap map; + for (double x = 0.0; x < kUpperLimit; x += 1.0) { + const auto weight = x; + const auto state = SE2d{SO2d{0.}, Vector2d{x, x}}; + map.emplace(spatial_hash_function(state), clusterizer_detail::ClusterCell{state, weight, 0, std::nullopt}); + } + return map; + } + + [[nodiscard]] static auto + make_particle_multicluster_dataset(double xmin, double xmax, double ymin, double ymax, double step) { + std::vector> particles; + + const auto xwidth = xmax - xmin; + const auto ywidth = ymax - ymin; + + // simulate particles in a grid with 4 (2x2) clusters with + // different peak heights. The highest peak is the one located on the + // upper-right. + + for (double x = step / 2.0; x <= xwidth; x += step) { + for (double y = step / 2.0; y <= ywidth; y += step) { + // adjust the height of each of the four peaks + const auto k = (2 * x < xwidth ? 0.0 : 1.0) + (2 * y < ywidth ? 0.0 : 2.0) + 1.0; + auto weight = std::abs(std::sin(2.0 * M_PI * x / xwidth)) * // + std::abs(std::sin(2.0 * M_PI * y / ywidth)) * // + k; + // add a field of zeros around the peaks to ease predicting the mean and + // covariance values of the highest peaks + weight = std::max(0.0, weight - k / 2.0); + particles.emplace_back(SE2d{SO2d{0.}, Vector2d{x + xmin, y + ymin}}, weight); + } + } + + return particles; + } +}; + +TEST_F(ClusterBasedEstimationDetailTesting, ParticleHashesCalculationStep) { + const auto s00 = SE2d{SO2d{0.0}, Vector2d{0.25, 0.25}}; + const auto s01 = SE2d{SO2d{0.0}, Vector2d{3.75, 0.75}}; + const auto s10 = SE2d{SO2d{2.0}, Vector2d{0.00, 0.00}}; + const auto s20 = SE2d{SO2d{2.0}, Vector2d{2.00, 0.00}}; + + const auto states = std::vector{s00, s01, s10, s20}; + + const auto hashes = precalculate_particle_hashes(states, spatial_hash_function); + + const auto hash00 = spatial_hash_function(s00); + const auto hash01 = spatial_hash_function(s01); + const auto hash10 = spatial_hash_function(s10); + const auto hash20 = spatial_hash_function(s20); + + ASSERT_EQ(hashes.size(), 4); + EXPECT_EQ(hashes[0], hash00); + EXPECT_EQ(hashes[1], hash01); + EXPECT_EQ(hashes[2], hash10); + EXPECT_EQ(hashes[3], hash20); +} + +TEST_F(ClusterBasedEstimationDetailTesting, GridCellDataMapGenerationStep) { + const auto s00 = SE2d{SO2d{0.0}, Vector2d{0.25, 0.25}}; // bin 1 + const auto s01 = SE2d{SO2d{0.0}, Vector2d{0.75, 0.75}}; // bin 1 + const auto s10 = SE2d{SO2d{2.0}, Vector2d{0.00, 0.00}}; // bin 2 + const auto s20 = SE2d{SO2d{2.0}, Vector2d{2.00, 0.00}}; // bin 3 + + const auto particles = std::vector>{ + std::make_pair(s00, 1.5), + std::make_pair(s01, 0.5), + std::make_pair(s10, 1.0), + std::make_pair(s20, 1.0), + }; + + auto states = beluga::views::states(particles); + auto weights = beluga::views::weights(particles); + auto hashes = states | ranges::views::transform(spatial_hash_function) | ranges::to; + + auto test_data = clusterizer_detail::make_cluster_map(states, weights, hashes); + + const auto hash00 = spatial_hash_function(s00); + const auto hash10 = spatial_hash_function(s10); + const auto hash20 = spatial_hash_function(s20); + + ASSERT_EQ(test_data.size(), 3); + ASSERT_NE(test_data.find(hash00), test_data.end()); + ASSERT_NE(test_data.find(hash10), test_data.end()); + ASSERT_NE(test_data.find(hash20), test_data.end()); + + EXPECT_EQ(test_data[hash00].weight, 2.0); + EXPECT_EQ(test_data[hash10].weight, 1.0); + EXPECT_EQ(test_data[hash20].weight, 1.0); + + ASSERT_THAT(test_data[hash00].representative_state, SE2Near(s00.so2(), s00.translation(), kTolerance)); + ASSERT_THAT(test_data[hash10].representative_state, SE2Near(s10.so2(), s10.translation(), kTolerance)); + ASSERT_THAT(test_data[hash20].representative_state, SE2Near(s20.so2(), s20.translation(), kTolerance)); + + ASSERT_FALSE(test_data[hash00].cluster_id.has_value()); + ASSERT_FALSE(test_data[hash10].cluster_id.has_value()); + ASSERT_FALSE(test_data[hash20].cluster_id.has_value()); +} + +TEST_F(ClusterBasedEstimationDetailTesting, MakePriorityQueue) { + // data preparation + auto data = generate_test_grid_cell_data_map(); + + // test proper + auto prio_queue = make_priority_queue(data, &clusterizer_detail::ClusterCell::weight); + EXPECT_EQ(prio_queue.size(), data.size()); + + // from there on the weights should be strictly decreasing + auto prev_weight = prio_queue.top().priority; + while (!prio_queue.empty()) { + const auto top = prio_queue.top(); + EXPECT_GE(prev_weight, top.priority); + prev_weight = top.priority; + prio_queue.pop(); + } +} + +TEST_F(ClusterBasedEstimationDetailTesting, MapGridCellsToClustersStep) { + const double k_field_side = 36.0; + const double k_half_field_side = 18.0; + + // create a map with four independent peaks + std::vector> coordinates; + for (double x = 0.0; x < k_field_side; x += 1.0) { + for (double y = 0.0; y < k_field_side; y += 1.0) { + const auto weight = std::abs(std::sin(10.0 * x * M_PI / 180.0)) * std::abs(std::sin(10.0 * y * M_PI / 180.0)); + coordinates.emplace_back(x, y, weight); + } + } + + typename clusterizer_detail::ClusterMap map; + + for (const auto& [x, y, w] : coordinates) { + const auto state = SE2d{SO2d{0.}, Vector2d{x, y}}; + map.emplace(spatial_hash_function(state), clusterizer_detail::ClusterCell{state, w, 0, std::nullopt}); + } + + const auto neighbors = [&](const auto& state) { + static const auto kAdjacentGridCells = { + SE2d{SO2d{0.0}, Vector2d{+kSpatialHashResolution, 0.0}}, + SE2d{SO2d{0.0}, Vector2d{-kSpatialHashResolution, 0.0}}, + SE2d{SO2d{0.0}, Vector2d{0.0, +kSpatialHashResolution}}, + SE2d{SO2d{0.0}, Vector2d{0.0, -kSpatialHashResolution}}, + }; + + return kAdjacentGridCells | // + ranges::views::transform([&state](const Sophus::SE2d& neighbor_pose) { return state * neighbor_pose; }) | + ranges::views::transform(spatial_hash_function); + }; + + // test proper + + // only cells beyond the 10% weight percentile to avoid the messy border + // between clusters beneath that threshold + const auto ten_percent_threshold = calculate_percentile_threshold( + map | ranges::views::values | ranges::views::transform(&clusterizer_detail::ClusterCell::weight), 0.15); + + clusterizer_detail::assign_clusters(map, neighbors); + + auto cells_above_minimum_threshold_view = + coordinates | ranges::views::filter([&](const auto& c) { return std::get<2>(c) >= ten_percent_threshold; }); + + const auto right_side_cell = [&](const auto& c) { return std::get<0>(c) >= k_half_field_side; }; + const auto left_side_cell = [&](const auto& c) { return !right_side_cell(c); }; + const auto top_side_cell = [&](const auto& c) { return std::get<1>(c) >= k_half_field_side; }; + const auto bottom_side_cell = [&](const auto& c) { return !top_side_cell(c); }; + + auto quadrant_1_view = cells_above_minimum_threshold_view | // + ranges::views::filter(left_side_cell) | // + ranges::views::filter(bottom_side_cell); + auto quadrant_2_view = cells_above_minimum_threshold_view | // + ranges::views::filter(right_side_cell) | // + ranges::views::filter(bottom_side_cell); + auto quadrant_3_view = cells_above_minimum_threshold_view | // + ranges::views::filter(left_side_cell) | // + ranges::views::filter(top_side_cell); + auto quadrant_4_view = cells_above_minimum_threshold_view | // + ranges::views::filter(right_side_cell) | // + ranges::views::filter(top_side_cell); + + const auto coord_to_hash = [&](const auto& coords) { + const auto& [x, y, w] = coords; + const auto state = SE2d{SO2d{0.}, Vector2d{x, y}}; + return spatial_hash_function(state); + }; + + const auto hash_to_id = [&](const auto& hash) { return map[hash].cluster_id.value(); }; + + auto quadrant_1_unique_ids = quadrant_1_view | // + ranges::views::transform(coord_to_hash) | // + ranges::views::transform(hash_to_id) | // + ranges::to>() | // + ranges::actions::sort | // + ranges::actions::unique; // + auto quadrant_2_unique_ids = quadrant_2_view | // + ranges::views::transform(coord_to_hash) | // + ranges::views::transform(hash_to_id) | // + ranges::to>() | // + ranges::actions::sort | // + ranges::actions::unique; // + auto quadrant_3_unique_ids = quadrant_3_view | // + ranges::views::transform(coord_to_hash) | // + ranges::views::transform(hash_to_id) | // + ranges::to>() | // + ranges::actions::sort | // + ranges::actions::unique; // + auto quadrant_4_unique_ids = quadrant_4_view | // + ranges::views::transform(coord_to_hash) | // + ranges::views::transform(hash_to_id) | // + ranges::to>() | // + ranges::actions::sort | // + ranges::actions::unique; // + + auto full_field_unique_ids = cells_above_minimum_threshold_view | // + ranges::views::transform(coord_to_hash) | // + ranges::views::transform(hash_to_id) | // + ranges::to>() | // + ranges::actions::sort | // + ranges::actions::unique; // + + // check that each quadrant receives its own cluster id, and that + // there are four clusters in total + EXPECT_EQ(quadrant_1_unique_ids.size(), 1); + EXPECT_EQ(quadrant_2_unique_ids.size(), 1); + EXPECT_EQ(quadrant_3_unique_ids.size(), 1); + EXPECT_EQ(quadrant_4_unique_ids.size(), 1); + + EXPECT_EQ(full_field_unique_ids.size(), 4); +} + +TEST_F(ClusterBasedEstimationDetailTesting, ClusterStateEstimationStep) { + const double k_field_side = 36.0; + + // create a map with four independent peaks + auto particles = make_particle_multicluster_dataset(0.0, k_field_side, 0.0, k_field_side, 1.0); + + const auto clusters = + ParticleClusterizer{ParticleClusterizerParam{kSpatialHashResolution, kAngularHashResolution, 0.9}}( + beluga::views::states(particles), beluga::views::weights(particles)); + + auto per_cluster_estimates = + estimate_clusters(beluga::views::states(particles), beluga::views::weights(particles), clusters); + + // check that the number of clusters is correct + ASSERT_EQ(per_cluster_estimates.size(), 4); + + // order by decreasing weight + ranges::sort(per_cluster_estimates, std::less{}, [](const auto& e) { return e.weight; }); + + // check that the cluster means were found in the expected order + EXPECT_THAT(per_cluster_estimates[0].mean, SE2Near(SO2d{0.0}, Vector2d{9.0, 9.0}, kTolerance)); + EXPECT_THAT(per_cluster_estimates[1].mean, SE2Near(SO2d{0.0}, Vector2d{27.0, 9.0}, kTolerance)); + EXPECT_THAT(per_cluster_estimates[2].mean, SE2Near(SO2d{0.0}, Vector2d{9.0, 27.0}, kTolerance)); + EXPECT_THAT(per_cluster_estimates[3].mean, SE2Near(SO2d{0.0}, Vector2d{27.0, 27.0}, kTolerance)); +} + +TEST_F(ClusterBasedEstimationDetailTesting, ClusterEstimation) { + // test the weights have effect by selecting a few states and ignoring others + const auto states = std::vector{ + SE2d{SO2d{Sophus::Constants::pi() / 6}, Vector2d{0.0, -3.0}}, // + SE2d{SO2d{Sophus::Constants::pi() / 2}, Vector2d{1.0, -2.0}}, // + SE2d{SO2d{Sophus::Constants::pi() / 3}, Vector2d{2.0, -1.0}}, // + SE2d{SO2d{Sophus::Constants::pi() / 2}, Vector2d{1.0, -2.0}}, // + SE2d{SO2d{Sophus::Constants::pi() / 6}, Vector2d{2.0, -3.0}}, // + SE2d{SO2d{Sophus::Constants::pi() / 2}, Vector2d{3.0, -2.0}}, // + SE2d{SO2d{Sophus::Constants::pi() / 3}, Vector2d{4.0, -2.0}}, // + SE2d{SO2d{Sophus::Constants::pi() / 2}, Vector2d{0.0, -3.0}}, // + }; + + // cluster 0 has the max weight, except for cluster 3 which will be ignored because it has only a single particle + const auto weights = std::vector{0.5, 0.5, 0.2, 0.3, 0.3, 0.2, 0.2, 1.0}; + const auto clusters = std::vector{0, 0, 1, 2, 2, 1, 1, 3}; + + const auto particles = ranges::views::zip(states, weights, clusters); + + auto cluster_0_particles = + particles | ranges::views::cache1 | ranges::views::filter([](const auto& p) { return std::get<2>(p) == 0; }); + + auto cluster_0_states = cluster_0_particles | beluga::views::elements<0>; + auto cluster_0_weights = cluster_0_particles | beluga::views::elements<1>; + + const auto [expected_pose, expected_covariance] = beluga::estimate(cluster_0_states, cluster_0_weights); + const auto per_cluster_estimates = beluga::estimate_clusters(states, weights, clusters); + + ASSERT_EQ(per_cluster_estimates.size(), 3); // cluster 3 should be ignored because it has only one particle + + const auto [_, pose, covariance] = + *ranges::max_element(per_cluster_estimates, std::less{}, [](const auto& e) { return e.weight; }); + + constexpr double kTolerance = 0.001; + + ASSERT_THAT(pose, SE2Near(expected_pose.so2(), expected_pose.translation(), kTolerance)); + ASSERT_THAT(covariance.col(0).eval(), Vector3Near(expected_covariance.col(0).eval(), kTolerance)); + ASSERT_THAT(covariance.col(1).eval(), Vector3Near(expected_covariance.col(1).eval(), kTolerance)); + ASSERT_THAT(covariance.col(2).eval(), Vector3Near(expected_covariance.col(2).eval(), kTolerance)); +} + +TEST_F(ClusterBasedEstimationDetailTesting, HeaviestClusterSelectionTest) { + const auto particles = make_particle_multicluster_dataset(-2.0, +2.0, -2.0, +2.0, 0.025); + + // determine the expected values of the mean and covariance of the highest + // weight cluster + const auto max_peak_filter = [](const auto& s) { return s.translation().x() >= 0.0 && s.translation().y() >= 0.0; }; + const auto mask_filter = [](const auto& sample) { return std::get<1>(sample); }; + + auto max_peak_mask = beluga::views::states(particles) | ranges::views::transform(max_peak_filter); + auto max_peak_masked_states = ranges::views::zip(beluga::views::states(particles), max_peak_mask) | + ranges::views::filter(mask_filter) | beluga::views::elements<0>; + auto max_peak_masked_weights = ranges::views::zip(beluga::views::weights(particles), max_peak_mask) | + ranges::views::filter(mask_filter) | beluga::views::elements<0>; + + const auto [expected_pose, expected_covariance] = beluga::estimate(max_peak_masked_states, max_peak_masked_weights); + + const auto [pose, covariance] = + beluga::cluster_based_estimate(beluga::views::states(particles), beluga::views::weights(particles)); + + ASSERT_THAT(pose, SE2Near(expected_pose.so2(), expected_pose.translation(), kTolerance)); + ASSERT_NEAR(covariance(0, 0), expected_covariance(0, 0), 0.001); + ASSERT_NEAR(covariance(0, 1), expected_covariance(0, 1), 0.001); + ASSERT_NEAR(covariance(0, 2), expected_covariance(0, 2), 0.001); + ASSERT_NEAR(covariance(1, 0), expected_covariance(1, 0), 0.001); + ASSERT_NEAR(covariance(1, 1), expected_covariance(1, 1), 0.001); + ASSERT_NEAR(covariance(1, 2), expected_covariance(1, 2), 0.001); + ASSERT_NEAR(covariance(2, 0), expected_covariance(2, 0), 0.001); + ASSERT_NEAR(covariance(2, 1), expected_covariance(2, 1), 0.001); + ASSERT_NEAR(covariance(2, 2), expected_covariance(2, 2), 0.001); +} + +TEST_F(ClusterBasedEstimationDetailTesting, NightmareDistributionTest) { + // particles so far away that they are isolated and will therefore form four separate single + // particle clusters + const auto states = std::vector{ + SE2d{SO2d{0.0}, Vector2d{-10.0, -10.0}}, // + SE2d{SO2d{0.0}, Vector2d{-10.0, +10.0}}, // + SE2d{SO2d{0.0}, Vector2d{+10.0, -10.0}}, // + SE2d{SO2d{0.0}, Vector2d{+10.0, +10.0}}}; + const auto weights = std::vector{0.2, 0.2, 0.2, 0.2}; + + // in this case, the cluster algorithm will not be able to group the particles and will + // default to the set mean and covariance + const auto [expected_pose, expected_covariance] = beluga::estimate(states, weights); + + auto particles = ranges::views::zip(states, weights) | ranges::to(); + const auto [pose, covariance] = beluga::cluster_based_estimate(states, weights); + + ASSERT_THAT(pose, SE2Near(expected_pose.so2(), expected_pose.translation(), kTolerance)); + ASSERT_NEAR(covariance(0, 0), expected_covariance(0, 0), 0.001); + ASSERT_NEAR(covariance(0, 1), expected_covariance(0, 1), 0.001); + ASSERT_NEAR(covariance(0, 2), expected_covariance(0, 2), 0.001); + ASSERT_NEAR(covariance(1, 0), expected_covariance(1, 0), 0.001); + ASSERT_NEAR(covariance(1, 1), expected_covariance(1, 1), 0.001); + ASSERT_NEAR(covariance(1, 2), expected_covariance(1, 2), 0.001); + ASSERT_NEAR(covariance(2, 0), expected_covariance(2, 0), 0.001); + ASSERT_NEAR(covariance(2, 1), expected_covariance(2, 1), 0.001); + ASSERT_NEAR(covariance(2, 2), expected_covariance(2, 2), 0.001); +} + +} // namespace + +} // namespace beluga