Add support for per-axis resolution when clustering (#146)

Fixes #68 

 `spatial_hash` now has a different resolution for each axis, which
allows us to discriminate clustering resolution for each axis.

Specialization for `Sophus::SE2d` now takes into account rotation for
spatial clusterization.


Signed-off-by: Ramiro Serra <[email protected]>

beluga/include/beluga/algorithm/sampling.hpp

@@ -163,19 +163,19 @@ auto random_sample(const Range& samples, const Weights& weights, RandomNumberGen
 
 /// Returns a callable object that allows setting the cluster of a particle.
 /**
- * \param resolution The size along any axis of the spatial cluster cell.
- * \return A callable object with prototype `(ParticleT && p) -> ParticleT`. \n
- *  `ParticleT` must satisfy \ref ParticlePage. \n
- *  The expression \c particle_traits<ParticleT>::cluster(p) must also
- *  be valid and return a `std::size_t &`. \n
- *  After the returned object is applied to a particle `p`, \c cluster(p) will be updated
- *  with the calculated spatial hash according to the specified resolution.
+ * \param hasher A copyable callable object with signature (const decltype(state(particle)) &) -> std::size_t, that
+ * returns a spatial cluster for a given particle state.
+ * \return A callable object with prototype `(ParticleT && p) ->ParticleT`. \n
+ *  `ParticleT` must satisfy \ref ParticlePage. \ n
+ *  The expression \c particle_traits<ParticleT>::cluster(p) must also be valid and return a `std::size_t &`. \n
+ *  After the returned object is applied to a particle `p`, \c cluster(p) will be updated with the calculated spatial
+ * hash according to the specified resolutions in each axis.
  */
-inline auto set_cluster(double resolution) {
-  return [resolution](auto&& particle) {
-    using state_type = std::decay_t<decltype(state(particle))>;
+template <class Hasher>
+inline auto set_cluster(Hasher&& hasher) {
+  return [hasher](auto&& particle) {
     auto new_particle = particle;
-    cluster(new_particle) = spatial_hash<state_type>{}(state(particle), resolution);
+    cluster(new_particle) = hasher(state(particle));
     return new_particle;
   };
 }
@@ -451,13 +451,17 @@ class FixedLimiter : public Mixin {
 };
 
 /// Parameters used to construct a KldLimiter instance.
+/**
+ * \tparam State Type that represents the state of the particle.
+ */
+template <class State>
 struct KldLimiterParam {
   /// Minimum number of particles to be sampled.
   std::size_t min_samples;
   /// Maximum number of particles to be sampled.
   std::size_t max_samples;
-  /// Cluster resolution in any axis, used to compute the spatial hash.
-  double spatial_resolution;
+  /// Hasher instance used to compute the spatial cluster for a given state.
+  spatial_hash<State> spatial_hasher;
   /// See beluga::kld_condition() for details.
   double kld_epsilon;
   /// See beluga::kld_condition() for details.
@@ -473,6 +477,8 @@ struct KldLimiterParam {
  *
  * \tparam Mixin The mixed-in type. `Mixin::self_type::particle_type` must exist and
  * satisfy \ref ParticlePage.
+ * \tparam State Type that represents the state of a particle. It should match with
+ * particle_traits<Mixin::self_type::particle_type::state>::state_type.
  *
  * Additionally, given:
  * - `P`, the type `Mixin::self_type::particle_type`
@@ -487,11 +493,11 @@ struct KldLimiterParam {
  *   assigns the cluster hash to the particle `p`. \n
  *   i.e. after the assignment `h` == \c particle_traits<P>::cluster(p) is true.
  */
-template <class Mixin>
+template <class Mixin, class State>
 class KldLimiter : public Mixin {
  public:
   /// Parameters type used to construct a KldLimiter instance.
-  using param_type = KldLimiterParam;
+  using param_type = KldLimiterParam<State>;
 
   /// Constructs a KldLimiter instance.
   /**
@@ -532,7 +538,7 @@ class KldLimiter : public Mixin {
   [[nodiscard]] auto take_samples() const {
     using particle_type = typename Mixin::self_type::particle_type;
     return ranges::views::transform(beluga::make_from_state<particle_type>) |
-           ranges::views::transform(beluga::set_cluster(parameters_.spatial_resolution)) |
+           ranges::views::transform(beluga::set_cluster(parameters_.spatial_hasher)) |
            ranges::views::take_while(
                beluga::kld_condition(parameters_.min_samples, parameters_.kld_epsilon, parameters_.kld_z),
                [](auto&& particle) { return cluster(particle); }) |

beluga/include/beluga/algorithm/spatial_hash.hpp

@@ -63,10 +63,12 @@ constexpr std::size_t floor_and_shift(double value) {
  * \return The calculated hash.
  */
 template <class T, std::size_t... Ids>
-constexpr std::size_t
-hash_impl(const T& value, double resolution, [[maybe_unused]] std::index_sequence<Ids...> index_sequence) {
+constexpr std::size_t hash_impl(
+    const T& value,
+    const std::array<double, sizeof...(Ids)>& resolution,
+    [[maybe_unused]] std::index_sequence<Ids...> index_sequence) {
   constexpr auto kBits = std::numeric_limits<std::size_t>::digits / sizeof...(Ids);
-  return (detail::floor_and_shift<kBits, Ids>(std::get<Ids>(value) / resolution) | ...);
+  return (detail::floor_and_shift<kBits, Ids>(std::get<Ids>(value) / resolution[Ids]) | ...);
 }
 
 }  // namespace detail
@@ -77,50 +79,92 @@ struct spatial_hash {};
 
 /// Specialization for arrays.
 template <class T, std::size_t N>
-struct spatial_hash<std::array<T, N>, std::enable_if_t<std::is_arithmetic_v<T>, void>> {
+class spatial_hash<std::array<T, N>, std::enable_if_t<std::is_arithmetic_v<T>, void>> {
  public:
-  /// Calculates the array hash, using the given resolution in all axes.
+  /// Type that represents the resolution in each axis.
+  using resolution_in_each_axis_t = std::array<double, N>;
+
+  /// Constructs a spatial hasher from an `std::array` of doubles.
+  /**
+   *  \param resolution std::array of doubles containing resolution for each index of the array to be hashed, with
+   * matching indices. I.e: array[0] will be hashed with resolution[0].
+   */
+  explicit spatial_hash(const resolution_in_each_axis_t& resolution) : resolution_{resolution} {}
+
+  /// Calculates the array hash, with the resolutions in each axis, given at construction time.
   /**
    * \param array Array to be hashed.
-   * \param resolution Resolution to be used in each axes.
    * \return The calculated hash.
    */
-  constexpr std::size_t operator()(const std::array<T, N>& array, double resolution = 1.) const {
-    return detail::hash_impl(array, resolution, std::make_index_sequence<N>());
+  constexpr std::size_t operator()(const std::array<T, N>& array) const {
+    return detail::hash_impl(array, resolution_, std::make_index_sequence<N>());
   }
+
+ private:
+  resolution_in_each_axis_t resolution_;
 };
 
 /// Specialization for tuples.
 template <template <class...> class Tuple, class... Types>
-struct spatial_hash<Tuple<Types...>, std::enable_if_t<(std::is_arithmetic_v<Types> && ...), void>> {
+class spatial_hash<Tuple<Types...>, std::enable_if_t<(std::is_arithmetic_v<Types> && ...), void>> {
  public:
-  /// Calculates the tuple hash, using the given resolution in all axes.
+  /// Type that represents the resolution in each axis.
+  using resolution_in_each_axis_t = std::array<double, sizeof...(Types)>;
+
+  /// Constructs a spatial hasher from an `std::array` of doubles.
+  /**
+   *  \param resolution std::array of doubles containing resolution for each index of the tuple to be hashed, with
+   * matching indices. I.e: std::get<0>(tuple) will be hashed with resolution[0].
+   */
+  explicit spatial_hash(const resolution_in_each_axis_t& resolution) : resolution_{resolution} {}
+
+  /// Calculates the array hash, with the resolutions in each axis, given at construction time.
   /**
    * \param tuple Tuple to be hashed.
-   * \param resolution Resolution to be used in each axes.
    * \return The calculated hash.
    */
-  constexpr std::size_t operator()(const Tuple<Types...>& tuple, double resolution = 1.) const {
-    return detail::hash_impl(tuple, resolution, std::make_index_sequence<sizeof...(Types)>());
+  constexpr std::size_t operator()(const Tuple<Types...>& tuple) const {
+    return detail::hash_impl(tuple, resolution_, std::make_index_sequence<sizeof...(Types)>());
   }
+
+ private:
+  resolution_in_each_axis_t resolution_;
 };
 
 /**
- * Will calculate the spatial hash based on the translation, the rotation is not used.
+ * Specialization for Sophus::SE2d. Will calculate the spatial hash based on the translation and rotation.
  */
 template <>
-struct spatial_hash<Sophus::SE2d, void> {
+class spatial_hash<Sophus::SE2d, void> {
  public:
-  /// Calculates the tuple hash, using the given resolution for both the x and y translation coordinates.
+  /// Constructs a spatial hasher from an `std::array` of doubles.
+  /**
+   *
+   * \param x_clustering_resolution Clustering resolution for the X axis, in meters.
+   * \param y_clustering_resolution Clustering resolution for the Y axis, in meters.
+   * \param theta_clustering_resolution Clustering resolution for the Theta axis, in radians.
+   */
+  explicit spatial_hash(
+      double x_clustering_resolution,
+      double y_clustering_resolution,
+      double theta_clustering_resolution)
+      : underlying_hasher_{{x_clustering_resolution, y_clustering_resolution, theta_clustering_resolution}} {};
+
+  /// Default constructor
+  spatial_hash() = default;
+
+  /// Calculates the tuple hash, using the given resolution for x, y and rotation given at construction time.
   /**
    * \param state The state to be hashed.
-   * \param resolution Resolution to be used for both x and y translation coordinates.
    * \return The calculated hash.
    */
-  std::size_t operator()(const Sophus::SE2d& state, double resolution = 1.) const {
+  std::size_t operator()(const Sophus::SE2d& state) const {
     const auto& position = state.translation();
-    return spatial_hash<std::tuple<double, double>>{}(std::make_tuple(position.x(), position.y()), resolution);
+    return underlying_hasher_(std::make_tuple(position.x(), position.y(), state.so2().log()));
   }
+
+ private:
+  spatial_hash<std::tuple<double, double, double>> underlying_hasher_{{1., 1., 1.}};
 };
 
 }  // namespace beluga

beluga/include/beluga/localization.hpp

@@ -99,7 +99,7 @@ using AdaptiveMonteCarloLocalization2d = ciabatta::mixin<
     SimpleStateEstimator2d,
     RandomStateGenerator,
     AdaptiveSampler,
-    KldLimiter,
+    ciabatta::curry<KldLimiter, Sophus::SE2d>::mixin,
     CombinedResamplingPolicy::template mixin,
     MotionDescriptor::template mixin,
     SensorDescriptor::template mixin,

beluga/test/beluga/algorithm/test_sampling.cpp

@@ -226,26 +226,34 @@ struct MockStorageWithCluster : public Mixin {
   explicit MockStorageWithCluster(Args&&... rest) : Mixin(std::forward<Args>(rest)...) {}
 };
 
+using state = std::pair<double, double>;
+
 TEST(KldLimiter, TakeMaximum) {
-  auto instance =
-      ciabatta::mixin<MockStorageWithCluster, beluga::KldLimiter>{beluga::KldLimiterParam{0, 1'200, 0.05, 0.05, 3.}};
+  constexpr std::array kClusteringResolution{1., 1.};
+  auto hasher = beluga::spatial_hash<state>{kClusteringResolution};
+  auto instance = ciabatta::mixin<MockStorageWithCluster, ciabatta::curry<beluga::KldLimiter, state>::mixin>{
+      beluga::KldLimiterParam<state>{200, 1'200, hasher, 0.05, 3.}};
   auto output = ranges::views::generate([]() { return std::make_pair(1., 1.); }) | instance.take_samples() |
                 ranges::to<std::vector>;
   ASSERT_EQ(output.size(), 1'200);
 }
 
 TEST(KldLimiter, TakeLimit) {
-  auto instance =
-      ciabatta::mixin<MockStorageWithCluster, beluga::KldLimiter>{beluga::KldLimiterParam{0, 1'200, 0.05, 0.05, 3.}};
+  constexpr std::array kClusteringResolution{0.05, 0.05};
+  auto hasher = beluga::spatial_hash<state>{kClusteringResolution};
+  auto instance = ciabatta::mixin<MockStorageWithCluster, ciabatta::curry<beluga::KldLimiter, state>::mixin>{
+      beluga::KldLimiterParam<state>{0, 1'200, hasher, 0.05, 3.}};
   auto output = ranges::views::generate([]() { return std::make_pair(1., 1.); }) |
                 ranges::views::intersperse(std::make_pair(2., 2.)) |
                 ranges::views::intersperse(std::make_pair(3., 3.)) | instance.take_samples() | ranges::to<std::vector>;
   ASSERT_EQ(output.size(), 135);
 }
 
 TEST(KldLimiter, TakeMinimum) {
-  auto instance =
-      ciabatta::mixin<MockStorageWithCluster, beluga::KldLimiter>{beluga::KldLimiterParam{200, 1'200, 0.05, 0.05, 3.}};
+  constexpr std::array kClusteringResolution{1., 1.};
+  auto hasher = beluga::spatial_hash<state>{kClusteringResolution};
+  auto instance = ciabatta::mixin<MockStorageWithCluster, ciabatta::curry<beluga::KldLimiter, state>::mixin>{
+      beluga::KldLimiterParam<state>{200, 1'200, hasher, 0.05, 3.}};
   auto output = ranges::views::generate([]() { return std::make_pair(1., 1.); }) |
                 ranges::views::intersperse(std::make_pair(2., 2.)) |
                 ranges::views::intersperse(std::make_pair(3., 3.)) | instance.take_samples() | ranges::to<std::vector>;

beluga/test/beluga/test_spatial_hash.cpp

@@ -24,50 +24,57 @@ namespace {
 
 TEST(SpatialHash, Round) {
   using Tuple = std::tuple<double, double>;
-  auto hash1 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.3, 5.0));
-  auto hash2 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.0, 5.0));
-  auto hash3 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.0, 5.3));
-  auto hash4 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.1, 5.1));
+  constexpr std::array kClusteringResolution{1., 1.};
+  auto hasher = beluga::spatial_hash<Tuple>{kClusteringResolution};
+  auto hash1 = hasher(std::make_tuple(10.3, 5.0));
+  auto hash2 = hasher(std::make_tuple(10.0, 5.0));
+  auto hash3 = hasher(std::make_tuple(10.0, 5.3));
+  auto hash4 = hasher(std::make_tuple(10.1, 5.1));
   EXPECT_EQ(hash1, hash2);
   EXPECT_EQ(hash2, hash3);
   EXPECT_EQ(hash3, hash4);
-  auto hash5 = beluga::spatial_hash<Tuple>{}(std::make_tuple(9.1, 5.1));
-  auto hash6 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.1, 4.1));
+  auto hash5 = hasher(std::make_tuple(9.1, 5.1));
+  auto hash6 = hasher(std::make_tuple(10.1, 4.1));
   EXPECT_NE(hash1, hash5);
   EXPECT_NE(hash1, hash6);
 }
 
 TEST(SpatialHash, Resolution) {
   using Tuple = std::tuple<double, double>;
-  constexpr double kResolution = 0.1;
-  auto hash1 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.3, 5.13), kResolution);
-  auto hash2 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.33, 5.14), kResolution);
+  constexpr std::array kClusteringResolution{0.1, 0.1};
+  auto hasher = beluga::spatial_hash<Tuple>{kClusteringResolution};
+
+  auto hash1 = hasher(std::make_tuple(10.3, 5.13));
+  auto hash2 = hasher(std::make_tuple(10.33, 5.14));
   EXPECT_EQ(hash1, hash2);
-  auto hash3 = beluga::spatial_hash<Tuple>{}(std::make_tuple(10.0, 5.0), kResolution);
+  auto hash3 = hasher(std::make_tuple(10.0, 5.0));
   EXPECT_NE(hash1, hash3);
 }
 
 TEST(SpatialHash, Negative) {
   using Tuple = std::tuple<double, double>;
-  constexpr double kResolution = 0.1;
-  auto hash1 = beluga::spatial_hash<Tuple>{}(std::make_tuple(-10.3, -2.13), kResolution);
-  auto hash2 = beluga::spatial_hash<Tuple>{}(std::make_tuple(-10.27, -2.14), kResolution);
+  constexpr std::array kClusteringResolution{0.1, 0.1};
+  auto hasher = beluga::spatial_hash<Tuple>{kClusteringResolution};
+  auto hash1 = hasher(std::make_tuple(-10.3, -2.13));
+  auto hash2 = hasher(std::make_tuple(-10.27, -2.14));
   EXPECT_EQ(hash1, hash2);
-  auto hash3 = beluga::spatial_hash<Tuple>{}(std::make_tuple(-10.0, -2.0), kResolution);
+  auto hash3 = hasher(std::make_tuple(-10.0, -2.0));
   EXPECT_NE(hash1, hash3);
 }
 
 TEST(SpatialHash, NoCollisions) {
   using Tuple = std::tuple<double, double, double>;
   constexpr int kLimit = 100;
+  constexpr std::array kClusteringResolution{1., 1., 1.};
+  auto hasher = beluga::spatial_hash<Tuple>{kClusteringResolution};
 
   // Brute-force search for collisions.
   // With kLimit = 100 and a 3-element tuple, we test 8'120'601 combinations.
   auto hashes = ranges::views::cartesian_product(
                     ranges::views::closed_iota(-kLimit, kLimit), ranges::views::closed_iota(-kLimit, kLimit),
                     ranges::views::closed_iota(-kLimit, kLimit)) |
-                ranges::views::transform([](const auto& tuple) {
-                  return beluga::spatial_hash<Tuple>{}(std::make_tuple(
+                ranges::views::transform([hasher](const auto& tuple) {
+                  return hasher(std::make_tuple(
                       static_cast<double>(std::get<0>(tuple)), static_cast<double>(std::get<1>(tuple)),
                       static_cast<double>(std::get<2>(tuple))));
                 }) |
@@ -82,8 +89,12 @@ TEST(SpatialHash, NoCollisions) {
 TEST(SpatialHash, Array) {
   using Array = std::array<double, 3>;
   using Tuple = std::tuple<double, double, double>;
-  auto hash1 = beluga::spatial_hash<Array>{}({1., 2., 3.});
-  auto hash2 = beluga::spatial_hash<Tuple>{}({1., 2., 3.});
+  constexpr std::array kClusteringResolution{1., 1., 1.};
+  auto array_hasher = beluga::spatial_hash<Array>{kClusteringResolution};
+  auto tuple_hasher = beluga::spatial_hash<Tuple>{kClusteringResolution};
+
+  auto hash1 = array_hasher({1., 2., 3.});
+  auto hash2 = tuple_hasher({1., 2., 3.});
   EXPECT_EQ(hash1, hash2);
 }
 

beluga/test/benchmark/benchmark_sampling.cpp

@@ -31,9 +31,9 @@ struct State {
 
 template <>
 struct beluga::spatial_hash<State> {
-  std::size_t operator()(const State& state, double resolution = 1.) const {
-    const auto pair = std::make_tuple(state.x, state.y);
-    return beluga::spatial_hash<std::decay_t<decltype(pair)>>{}(pair, resolution);
+  std::size_t operator()(const State& state) const {
+    const auto tuple = std::make_tuple(state.x, state.y);
+    return beluga::spatial_hash<std::decay_t<decltype(tuple)>>{std::array{1., 1.}}(tuple);
   }
 };
 
@@ -88,14 +88,14 @@ void BM_AdaptiveResample(benchmark::State& state) {
 
   std::size_t min_samples = 0;
   std::size_t max_samples = container_size;
-  double resolution = 1.;
+  auto hasher = beluga::spatial_hash<State>{};
   double kld_epsilon = 0.05;
   double kld_z = 3.;
 
   for (auto _ : state) {
     auto&& samples = ranges::views::generate(beluga::random_sample(
                          beluga::views::all(container), beluga::views::weights(container), generator)) |
-                     ranges::views::transform(beluga::set_cluster(resolution)) |
+                     ranges::views::transform(beluga::set_cluster(hasher)) |
                      ranges::views::take_while(
                          beluga::kld_condition(min_samples, kld_epsilon, kld_z), beluga::cluster<const Particle&>) |
                      ranges::views::take(container_size) | ranges::views::common;

beluga/test/benchmark/benchmark_spatial_hash.cpp

@@ -23,13 +23,15 @@ namespace {
 
 void BM_Hashing(benchmark::State& state) {
   using Tuple = std::tuple<double, double, double>;
+  constexpr std::array kClusteringResolution{1., 1., 1.};
+  auto hasher = beluga::spatial_hash<Tuple>{kClusteringResolution};
 
   const auto count = state.range(0);
   state.SetComplexityN(count);
 
   for (auto _ : state) {
     auto hashes = ranges::views::generate([]() { return std::make_tuple(1., 2., 3.); }) |
-                  ranges::views::transform([](const auto& tuple) { return beluga::spatial_hash<Tuple>{}(tuple); }) |
+                  ranges::views::transform([hasher](const auto& tuple) { return hasher(tuple); }) |
                   ranges::views::take_exactly(count);
 
     auto first = std::begin(hashes);