diff --git a/beluga/include/beluga/algorithm/cluster_based_estimator.hpp b/beluga/include/beluga/algorithm/cluster_based_estimator.hpp new file mode 100644 index 000000000..55219d26c --- /dev/null +++ b/beluga/include/beluga/algorithm/cluster_based_estimator.hpp @@ -0,0 +1,320 @@ +// 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_ESTIMATION_CLUSTER_BASED_ESTIMATOR_HPP +#define BELUGA_ESTIMATION_CLUSTER_BASED_ESTIMATOR_HPP + +// standard library +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// external +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// project +#include +#include +#include + +/** + * \file + * \brief Implementation of a cluster-base estimator mixin. + */ + +namespace beluga { + +/// \brief A struct that holds the data of a single cell in the grid. +struct GridCellData { + double weight{0.0}; ///< average weight of the cell + std::size_t num_particles{0}; ///< number of particles in the cell + Sophus::SE2d representative_pose_in_world; ///< state of a particle that is within the cell + std::optional cluster_id; ///< cluster id of the cell +}; + +/// \brief A map that holds the sparse data about the particles grouped in cells. Used by the clusterization algorithm. +using GridCellDataMap2D = std::unordered_map; + +/// \brief Populate the grid cell data map with the data from the particles and their weights. +/// \tparam GridCellDataMapType Type of the grid cell data map. +/// \tparam Range Type of the states range. +/// \tparam Weights Type of the weights range. +/// \tparam Hashes Type of the hashes range. +/// \param states Range containing the states of the particles. +/// \param weights Range containing the weights of the particles. +/// \param hashes Range containing the hashes of the particles. +/// \return New instance of the grid cell data map populated with the information from the states. +template +[[nodiscard]] auto populate_grid_cell_data_from_particles(Range&& states, Weights&& weights, const Hashes& hashes) { + GridCellDataMapType grid_cell_data; + + // preallocate memory with a very rough estimation of the number of grid_cells we might end up with + grid_cell_data.reserve(states.size() / 5); + + // calculate the accumulated cell weight and save a single representative_pose_in_world for each cell + for (const auto& [state, weight, hash] : ranges::views::zip(states, weights, hashes)) { + auto [it, inserted] = grid_cell_data.try_emplace(hash, GridCellData{}); + it->second.weight += weight; + it->second.num_particles++; + if (inserted) { + it->second.representative_pose_in_world = state; + } + } + + // 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. + for (auto& [hash, entry] : grid_cell_data) { + entry.weight /= static_cast(entry.num_particles); // num_particles is guaranteed to be > 0 + } + + return grid_cell_data; +} + +/// \brief Calculate the weight threshold that corresponds to a given percentile of the weights. +/// \tparam GridCellDataMapType Type of the grid cell data map. +/// \param grid_cell_data The grid cell data map. +/// \param threshold The percentile of the weights to calculate the threshold for (range: 0.0 to 1.0) +/// \return Threshold value that corresponds to the given percentile of the weights. +template +[[nodiscard]] auto calculate_percentile_weight_threshold(GridCellDataMapType&& grid_cell_data, double threshold) { + const auto extract_weight_f = [](const auto& grid_cell) { return grid_cell.second.weight; }; + auto weights = grid_cell_data | ranges::views::transform(extract_weight_f) | ranges::to>() | + ranges::actions::sort; + return weights[static_cast(static_cast(weights.size()) * threshold)]; +} + +/// \brief Cap the weight of each cell in the grid cell data map to a given value. +/// \tparam GridCellDataMapType Type of the grid cell data map. +/// \param grid_cell_data The grid cell data map. +/// \param weight_cap The maximum weight value to be assigned to each cell. +template +void cap_grid_cell_data_weights(GridCellDataMapType&& grid_cell_data, double weight_cap) { + for (auto& [hash, entry] : grid_cell_data) { + const auto capped_weight = std::min(entry.weight, weight_cap); + entry.weight = capped_weight; + } +} + +/// \brief Creates the priority queue used by the clustering information from the grid cell data map. +/// \tparam GridCellDataMapType Type of the grid cell data map. +/// \param grid_cell_data The grid cell data map. +/// \return A priority queue containing the information from the grid cell data map. +template +[[nodiscard]] auto populate_priority_queue(GridCellDataMapType&& grid_cell_data) { + struct PriorityQueueItem { + double priority; // priority value used to order the queue (higher value first). + std::size_t hash; // hash of the cell in the grid cell data map. + }; + + struct PriorityQueueItemCompare { + constexpr bool operator()(const PriorityQueueItem& lhs, const PriorityQueueItem& rhs) const { + return lhs.priority < rhs.priority; // sort in decreasing priority order + } + }; + + const auto cell_data_to_queue_item = [](const auto& grid_cell) { + return PriorityQueueItem{grid_cell.second.weight, grid_cell.first}; + }; + + auto queue_container = grid_cell_data | // + ranges::views::transform(cell_data_to_queue_item) | // + ranges::to>(); // + return std::priority_queue, PriorityQueueItemCompare>( + PriorityQueueItemCompare{}, std::move(queue_container)); +} + +/// \brief Function that runs the clustering algorithm and assigns a cluster id to each cell in the grid cell data map. +/// \tparam GridCellDataMapType Type of the grid cell data map. +/// \tparam Hasher Type of the hash function used to convert states into hashes. +/// \tparam Neighbors Type of the range containing the neighbors of a cell. +/// \param grid_cell_data The grid cell data map. +/// \param spatial_hash_function_ The hash object instance. +/// \param neighbors Range containing the neighbors of a cell. +/// \param weight_cap The maximum weight value to be assigned to each cell. +template +void map_cells_to_clusters( + GridCellDataMapType&& grid_cell_data, + Hasher&& spatial_hash_function_, + Neighbors&& neighbors, + double weight_cap) { + auto grid_cell_queue = populate_priority_queue(grid_cell_data); + + std::size_t next_cluster_id = 0; + + while (!grid_cell_queue.empty()) { + const auto grid_cell_hash = grid_cell_queue.top().hash; + grid_cell_queue.pop(); + + // any hash that comes out of the queue is known to exist in the cell data map + auto& grid_cell = grid_cell_data[grid_cell_hash]; + const auto& grid_cell_weight = grid_cell.weight; + const auto& representative_pose_in_world = grid_cell.representative_pose_in_world; + + // if there's no cluster id assigned to the cell, assign it a new one + if (!grid_cell.cluster_id.has_value()) { + grid_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. + + const auto get_neighbor_hash = [&](const auto& neighbor_pose_in_representative) { + return spatial_hash_function_(representative_pose_in_world * neighbor_pose_in_representative); + }; + + const auto filter_invalid_neighbors = [&](const auto& neighbor_hash) { + auto it = grid_cell_data.find(neighbor_hash); + return ( + (it != grid_cell_data.end()) && // is within the map + (!it->second.cluster_id.has_value()) && // AND not yet mapped to a cluster + (it->second.weight <= grid_cell_weight)); // AND has lower weight than the current cell + }; + + auto valid_neighbor_hashes_view = // + neighbors | // + ranges::views::transform(get_neighbor_hash) | // + ranges::views::cache1 | // + ranges::views::filter(filter_invalid_neighbors); // + + for (const auto& neighbor_hash : valid_neighbor_hashes_view) { + auto& neighbor = grid_cell_data[neighbor_hash]; + neighbor.cluster_id = grid_cell.cluster_id; + const auto inflated_priority = + weight_cap + neighbor.weight; // since weights are capped at weight_cap, this gives us a value that is + // guaranteed to be higher than any other weight from a local maximum. + grid_cell_queue.push({inflated_priority, neighbor_hash}); // reintroduce with inflated priority + } + } +} + +/// Parameters used to construct a ClusterBasedEstimator instance. +struct ClusterBasedStateEstimatorParam { + double spatial_hash_resolution = 0.20; ///< clustering algorithm spatial resolution + double angular_hash_hesolution = 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; +}; + +/// Primary template for a cluster-based estimation algorithm. +/** + * Particles are grouped into clusters around local maxima, and the state mean and covariance + * of the cluster with the highest total weight is returned. + * + * This class implements the EstimationInterface interface + * and satisfies \ref StateEstimatorPage. + */ +class ClusterBasedStateEstimator { + public: + using param_type = ClusterBasedStateEstimatorParam; ///< Type of the parameters used to construct the estimator. + + /// Constructs a ClusterBasedStateEstimator instance. + /** + * \param parameters Algorithm parameters. + */ + explicit ClusterBasedStateEstimator(const param_type& parameters) : parameters_{parameters} {} + + /// Estimate the weighted mean and covariance of largest (largest aggregated weight) cluster within the particle set. + /** + * \tparam Particles The type of the states range. + * \param particles The particle set. + * \return The weighted mean state and covariance of the largest cluster. + */ + template + [[nodiscard]] auto estimate(Particles&& particles) const; + + private: + param_type parameters_; + + /// \brief spatial hash function used to group particles in cells + const beluga::spatial_hash spatial_hash_function_{ + parameters_.spatial_hash_resolution, // x + parameters_.spatial_hash_resolution, // y + parameters_.angular_hash_hesolution // theta + }; + + /// \brief Adjacent grid cells used by the clustering algorithm. + const std::vector adjacent_grid_cells_ = { + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{+parameters_.spatial_hash_resolution, 0.0}}, + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{-parameters_.spatial_hash_resolution, 0.0}}, + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{0.0, +parameters_.spatial_hash_resolution}}, + Sophus::SE2d{Sophus::SO2d{0.0}, Sophus::Vector2d{0.0, -parameters_.spatial_hash_resolution}}, + Sophus::SE2d{Sophus::SO2d{+parameters_.angular_hash_hesolution}, Sophus::Vector2d{0.0, 0.0}}, + Sophus::SE2d{Sophus::SO2d{-parameters_.angular_hash_hesolution}, Sophus::Vector2d{0.0, 0.0}}, + }; +}; + +template +auto ClusterBasedStateEstimator::estimate(Particles&& particles) const { + auto hashes = beluga::views::states(particles) | ranges::views::transform(spatial_hash_function_) | + ranges::to>(); + + auto grid_cell_data = populate_grid_cell_data_from_particles( + beluga::views::states(particles), beluga::views::weights(particles), hashes); + + const auto weight_cap = calculate_percentile_weight_threshold(grid_cell_data, parameters_.weight_cap_percentile); + + cap_grid_cell_data_weights(grid_cell_data, weight_cap); + map_cells_to_clusters(grid_cell_data, spatial_hash_function_, adjacent_grid_cells_, weight_cap); + + const auto cluster_from_hash = [&grid_cell_data](const std::size_t hash) { + const auto& grid_cell = grid_cell_data[hash]; + return grid_cell.cluster_id; + }; + + const auto clusters = hashes | ranges::views::transform(cluster_from_hash) | ranges::views::common; + + auto per_cluster_estimates = + estimate_clusters(beluga::views::states(particles), beluga::views::weights(particles), clusters); + + if (per_cluster_estimates.empty()) { + // hmmm... maybe the particles are too fragmented? calculate the overall mean and covariance + return beluga::estimate(beluga::views::states(particles), beluga::views::weights(particles)); + } + + const auto [_, mean, covariance] = + *ranges::max_element(per_cluster_estimates, std::less{}, [](const auto& t) { return std::get<0>(t); }); + 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 392ebf0b4..c7ae67cd4 100644 --- a/beluga/include/beluga/algorithm/estimation.hpp +++ b/beluga/include/beluga/algorithm/estimation.hpp @@ -15,6 +15,13 @@ #ifndef BELUGA_ALGORITHM_ESTIMATION_HPP #define BELUGA_ALGORITHM_ESTIMATION_HPP +// standard library +#include +#include +#include + +// external +#include #include #include #include @@ -22,10 +29,12 @@ #include #include #include + #include #include -#include +// project +#include /** * \file @@ -128,6 +137,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 +154,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()}; @@ -240,6 +250,49 @@ std::pair, Sophus::Matrix3> estimate(Poses&& poses) return beluga::estimate(poses, ranges::views::repeat_n(1.0, static_cast(poses.size()))); } +/// \brief For each cluster, estimate the mean and covariance of the states that belong to it. +/// \tparam GridCellDataMapType Type of the grid cell data map. +/// \tparam Range 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 tuples, containing the weight, mean and covariance of each cluster, in no particular order. +template +[[nodiscard]] auto estimate_clusters(Range&& states, Weights&& weights, Clusters&& clusters) { + static constexpr auto weight = [](const auto& t) { return std::get<1>(t); }; + static constexpr auto cluster = [](const auto& t) { return std::get<2>(t); }; + + auto particles = ranges::views::zip(states, weights, clusters); + + const auto cluster_ids = clusters | ranges::to; + + // for each cluster found, estimate the mean and covariance of the states that belong to it + using StateType = std::decay_t(beluga::estimate(states, weights)))>; + using CovarianceType = std::decay_t(beluga::estimate(states, weights)))>; + std::vector> cluster_estimates; + + for (const auto id : cluster_ids) { + auto filtered_particles = + particles | ranges::views::cache1 | ranges::views::filter([id](const auto& p) { return cluster(p) == id; }); + + const auto particle_count = ranges::distance(filtered_particles); + if (particle_count < 2) { + // if there's only one sample in the cluster we can't estimate the covariance + continue; + } + + const auto total_weight = ranges::accumulate(filtered_particles, 0.0, std::plus{}, weight); + auto filtered_states = filtered_particles | beluga::views::elements<0>; + auto filtered_weights = filtered_particles | beluga::views::elements<1>; + const auto [mean, covariance] = estimate(filtered_states, filtered_weights); + cluster_estimates.emplace_back(total_weight, std::move(mean), std::move(covariance)); + } + + return cluster_estimates; +} + } // namespace beluga #endif diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 862056abf..681f1241d 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_estimator.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_estimator.cpp b/beluga/test/beluga/algorithm/test_cluster_based_estimator.cpp new file mode 100644 index 000000000..9b628d410 --- /dev/null +++ b/beluga/test/beluga/algorithm/test_cluster_based_estimator.cpp @@ -0,0 +1,421 @@ +// 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 + +namespace beluga { +namespace { + +using testing::SE2Near; + +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; + + GridCellDataMap2D 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), GridCellData{weight, 0, state, std::nullopt}); + } + return map; + } + + [[nodiscard]] static auto + make_particle_multicluster_dataset(double xmin, double xmax, double ymin, double ymax, double step) { + std::vector states; + std::vector weights; + + 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 on is the one located on the + // higher-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); + states.emplace_back(SO2d{0.}, Vector2d{x + xmin, y + ymin}); + weights.emplace_back(weight); + } + } + + return std::make_tuple(states, weights); + } +}; + +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 states = std::vector{s00, s01, s10, s20}; + const auto weights = std::vector{1.5, 0.5, 1.0, 1.0}; + + auto hashes = precalculate_particle_hashes(states, spatial_hash_function); + auto test_data = populate_grid_cell_data_from_particles(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, 1.0); // this one is averaged between both particles + EXPECT_EQ(test_data[hash10].weight, 1.0); + EXPECT_EQ(test_data[hash20].weight, 1.0); + + ASSERT_THAT(test_data[hash00].representative_pose_in_world, SE2Near(s00.so2(), s00.translation(), kTolerance)); + ASSERT_THAT(test_data[hash10].representative_pose_in_world, SE2Near(s10.so2(), s10.translation(), kTolerance)); + ASSERT_THAT(test_data[hash20].representative_pose_in_world, 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, WeightThresholdCalculationStep) { + auto map = generate_test_grid_cell_data_map(); + const auto threshold = calculate_percentile_weight_threshold(map, 0.9); + EXPECT_DOUBLE_EQ(threshold, 27.0); +} + +TEST_F(ClusterBasedEstimationDetailTesting, WeightCappingStep) { + // data preparation + auto original_map = generate_test_grid_cell_data_map(); + const auto weight_cap = calculate_percentile_weight_threshold(original_map, 0.9); + + // test proper + auto tested_map = original_map; + cap_grid_cell_data_weights(tested_map, weight_cap); + + for (const auto& [hash, grid_cell] : tested_map) { + const auto original_grid_cell = original_map[hash]; + EXPECT_DOUBLE_EQ(grid_cell.weight, std::min(original_grid_cell.weight, weight_cap)); + } +} + +TEST_F(ClusterBasedEstimationDetailTesting, PriorityQueuePopulationStep) { + // data preparation + auto map = generate_test_grid_cell_data_map(); + const auto weight_cap = calculate_percentile_weight_threshold(map, 0.9); + cap_grid_cell_data_weights(map, weight_cap); + + // test proper + auto prio_queue = populate_priority_queue(map); + EXPECT_EQ(prio_queue.size(), map.size()); + + // the top element necessarily has the highest weight equal to the cap + EXPECT_DOUBLE_EQ(prio_queue.top().priority, weight_cap); + + // 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); + } + } + + GridCellDataMap2D map; + for (const auto& [x, y, w] : coordinates) { + const auto state = SE2d{SO2d{0.}, Vector2d{x, y}}; + map.emplace(spatial_hash_function(state), GridCellData{w, 0, state, std::nullopt}); + } + + const auto weight_cap = calculate_percentile_weight_threshold(map, 0.9); + + cap_grid_cell_data_weights(map, weight_cap); + + const auto adjacent_grid_cells = { + 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}}, + }; + + // 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_weight_threshold(map, 0.15); + map_cells_to_clusters(map, spatial_hash_function, adjacent_grid_cells, weight_cap); + + 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, ClusterStatEstimationStep) { + const double k_field_side = 36.0; + + // create a map with four independent peaks + auto [states, weights] = make_particle_multicluster_dataset(0.0, k_field_side, 0.0, k_field_side, 1.0); + + const auto adjacent_grid_cells = { + 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}}, + }; + + auto hashes = precalculate_particle_hashes(states, spatial_hash_function); + auto grid_cell_data = populate_grid_cell_data_from_particles(states, weights, hashes); + const auto weight_cap = calculate_percentile_weight_threshold(grid_cell_data, 0.9); + cap_grid_cell_data_weights(grid_cell_data, weight_cap); + map_cells_to_clusters(grid_cell_data, spatial_hash_function, adjacent_grid_cells, weight_cap); + + const auto cluster_from_hash = [&grid_cell_data](const std::size_t hash) { + const auto& grid_cell = grid_cell_data[hash]; + return grid_cell.cluster_id; + }; + + const auto clusters = hashes | ranges::views::transform(cluster_from_hash) | ranges::views::common; + + auto per_cluster_estimates = estimate_clusters(states, weights, clusters); + + // check that the number of clusters is correct + ASSERT_EQ(per_cluster_estimates.size(), 4); + + // order by decreasing weight + std::sort(per_cluster_estimates.begin(), per_cluster_estimates.end(), [](const auto& a, const auto& b) { + return std::get<0>(a) > std::get<0>(b); + }); + + // check that the cluster means were found in the expected order + EXPECT_THAT(std::get<1>(per_cluster_estimates[0]), SE2Near(SO2d{0.0}, Vector2d{27.0, 27.0}, kTolerance)); + EXPECT_THAT(std::get<1>(per_cluster_estimates[1]), SE2Near(SO2d{0.0}, Vector2d{9.0, 27.0}, kTolerance)); + EXPECT_THAT(std::get<1>(per_cluster_estimates[2]), SE2Near(SO2d{0.0}, Vector2d{27.0, 9.0}, kTolerance)); + EXPECT_THAT(std::get<1>(per_cluster_estimates[3]), SE2Near(SO2d{0.0}, Vector2d{9.0, 9.0}, kTolerance)); +} + +template +class MockMixin : public Mixin { + public: + MOCK_METHOD(const std::vector&, states, (), (const)); + MOCK_METHOD(const std::vector&, weights, (), (const)); +}; + +TEST_F(ClusterBasedEstimationDetailTesting, HeaviestClusterSelectionTest) { + const auto [states, weights] = make_particle_multicluster_dataset(-2.0, +2.0, -2.0, +2.0, 0.025); + + const auto uut = beluga::ClusterBasedStateEstimator{beluga::ClusterBasedStateEstimatorParam{}}; + + // 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 = states | ranges::views::transform(max_peak_filter); + auto max_peak_masked_states = + ranges::views::zip(states, max_peak_mask) | ranges::views::filter(mask_filter) | beluga::views::elements<0>; + auto max_peak_masked_weights = + ranges::views::zip(weights, 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); + + auto particles = ranges::views::zip(states, weights) | ranges::to(); + const auto [pose, covariance] = uut.estimate(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) { + const auto uut = beluga::ClusterBasedStateEstimator{beluga::ClusterBasedStateEstimatorParam{}}; + + // 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] = uut.estimate(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); +} + +} // namespace + +} // namespace beluga diff --git a/beluga/test/beluga/algorithm/test_estimation.cpp b/beluga/test/beluga/algorithm/test_estimation.cpp index 26acfcdc1..527aeac12 100644 --- a/beluga/test/beluga/algorithm/test_estimation.cpp +++ b/beluga/test/beluga/algorithm/test_estimation.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -229,6 +230,47 @@ TEST_F(PoseCovarianceEstimation, RandomWalkWithSmoothRotationAndNonUniformWeight ASSERT_THAT(covariance.col(2).eval(), Vector3Near({0.0000, 0.0000, 0.0855}, kTolerance)); } +TEST_F(PoseCovarianceEstimation, ClusterEstimation) { + // test the weights have effect by selecting a few states and ignoring others + const auto states = std::vector{ + SE2d{SO2d{Constants::pi() / 6}, Vector2d{0.0, -3.0}}, // + SE2d{SO2d{Constants::pi() / 2}, Vector2d{1.0, -2.0}}, // + SE2d{SO2d{Constants::pi() / 3}, Vector2d{2.0, -1.0}}, // + SE2d{SO2d{Constants::pi() / 2}, Vector2d{1.0, -2.0}}, // + SE2d{SO2d{Constants::pi() / 6}, Vector2d{2.0, -3.0}}, // + SE2d{SO2d{Constants::pi() / 2}, Vector2d{3.0, -2.0}}, // + SE2d{SO2d{Constants::pi() / 3}, Vector2d{4.0, -2.0}}, // + SE2d{SO2d{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& t) { return std::get<0>(t); }); + + 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)); +} + struct MeanStandardDeviationEstimation : public testing::Test {}; TEST_F(MeanStandardDeviationEstimation, UniformWeightOverload) {