diff --git a/beluga/include/beluga/sensor/likelihood_field_model_common.hpp b/beluga/include/beluga/sensor/likelihood_field_model_common.hpp index 1d1d7c896..d3b39559e 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model_common.hpp +++ b/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 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(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::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 make_likelihood_field(const param_type& params, const OccupancyGrid& grid) { const auto squared_distance = [&grid](std::size_t first, std::size_t second) { diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index e7d52268b..793e7576e 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/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 diff --git a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp index 593b8dae1..55fc2a6a6 100644 --- a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp @@ -31,34 +31,6 @@ using beluga::testing::StaticOccupancyGrid; using UUT = beluga::LikelihoodFieldModel>; -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 diff --git a/beluga/test/beluga/sensor/test_likelihood_field_model_common.cpp b/beluga/test/beluga/sensor/test_likelihood_field_model_common.cpp new file mode 100644 index 000000000..3896f006e --- /dev/null +++ b/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 +#include + +#include +#include + +#include +#include +#include + +#include "beluga/sensor/likelihood_field_model.hpp" +#include "beluga/test/static_occupancy_grid.hpp" + +namespace { + +using beluga::testing::StaticOccupancyGrid; + +using UUT = beluga::LikelihoodFieldModelCommon>; + +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 diff --git a/beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp b/beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp index 5562d33d0..3f43dfcb3 100644 --- a/beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp @@ -31,35 +31,6 @@ using beluga::testing::StaticOccupancyGrid; using UUT = beluga::LikelihoodFieldProbModel>; -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 diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index 970059297..6979793b4 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -18,7 +18,6 @@ #include #include #include -//#include #include #include #include @@ -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_)); }