diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 109322712..51fa41aef 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #endif diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp index 87c748149..219220cfd 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model.hpp @@ -22,14 +22,10 @@ #include #include -#include #include +#include #include -#include -#include -#include #include -#include /** * \file @@ -42,27 +38,7 @@ namespace beluga { /** * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. */ -struct LikelihoodFieldModelParam { - /// Maximum distance to obstacle. - /** - * When creating a distance map, if the distance to an obstacle is higher than the value specified here, - * then this value will be used. - */ - double max_obstacle_distance = 100.0; - /// Maximum range of a laser ray. - double max_laser_distance = 2.0; - /// Weight used to combine the probability of hitting an obstacle. - double z_hit = 0.5; - /// Weight used to combine the probability of random noise in perception. - double z_random = 0.5; - /// Standard deviation of a gaussian centered arounds obstacles. - /** - * Used to calculate the probability of the obstacle being hit. - */ - double sigma_hit = 0.2; - /// Whether to model unknown space or assume it free. - bool model_unknown_space = false; -}; +using LikelihoodFieldModelParam = LikelihoodFieldModelCommonParam; /// Likelihood field sensor model for range finders. /** @@ -79,7 +55,7 @@ struct LikelihoodFieldModelParam { * It must satisfy \ref OccupancyGrid2Page. */ template -class LikelihoodFieldModel { +class LikelihoodFieldModel : public LikelihoodFieldModelCommon { public: /// State type of a particle. using state_type = Sophus::SE2d; @@ -101,12 +77,7 @@ class LikelihoodFieldModel { * for particle states. */ explicit LikelihoodFieldModel(const param_type& params, const map_type& grid) - : params_{params}, - likelihood_field_{make_likelihood_field(params, grid)}, - world_to_likelihood_field_transform_{grid.origin().inverse()} {} - - /// Returns the likelihood field, constructed from the provided map. - [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_; } + : LikelihoodFieldModelCommon(params, grid) {} /// Returns a state weighting function conditioned on 2D lidar hits. /** @@ -116,12 +87,12 @@ class LikelihoodFieldModel { */ [[nodiscard]] auto operator()(measurement_type&& points) const { return [this, points = std::move(points)](const state_type& state) -> weight_type { - const auto transform = world_to_likelihood_field_transform_ * state; + const auto transform = this->world_to_likelihood_field_transform_ * state; const auto x_offset = transform.translation().x(); const auto y_offset = transform.translation().y(); const auto cos_theta = transform.so2().unit_complex().x(); const auto sin_theta = transform.so2().unit_complex().y(); - const auto unknown_space_occupancy_prob = static_cast(1. / params_.max_laser_distance); + const auto unknown_space_occupancy_prob = static_cast(1. / this->params_.max_laser_distance); return std::transform_reduce( points.cbegin(), points.cend(), 1.0, std::plus{}, [this, x_offset, y_offset, cos_theta, sin_theta, unknown_space_occupancy_prob](const auto& point) { @@ -131,72 +102,13 @@ class LikelihoodFieldModel { const auto x = point.first * cos_theta - point.second * sin_theta + x_offset; const auto y = point.first * sin_theta + point.second * cos_theta + y_offset; const auto pz = - static_cast(likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob)); + static_cast(this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob)); // TODO(glpuga): Investigate why AMCL and QuickMCL both use this formula for the weight. // See https://github.com/Ekumen-OS/beluga/issues/153 return pz * pz * pz; }); }; } - - /// Update the sensor model with a new occupancy grid map. - /** - * This method re-computes the underlying likelihood field. - * - * \param grid New occupancy grid representing the static map. - */ - void update_map(const map_type& grid) { - likelihood_field_ = make_likelihood_field(params_, grid); - world_to_likelihood_field_transform_ = grid.origin().inverse(); - } - - private: - param_type params_; - ValueGrid2 likelihood_field_; - Sophus::SE2d world_to_likelihood_field_transform_; - - static ValueGrid2 make_likelihood_field(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid) { - const auto squared_distance = [&grid](std::size_t first, std::size_t second) { - return static_cast((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm()); - }; - - const auto squared_max_distance = static_cast(params.max_obstacle_distance * params.max_obstacle_distance); - const auto truncate_to_max_distance = [squared_max_distance](auto squared_distance) { - return std::min(squared_distance, squared_max_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; - }; - - const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); }; - - // determine distances to obstacles and calculate likelihood values in-place - // to minimize memory usage when dealing with large maps - auto distance_map = nearest_obstacle_distance_map(grid.obstacle_mask(), squared_distance, neighborhood); - - if (params.model_unknown_space) { - const double inverse_max_distance = 1 / params.max_laser_distance; - const double background_distance = -two_squared_sigma * std::log((inverse_max_distance - offset) / amplitude); - - distance_map |= beluga::actions::overlay(grid.unknown_mask(), background_distance); - } - - auto likelihood_values = std::move(distance_map) | // - ranges::actions::transform(truncate_to_max_distance) | // - ranges::actions::transform(to_likelihood); - - return ValueGrid2{std::move(likelihood_values), grid.width(), grid.resolution()}; - } }; } // namespace beluga diff --git a/beluga/include/beluga/sensor/likelihood_field_model_common.hpp b/beluga/include/beluga/sensor/likelihood_field_model_common.hpp new file mode 100644 index 000000000..ab74c6269 --- /dev/null +++ b/beluga/include/beluga/sensor/likelihood_field_model_common.hpp @@ -0,0 +1,156 @@ +// 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. + +#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL_COMMON_HPP +#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL_COMMON_HPP + +#include +#include +#include +#include + +/** + * \file + * \brief Implementation of a likelihood field common sensor model for range finders. + */ + +namespace beluga { + +/// Parameters used to construct a LikelihoodFieldModelCommon instance. +/** + * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. + */ +struct LikelihoodFieldModelCommonParam { + /// Maximum distance to obstacle. + /** + * When creating a distance map, if the distance to an obstacle is higher than the value specified here, + * then this value will be used. + */ + double max_obstacle_distance = 100.0; + /// Maximum range of a laser ray. + double max_laser_distance = 2.0; + /// Weight used to combine the probability of hitting an obstacle. + double z_hit = 0.5; + /// Weight used to combine the probability of random noise in perception. + double z_random = 0.5; + /// Standard deviation of a gaussian centered arounds obstacles. + /** + * Used to calculate the probability of the obstacle being hit. + */ + double sigma_hit = 0.2; + /// Whether to model unknown space or assume it free. + bool model_unknown_space = false; +}; + +/// Likelihood field common sensor model for range finders. +/** + * This model relies on a pre-computed likelihood map of the environment. + * It is less computationally intensive than the beluga::BeamSensorModel + * because no ray-tracing is required, and it can also provide better + * performance in environments with non-smooth occupation maps. See + * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4, + * for further reference. + * + * \note This class satisfies \ref SensorModelPage. + * + * \tparam OccupancyGrid Type representing an occupancy grid. + * It must satisfy \ref OccupancyGrid2Page. + */ +template +class LikelihoodFieldModelCommon { + public: + /// Map representation type. + using map_type = OccupancyGrid; + /// Parameter type that the constructor uses to configure the likelihood field model. + using param_type = LikelihoodFieldModelCommonParam; + + /// Constructs a LikelihoodFieldCommonModel instance. + /** + * \param params Parameters to configure this instance. + * See beluga::LikelihoodFieldModelCommon for details. + * \param grid Occupancy grid representing the static map that the sensor model + * uses to compute a likelihood field for lidar hits and compute importance weights + * for particle states. + */ + explicit LikelihoodFieldModelCommon(const param_type& params, const map_type& grid) + : params_{params}, + likelihood_field_{make_likelihood_field(params, grid)}, + world_to_likelihood_field_transform_{grid.origin().inverse()} {} + + /// Returns the likelihood field, constructed from the provided map. + [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_; } + + /// Update the sensor model with a new occupancy grid map. + /** + * This method re-computes the underlying likelihood field. + * + * \param grid New occupancy grid representing the static map. + */ + void update_map(const map_type& grid) { + likelihood_field_ = make_likelihood_field(params_, grid); + world_to_likelihood_field_transform_ = grid.origin().inverse(); + } + + protected: + param_type params_; + ValueGrid2 likelihood_field_; + Sophus::SE2d world_to_likelihood_field_transform_; + + 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) { + return static_cast((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm()); + }; + + const auto squared_max_distance = static_cast(params.max_obstacle_distance * params.max_obstacle_distance); + const auto truncate_to_max_distance = [squared_max_distance](auto squared_distance) { + return std::min(squared_distance, squared_max_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; + }; + + const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); }; + + // determine distances to obstacles and calculate likelihood values in-place + // to minimize memory usage when dealing with large maps + auto distance_map = nearest_obstacle_distance_map(grid.obstacle_mask(), squared_distance, neighborhood); + + if (params.model_unknown_space) { + const double inverse_max_distance = 1 / params.max_laser_distance; + const double background_distance = -two_squared_sigma * std::log((inverse_max_distance - offset) / amplitude); + + distance_map |= beluga::actions::overlay(grid.unknown_mask(), background_distance); + } + + auto likelihood_values = std::move(distance_map) | // + ranges::actions::transform(truncate_to_max_distance) | // + ranges::actions::transform(to_likelihood); + + return ValueGrid2{std::move(likelihood_values), grid.width(), grid.resolution()}; + } +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/sensor/likelihood_field_prob_model.hpp b/beluga/include/beluga/sensor/likelihood_field_prob_model.hpp new file mode 100644 index 000000000..1058a48cc --- /dev/null +++ b/beluga/include/beluga/sensor/likelihood_field_prob_model.hpp @@ -0,0 +1,115 @@ +// 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. + +#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_PROB_MODEL_HPP +#define BELUGA_SENSOR_LIKELIHOOD_FIELD_PROB_MODEL_HPP + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +/** + * \file + * \brief Implementation of a likelihood field prob sensor model for range finders. + */ + +namespace beluga { + +/// Parameters used to construct a LikelihoodFieldProbModelParam instance. +/** + * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. + */ +using LikelihoodFieldProbModelParam = LikelihoodFieldModelCommonParam; + +/// Likelihood field prob sensor model for range finders. +/** + * This model relies on a pre-computed likelihood map of the environment. + * It is less computationally intensive than the beluga::BeamSensorModel + * because no ray-tracing is required, and it can also provide better + * performance in environments with non-smooth occupation maps. See + * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4, + * for further reference. + * + * \note This class satisfies \ref SensorModelPage. + * + * \tparam OccupancyGrid Type representing an occupancy grid. + * It must satisfy \ref OccupancyGrid2Page. + */ +template +class LikelihoodFieldProbModel : public LikelihoodFieldModelCommon { + public: + /// State type of a particle. + using state_type = Sophus::SE2d; + /// Weight type of the particle. + using weight_type = double; + /// Measurement type of the sensor: a point cloud for the range finder. + using measurement_type = std::vector>; + /// Map representation type. + using map_type = OccupancyGrid; + /// Parameter type that the constructor uses to configure the likelihood field model. + using param_type = LikelihoodFieldProbModelParam; + + /// Constructs a LikelihoodFieldProbModel instance. + /** + * \param params Parameters to configure this instance. + * See beluga::LikelihoodFieldProbModelParam for details. + * \param grid Occupancy grid representing the static map that the sensor model + * uses to compute a likelihood field for lidar hits and compute importance weights + * for particle states. + */ + explicit LikelihoodFieldProbModel(const param_type& params, const map_type& grid) + : LikelihoodFieldModelCommon(params, grid) {} + + /// Returns a state weighting function conditioned on 2D lidar hits. + /** + * \param points 2D lidar hit points in the reference frame of particle states. + * \return a state weighting function satisfying \ref StateWeightingFunctionPage + * and borrowing a reference to this sensor model (and thus their lifetime are bound). + */ + [[nodiscard]] auto operator()(measurement_type&& points) const { + return [this, points = std::move(points)](const state_type& state) -> weight_type { + const auto transform = this->world_to_likelihood_field_transform_ * state; + const auto x_offset = transform.translation().x(); + const auto y_offset = transform.translation().y(); + const auto cos_theta = transform.so2().unit_complex().x(); + const auto sin_theta = transform.so2().unit_complex().y(); + const auto unknown_space_occupancy_prob = static_cast(1. / this->params_.max_laser_distance); + + return std::exp(std::transform_reduce( + points.cbegin(), points.cend(), 0.0, std::plus{}, + [this, x_offset, y_offset, cos_theta, sin_theta, unknown_space_occupancy_prob](const auto& point) { + // Transform the end point of the laser to the grid local coordinate system. + // Not using Eigen/Sophus because they make the routine x10 slower. + // See `benchmark_likelihood_field_model.cpp` for reference. + const auto x = point.first * cos_theta - point.second * sin_theta + x_offset; + const auto y = point.first * sin_theta + point.second * cos_theta + y_offset; + const auto pz = + static_cast(this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob)); + return std::log(pz); + })); + }; + } +}; + +} // namespace beluga + +#endif diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index ece4b8a69..4d15a2404 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -54,6 +54,8 @@ add_executable( sensor/test_landmark_sensor_model.cpp sensor/test_lfm_with_unknown_space.cpp sensor/test_likelihood_field_model.cpp + sensor/test_likelihood_field_model_common.cpp + sensor/test_likelihood_field_prob_model.cpp sensor/test_ndt_model.cpp test_3d_embedding.cpp test_primitives.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..3d59f8e65 --- /dev/null +++ b/beluga/test/beluga/sensor/test_likelihood_field_model_common.cpp @@ -0,0 +1,62 @@ +// 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 new file mode 100644 index 000000000..3f43dfcb3 --- /dev/null +++ b/beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp @@ -0,0 +1,199 @@ +// 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_prob_model.hpp" +#include "beluga/test/static_occupancy_grid.hpp" + +namespace { + +using beluga::testing::StaticOccupancyGrid; + +using UUT = beluga::LikelihoodFieldProbModel>; + +TEST(LikelihoodFieldProbModel, ImportanceWeight) { + constexpr double kResolution = 0.5; + // clang-format off + const auto grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, true , false, false, + false, false, false, false, false, + false, false, false, false, false}, + kResolution}; + // clang-format on + + const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{1.25, 1.25}}); + ASSERT_NEAR(1.022, state_weighting_function(grid.origin()), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{2.25, 2.25}}); + ASSERT_NEAR(0.025, state_weighting_function(grid.origin()), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{-50.0, 50.0}}); + ASSERT_NEAR(0.050, state_weighting_function(grid.origin()), 0.003); + } + + { + auto state_weighting_function = + sensor_model(std::vector>{{1.20, 1.20}, {1.25, 1.25}, {1.30, 1.30}}); + ASSERT_NEAR(1.068, state_weighting_function(grid.origin()), 0.01); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{0.0, 0.0}}); + ASSERT_NEAR(1.022, state_weighting_function(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{1.25, 1.25}}), 0.003); + } +} + +TEST(LikelihoodFieldProbModel, GridWithOffset) { + constexpr double kResolution = 2.0; + // clang-format off + const auto grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, true }, + kResolution, + Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{-5, -5}}}; + // clang-format on + + const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{4.5, 4.5}}); + ASSERT_NEAR(1.022, state_weighting_function(Sophus::SE2d{}), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{9.5, 9.5}}); + ASSERT_NEAR(1.022, state_weighting_function(grid.origin()), 0.003); + } +} + +TEST(LikelihoodFieldProbModel, GridWithRotation) { + constexpr double kResolution = 2.0; + // clang-format off + const auto grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, true }, + kResolution, + Sophus::SE2d{Sophus::SO2d{Sophus::Constants::pi() / 2}, Eigen::Vector2d{0.0, 0.0}}}; + // clang-format on + + const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{-9.5, 9.5}}); + ASSERT_NEAR(1.022, state_weighting_function(Sophus::SE2d{}), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{9.5, 9.5}}); + ASSERT_NEAR(1.022, state_weighting_function(grid.origin()), 0.003); + } +} + +TEST(LikelihoodFieldProbModel, GridWithRotationAndOffset) { + constexpr double kResolution = 2.0; + // clang-format off + const auto origin_rotation = Sophus::SO2d{Sophus::Constants::pi() / 2}; + const auto origin = Sophus::SE2d{origin_rotation, origin_rotation * Eigen::Vector2d{-5, -5}}; + + const auto grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, true }, + kResolution, + origin}; + // clang-format on + + const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + auto sensor_model = UUT{params, grid}; + + { + auto state_weighting_function = sensor_model(std::vector>{{-4.5, 4.5}}); + ASSERT_NEAR(1.022, state_weighting_function(Sophus::SE2d{}), 0.003); + } + + { + auto state_weighting_function = sensor_model(std::vector>{{9.5, 9.5}}); + ASSERT_NEAR(1.022, state_weighting_function(grid.origin()), 0.003); + } +} + +TEST(LikelihoodFieldProbModel, GridUpdates) { + const auto origin = Sophus::SE2d{}; + + constexpr double kResolution = 0.5; + // clang-format off + auto grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, true , false, false, + false, false, false, false, false, + false, false, false, false, false}, + kResolution, origin}; + // clang-format on + + const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + auto sensor_model = UUT{params, std::move(grid)}; + + { + auto state_weighting_function = sensor_model(std::vector>{{1., 1.}}); + EXPECT_NEAR(1.0223556756973267, state_weighting_function(origin), 1e-6); + } + + // clang-format off + grid = StaticOccupancyGrid<5, 5>{{ + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, false, + false, false, false, false, true}, + kResolution, origin}; + // clang-format on + sensor_model.update_map(std::move(grid)); + + { + auto state_weighting_function = sensor_model(std::vector>{{1., 1.}}); + EXPECT_NEAR(0.025, state_weighting_function(origin), 1e-3); + } +} + +} // namespace diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 9e7ad3754..e0a917c87 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -74,6 +75,7 @@ namespace beluga_amcl { namespace { constexpr std::string_view kLikelihoodFieldModelName = "likelihood_field"; +constexpr std::string_view kLikelihoodFieldProbModelName = "likelihood_field_prob"; constexpr std::string_view kBeamSensorModelName = "beam"; } // namespace @@ -306,6 +308,15 @@ auto AmclNode::get_sensor_model(std::string_view name, nav_msgs::msg::OccupancyG params.sigma_hit = get_parameter("sigma_hit").as_double(); return beluga::LikelihoodFieldModel{params, beluga_ros::OccupancyGrid{map}}; } + if (name == kLikelihoodFieldProbModelName) { + auto params = beluga::LikelihoodFieldProbModelParam{}; + params.max_obstacle_distance = get_parameter("laser_likelihood_max_dist").as_double(); + params.max_laser_distance = get_parameter("laser_max_range").as_double(); + params.z_hit = get_parameter("z_hit").as_double(); + params.z_random = get_parameter("z_rand").as_double(); + params.sigma_hit = get_parameter("sigma_hit").as_double(); + return beluga::LikelihoodFieldProbModel{params, beluga_ros::OccupancyGrid{map}}; + } if (name == kBeamSensorModelName) { auto params = beluga::BeamModelParam{}; params.z_hit = get_parameter("z_hit").as_double(); diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 7ab81ffd4..46d59212e 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -109,7 +109,8 @@ class Amcl { /// Sensor model variant type for runtime selection support. using sensor_model_variant = std::variant< - beluga::LikelihoodFieldModel, // + beluga::LikelihoodFieldModel, // + beluga::LikelihoodFieldProbModel, // beluga::BeamSensorModel>; /// Execution policy variant type for runtime selection support. diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index 4f52f9afc..ead2b4f5f 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -18,9 +18,10 @@ #include #include #include -#include +#include #include #include +#include namespace beluga_ros { @@ -97,7 +98,7 @@ auto Amcl::update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_sc } force_update_ = false; - return beluga::estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); + return beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); } } // namespace beluga_ros