fixup! Add new resampling policies

beluga/include/beluga/algorithm/resampling_rate_policies/selective_resampling_policy.hpp

@@ -17,6 +17,9 @@
 
 #include <utility>
 
+#include <range/v3/numeric/accumulate.hpp>
+#include <range/v3/view/transform.hpp>
+
 #include <ciabatta/ciabatta.hpp>
 
 namespace beluga {
@@ -27,9 +30,11 @@ struct SelectiveResamplingPolicy : public Mixin {
   template <class... Args>
   explicit SelectiveResamplingPolicy(Args&&... rest) : Mixin(std::forward<Args>(rest)...) {}
 
-  [[nodiscard]] bool romp_do_resampling() const {
-    // TODO(unknown): missing implementation
-    return true;
+  [[nodiscard]] bool srp_do_resampling() const {
+    const auto n_eff =
+        ranges::accumulate(this->self().weights() | ranges::views::transform([](const auto w) { return w * w; }), 0.);
+    const auto n = static_cast<double>(std::size(this->self().weights()));
+    return n_eff < n / 2.;
   }
 };
 

beluga/include/beluga/motion/differential_drive_model.hpp

@@ -146,7 +146,6 @@ class DifferentialDriveModel : public Mixin {
     last_pose_ = pose;
 
     // TODO(unknown): this logic should not be in the differential drive model, since it's shared by other models.
-    // notify observers about the pose update
     this->self().romp_update_motion(pose);
   }
 

beluga/include/beluga/runtime_dispatch/runtime_dispatch.hpp

@@ -42,7 +42,7 @@ struct RuntimeDispatch : public Mixin {
     voters_.push_back([this] { return this->self().romp_do_resampling(); });  // resample-on-move policy
 
     if (configuration_.selective_resampling) {
-      voters_.push_back([this] { return this->self().srp_do_resample(); });  // selective resampling policy
+      voters_.push_back([this] { return this->self().srp_do_resampling(); });  // selective resampling policy
     }
   }
 

beluga/test/beluga/CMakeLists.txt

@@ -10,6 +10,7 @@ ament_add_gmock(test_beluga
   random/test_multivariate_normal_distribution.cpp
   resampling_rate_policies/test_resample_interval_policy.cpp
   resampling_rate_policies/test_resample_on_motion_policy.cpp
+  resampling_rate_policies/test_selective_resampling_policy.cpp
   runtime_dispatch/test_runtime_dispatch.cpp
   sensor/test_likelihood_field_model.cpp
   test_tuple_vector.cpp

beluga/test/beluga/motion/test_differential_drive_model.cpp

@@ -34,13 +34,15 @@ class MockMixin : public ciabatta::mixin<MockMixin<Mixin>, Mixin> {
  public:
   using ciabatta::mixin<MockMixin<Mixin>, Mixin>::mixin;
 
-  void romp_update_motion([[maybe_unused]] const Sophus::SE2d& current_pose) {}
+  MOCK_METHOD(void, romp_update_motion, (const Sophus::SE2d&));
 };
 
 class DifferentialDriveModelTest : public ::testing::Test {
  protected:
   MockMixin<beluga::DifferentialDriveModel> mixin_{
       beluga::DifferentialDriveModelParam{0.0, 0.0, 0.0, 0.0}};  // No variance
+
+  void SetUp() override { EXPECT_CALL(mixin_, romp_update_motion(::testing::_)).WillRepeatedly(::testing::Return()); }
 };
 
 TEST_F(DifferentialDriveModelTest, NoUpdate) {

beluga/test/beluga/resampling_rate_policies/test_resample_interval_policy.cpp

@@ -14,8 +14,6 @@
 
 #include <gtest/gtest.h>
 
-#include <functional>
-
 #include <ciabatta/ciabatta.hpp>
 
 #include <beluga/algorithm/resampling_rate_policies/resample_interval_policy.hpp>

beluga/test/beluga/resampling_rate_policies/test_selective_resampling_policy.cpp

@@ -0,0 +1,75 @@
+// Copyright 2023 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 <gtest/gtest.h>
+
+#include <utility>
+#include <vector>
+
+#include <ciabatta/ciabatta.hpp>
+#include <range/v3/view/all.hpp>
+#include <range/v3/view/const.hpp>
+
+#include <beluga/algorithm/resampling_rate_policies/selective_resampling_policy.hpp>
+
+namespace beluga {
+
+namespace {
+
+template <template <class> class Mixin>
+class MockMixin : public ciabatta::mixin<MockMixin<Mixin>, Mixin> {
+ public:
+  using ciabatta::mixin<MockMixin<Mixin>, Mixin>::mixin;
+  std::vector<double> weights_;
+
+  using view_t = decltype(ranges::views::all(weights_));
+
+  [[nodiscard]] auto weights() const { return ranges::views::all(weights_) | ranges::views::const_; }
+
+  void set_weights(std::vector<double> v) { weights_ = std::move(v); }
+
+ private:
+};
+
+struct SelectiveResamplingPolicyTests : public ::testing::Test {};
+
+TEST_F(SelectiveResamplingPolicyTests, Construction) {
+  // UUT does not blow up during contruction
+  [[maybe_unused]] MockMixin<beluga::SelectiveResamplingPolicy> uut{};
+}
+
+TEST_F(SelectiveResamplingPolicyTests, ResampleEveryNthIteration) {
+  // uut allows resampling every N-th iteration, starting on the N-th one
+  MockMixin<beluga::SelectiveResamplingPolicy> uut{};
+
+  // very low n_eff, do resampling
+  uut.set_weights({0., 0., 0., 0., 0.});
+  ASSERT_TRUE(uut.srp_do_resampling());
+
+  // very high n_eff, don't do resampling
+  uut.set_weights({1., 1., 1., 1., 1.});
+  ASSERT_FALSE(uut.srp_do_resampling());
+
+  // n_eff slightly below N/2, do resampling
+  uut.set_weights({0.707, 0.707, 0.707, 0.707, 0.6});
+  ASSERT_TRUE(uut.srp_do_resampling());
+
+  // n_eff slightly above N/2, don't do resampling
+  uut.set_weights({0.707, 0.707, 0.707, 0.707, 0.8});
+  ASSERT_FALSE(uut.srp_do_resampling());
+}
+
+}  // namespace
+
+}  // namespace beluga

beluga/test/beluga/runtime_dispatch/test_runtime_dispatch.cpp

@@ -36,14 +36,14 @@ class MockMixin : public ciabatta::mixin<MockMixin<Mixin>, Mixin> {
 
   MOCK_METHOD(bool, romp_do_resampling, ());
   MOCK_METHOD(bool, rip_do_resampling, ());
-  MOCK_METHOD(bool, srp_do_resample, ());
+  MOCK_METHOD(bool, srp_do_resampling, ());
 };
 
 struct RuntimeDispatchTests : public ::testing::Test {};
 
 TEST_F(RuntimeDispatchTests, Construction) {
   // uut does not blow up during contruction
-  [[maybe_unused]] MockMixin<beluga::RuntimeDispatch> uut{RuntimeDispatchParam{}};
+  MockMixin<beluga::RuntimeDispatch> uut{RuntimeDispatchParam{}};
   uut.initialize();
 }
 
@@ -61,7 +61,7 @@ TEST_F(RuntimeDispatchTests, ResamplingVoteWithSelectiveResamplingOff) {
 
   EXPECT_CALL(uut, romp_do_resampling()).WillRepeatedly(Invoke([&romp_value]() { return romp_value; }));
   EXPECT_CALL(uut, rip_do_resampling()).WillRepeatedly(Invoke([&rip_value]() { return rip_value; }));
-  EXPECT_CALL(uut, srp_do_resample()).Times(0);
+  EXPECT_CALL(uut, srp_do_resampling()).Times(0);
 
   const auto test_tuples = {
       std::make_tuple(false, false, false),
@@ -77,6 +77,37 @@ TEST_F(RuntimeDispatchTests, ResamplingVoteWithSelectiveResamplingOff) {
   }
 }
 
+TEST_F(RuntimeDispatchTests, ResamplingVoteWithSelectiveResamplingOn) {
+  // Tests that only when all resampling policies agree on doing resampling
+  // the vote is positive. This test is for the case with selective resampling on.
+  RuntimeDispatchParam config;
+  config.selective_resampling = true;
+  MockMixin<beluga::RuntimeDispatch> uut{config};
+
+  uut.initialize();
+
+  bool romp_value;
+  bool rip_value;
+  bool srp_value;
+
+  EXPECT_CALL(uut, romp_do_resampling()).WillRepeatedly(Invoke([&romp_value]() { return romp_value; }));
+  EXPECT_CALL(uut, rip_do_resampling()).WillRepeatedly(Invoke([&rip_value]() { return rip_value; }));
+  EXPECT_CALL(uut, srp_do_resampling()).WillRepeatedly(Invoke([&srp_value]() { return srp_value; }));
+
+  const auto test_tuples = {
+      std::make_tuple(false, false, false, false), std::make_tuple(true, false, false, false),
+      std::make_tuple(false, true, false, false),  std::make_tuple(true, true, false, false),
+      std::make_tuple(false, false, true, false),  std::make_tuple(true, false, true, false),
+      std::make_tuple(false, true, true, false),   std::make_tuple(true, true, true, true),
+  };
+
+  for (const auto& test_values : test_tuples) {
+    bool vote_value;
+    std::tie(romp_value, rip_value, srp_value, vote_value) = test_values;
+    ASSERT_EQ(vote_value, uut.do_resampling_vote());
+  }
+}
+
 }  // namespace
 
 }  // namespace beluga

beluga_amcl/src/amcl_node.cpp

@@ -628,6 +628,7 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map)
     motion_params,
     sensor_params,
     OccupancyGrid{map});
+  particle_filter_->initialize();
 
   if (initialize_from_params) {
     Eigen::Vector3d mean;