Extend development containers (#173)

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

beluga/include/beluga/algorithm/estimation.hpp

@@ -19,6 +19,7 @@
 #include <range/v3/range/primitives.hpp>
 #include <range/v3/view/common.hpp>
 #include <range/v3/view/repeat_n.hpp>
+#include <range/v3/view/take.hpp>
 #include <range/v3/view/transform.hpp>
 #include <sophus/se2.hpp>
 

beluga/include/beluga/algorithm/particle_filter.hpp

@@ -19,6 +19,7 @@
 #include <random>
 #include <utility>
 
+#include <range/v3/algorithm/max_element.hpp>
 #include <range/v3/view/common.hpp>
 
 /**
@@ -193,6 +194,13 @@ class BootstrapParticleFilter : public Mixin {
     if (resampling_vote_result) {
       this->self().initialize_particles(
           this->self().generate_samples_from_particles(generator_) | this->self().take_samples());
+    } else {
+      // max normalize weights to avoid unbounded growth
+      auto weights_view = this->self().weights() | ranges::views::common;
+      const auto weight_maximum_norm = *ranges::max_element(weights_view);
+      std::transform(
+          std::begin(weights_view), std::end(weights_view), std::begin(weights_view),
+          [weight_maximum_norm](auto weight) { return weight / weight_maximum_norm; });
     }
   }
 

beluga/test/beluga/algorithm/test_particle_filter.cpp

@@ -106,34 +106,32 @@ TEST(BootstrapParticleFilter, UpdateWithoutResampling) {
 
   auto expected_initial_state = 0.0;
   auto expected_final_state = motion_increment;
-  auto expected_initial_weight = 1.0;
-  auto expected_final_weight = weight_reduction_factor;
 
   EXPECT_CALL(filter, apply_motion(::testing::_)).WillRepeatedly(ReturnPointee(&expected_final_state));
   EXPECT_CALL(filter, importance_weight(::testing::_)).WillRepeatedly(Return(weight_reduction_factor));
   EXPECT_CALL(filter, do_resampling_vote()).WillRepeatedly(Return(false));
 
   for (auto iteration = 0; iteration < 5; ++iteration) {
+    // at the start of the iteration weights are max normalized
     ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_initial_state));
-    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.0));
 
-    // apply motion on all particles, particles will have updated states, but unchanged weight
+    // apply motion on all particles; particles will have updated states, but unchanged weights
     filter.sample();
     ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
-    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.0));
 
     // updating particle weights, particles will have updated weights, but unchanged state
     filter.reweight();
     ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
-    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_final_weight));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(weight_reduction_factor));
 
-    // resample, but with sampling policy preventing decimation
+    // resample, but with sampling policy preventing decimation. Particle weights will be renormalized.
     filter.resample();
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.0));
 
     expected_initial_state = expected_final_state;
-    expected_initial_weight = expected_final_weight;
     expected_final_state += motion_increment;
-    expected_final_weight *= weight_reduction_factor;
   }
 }
 
@@ -145,29 +143,29 @@ TEST(BootstrapParticleFilter, UpdateWithResampling) {
 
   auto expected_initial_state = 0.0;
   auto expected_final_state = motion_increment;
-  const auto expected_initial_weight = 1.0;
-  const auto expected_final_weight = weight_reduction_factor;
 
   EXPECT_CALL(filter, apply_motion(::testing::_)).WillRepeatedly(ReturnPointee(&expected_final_state));
   EXPECT_CALL(filter, importance_weight(::testing::_)).WillRepeatedly(Return(weight_reduction_factor));
   EXPECT_CALL(filter, do_resampling_vote()).WillRepeatedly(Return(true));
 
   for (auto iteration = 0; iteration < 4; ++iteration) {
+    // at the start of the iteration weights are max normalized
     ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_initial_state));
-    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.0));
 
     // apply motion on all particles, particles will have updated states, but unchanged weight
     filter.sample();
     ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
-    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.0));
 
     // updating particle weights, particles will have updated weights, but unchanged state
     filter.reweight();
     ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
-    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_final_weight));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(weight_reduction_factor));
 
-    // resample, resampling will reset weights
+    // resample, resampling will reset to 1.0 (therefore keeping them max normalized)
     filter.resample();
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.0));
 
     expected_initial_state = expected_final_state;
     expected_final_state += motion_increment;