add test LikelihoodFieldModelCommon

Signed-off-by: fbattocchia <[email protected]>

beluga/include/beluga/sensor/likelihood_field_model_common.hpp

@@ -86,7 +86,6 @@ class LikelihoodFieldModelCommon {
   explicit LikelihoodFieldModelCommon(const param_type& params, const map_type& grid)
       : params_{params},
         likelihood_field_{make_likelihood_field(params, grid)},
-        max_distance_likelihood_{make_likelihood_threshold(params)},
         world_to_likelihood_field_transform_{grid.origin().inverse()} {}
 
   /// Returns the likelihood field, constructed from the provided map.
@@ -106,27 +105,7 @@ class LikelihoodFieldModelCommon {
  protected:
   param_type params_;
   ValueGrid2<float> likelihood_field_;
-  double max_distance_likelihood_;  
   Sophus::SE2d world_to_likelihood_field_transform_;
-
-  static double make_likelihood_threshold(const param_type& params) {
-    const auto squared_max_distance = static_cast<float>(params.max_obstacle_distance * params.max_obstacle_distance);
-
-    /// Pre-computed variables
-    const double two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit;
-    assert(two_squared_sigma > 0.0);
-
-    const double amplitude = params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants<double>::pi()));
-    assert(amplitude > 0.0);
-
-    const double offset = params.z_random / params.max_laser_distance;
-
-    const auto to_likelihood = [amplitude, two_squared_sigma, offset](double squared_distance) {
-      return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;
-    };
-
-    return to_likelihood(squared_max_distance);
-  }
 
   static ValueGrid2<float> make_likelihood_field(const param_type& params, const OccupancyGrid& grid) {
     const auto squared_distance = [&grid](std::size_t first, std::size_t second) {

beluga/test/beluga/CMakeLists.txt

@@ -53,6 +53,7 @@ add_executable(
   sensor/test_bearing_sensor_model.cpp
   sensor/test_landmark_sensor_model.cpp
   sensor/test_lfm_with_unknown_space.cpp
+  sensor/test_likelihood_field_model_common.cpp
   sensor/test_likelihood_field_model.cpp
   sensor/test_likelihood_field_prob_model.cpp
   sensor/test_ndt_model.cpp

beluga/test/beluga/sensor/test_likelihood_field_model.cpp

@@ -31,34 +31,6 @@ using beluga::testing::StaticOccupancyGrid;
 
 using UUT = beluga::LikelihoodFieldModel<StaticOccupancyGrid<5, 5>>;
 
-TEST(LikelihoodFieldModel, LikelihoodField) {
-  constexpr double kResolution = 0.5;
-  // clang-format off
-  const auto grid = StaticOccupancyGrid<5, 5>{{
-    false, false, false, false, true ,
-    false, false, false, true , false,
-    false, false, true , false, false,
-    false, true , false, false, false,
-    true , false, false, false, false},
-    kResolution};
-
-  const double expected_likelihood_field[] = {  // NOLINT(modernize-avoid-c-arrays)
-    0.025, 0.025, 0.025, 0.069, 1.022,
-    0.025, 0.027, 0.069, 1.022, 0.069,
-    0.025, 0.069, 1.022, 0.069, 0.025,
-    0.069, 1.022, 0.069, 0.027, 0.025,
-    1.022, 0.069, 0.025, 0.025, 0.025
-  };
-  // clang-format on
-
-  const auto params = beluga::LikelihoodFieldModelParam{2.0, 20.0, 0.5, 0.5, 0.2};
-  auto sensor_model = UUT{params, grid};
-
-  ASSERT_THAT(
-      sensor_model.likelihood_field().data(),
-      testing::Pointwise(testing::DoubleNear(0.003), expected_likelihood_field));
-}
-
 TEST(LikelihoodFieldModel, ImportanceWeight) {
   constexpr double kResolution = 0.5;
   // clang-format off

beluga/test/beluga/sensor/test_likelihood_field_model_common.cpp

@@ -0,0 +1,63 @@
+// Copyright 2022-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 <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <utility>
+#include <vector>
+
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+#include <sophus/common.hpp>
+
+#include "beluga/sensor/likelihood_field_model.hpp"
+#include "beluga/test/static_occupancy_grid.hpp"
+
+namespace {
+
+using beluga::testing::StaticOccupancyGrid;
+
+using UUT = beluga::LikelihoodFieldModelCommon<StaticOccupancyGrid<5, 5>>;
+
+TEST(LikelihoodFieldModelCommon, LikelihoodField) {
+
+  constexpr double kResolution = 0.5;
+  // clang-format off
+  const auto grid = StaticOccupancyGrid<5, 5>{{
+    false, false, false, false, true ,
+    false, false, false, true , false,
+    false, false, true , false, false,
+    false, true , false, false, false,
+    true , false, false, false, false},
+    kResolution};
+
+  const double expected_likelihood_field[] = {  // NOLINT(modernize-avoid-c-arrays)
+    0.025, 0.025, 0.025, 0.069, 1.022,
+    0.025, 0.027, 0.069, 1.022, 0.069,
+    0.025, 0.069, 1.022, 0.069, 0.025,
+    0.069, 1.022, 0.069, 0.027, 0.025,
+    1.022, 0.069, 0.025, 0.025, 0.025
+  };
+  // clang-format on
+
+  const auto params = beluga::LikelihoodFieldModelCommonParam{2.0, 20.0, 0.5, 0.5, 0.2};
+  auto sensor_model = UUT{params, grid};
+
+  ASSERT_THAT(
+      sensor_model.likelihood_field().data(),
+      testing::Pointwise(testing::DoubleNear(0.003), expected_likelihood_field));
+}
+
+}  // namespace

beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp

@@ -31,35 +31,6 @@ using beluga::testing::StaticOccupancyGrid;
 
 using UUT = beluga::LikelihoodFieldProbModel<StaticOccupancyGrid<5, 5>>;
 
-TEST(LikelihoodFieldProbModel, LikelihoodField) {
-
-  constexpr double kResolution = 0.5;
-  // clang-format off
-  const auto grid = StaticOccupancyGrid<5, 5>{{
-    false, false, false, false, true ,
-    false, false, false, true , false,
-    false, false, true , false, false,
-    false, true , false, false, false,
-    true , false, false, false, false},
-    kResolution};
-
-  const double expected_likelihood_field[] = {  // NOLINT(modernize-avoid-c-arrays)
-    0.025, 0.025, 0.025, 0.069, 1.022,
-    0.025, 0.027, 0.069, 1.022, 0.069,
-    0.025, 0.069, 1.022, 0.069, 0.025,
-    0.069, 1.022, 0.069, 0.027, 0.025,
-    1.022, 0.069, 0.025, 0.025, 0.025
-  };
-  // clang-format on
-
-  const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2};
-  auto sensor_model = UUT{params, grid};
-
-  ASSERT_THAT(
-      sensor_model.likelihood_field().data(),
-      testing::Pointwise(testing::DoubleNear(0.003), expected_likelihood_field));
-}
-
 TEST(LikelihoodFieldProbModel, ImportanceWeight) {
   constexpr double kResolution = 0.5;
   // clang-format off

beluga_ros/src/amcl.cpp

@@ -18,7 +18,6 @@
 #include <beluga/actions/normalize.hpp>
 #include <beluga/actions/propagate.hpp>
 #include <beluga/actions/reweight.hpp>
-//#include <beluga/algorithm/estimation.hpp>
 #include <beluga/algorithm/cluster_based_estimation.hpp>
 #include <beluga/views/random_intersperse.hpp>
 #include <beluga/views/take_while_kld.hpp>
@@ -99,8 +98,7 @@ auto Amcl::update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_sc
   }
 
   force_update_ = false;
-  beluga::ParticleClusterizerParam parameters = { 0.50, (10 * M_PI / 180)}; 
-  return beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_), parameters);
+  return beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_));
 
 }