diff --git a/beluga/include/beluga/sensor/likelihood_field_model_base.hpp b/beluga/include/beluga/sensor/likelihood_field_model_base.hpp index 5ba5fa594b..57ef040f87 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model_base.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model_base.hpp @@ -121,18 +121,26 @@ class LikelihoodFieldModelBase { Sophus::SE2d world_to_likelihood_field_transform_; /*!< Transformation from world coordinates to the likelihood field coordinate system. */ - /// Creates a likelihood field from an occupancy grid. + /// Gaussian likelihood profile: maps a squared distance-to-obstacle to a likelihood value. /** - * \param params Parameters to configure the likelihood field. - * \param grid Occupancy grid representing the static map. - * \return Likelihood field computed from the occupancy grid. + * Holds the gaussian coefficients derived from the model parameters so they are computed once + * and reused. Shared between `make_likelihood_field()` (applied over every grid cell) and + * derived models that need the likelihood value at a specific distance (see + * beluga::LikelihoodFieldProbModel). */ - 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()); - }; + struct LikelihoodProfile { + double amplitude; ///< Peak contribution of the obstacle-hit gaussian. + double two_squared_sigma; ///< 2 * sigma_hit^2, the gaussian denominator. + double offset; ///< Constant contribution of random perception noise. + + /// Returns the likelihood value for a given squared distance to the nearest obstacle. + [[nodiscard]] double operator()(double squared_distance) const { + return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset; + } + }; - /// Pre-computed variables + /// Builds the likelihood profile (gaussian coefficients) from the model parameters. + [[nodiscard]] static LikelihoodProfile make_likelihood_profile(const param_type& params) { const double two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit; assert(two_squared_sigma > 0.0); @@ -141,10 +149,22 @@ class LikelihoodFieldModelBase { 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 LikelihoodProfile{amplitude, two_squared_sigma, offset}; + } + + /// Creates a likelihood field from an occupancy grid. + /** + * \param params Parameters to configure the likelihood field. + * \param grid Occupancy grid representing the static map. + * \return Likelihood field computed from the occupancy grid. + */ + 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 profile = make_likelihood_profile(params); + const auto neighborhood = [&grid](std::size_t index) { return grid.neighborhood4(index); }; const auto squared_max_distance = static_cast(params.max_obstacle_distance * params.max_obstacle_distance); @@ -160,7 +180,7 @@ class LikelihoodFieldModelBase { if (params.model_unknown_space) { const auto inverse_max_distance = 1 / params.max_laser_distance; const auto squared_background_distance = - -two_squared_sigma * std::log((inverse_max_distance - offset) / amplitude); + -profile.two_squared_sigma * std::log((inverse_max_distance - profile.offset) / profile.amplitude); const auto get_effective_unknown_value = [only_obstacle_boundaries = params.only_obstacle_boundaries](auto&& tuple) { @@ -179,7 +199,7 @@ class LikelihoodFieldModelBase { } auto likelihood_values = std::move(distance_map) | // - ranges::actions::transform(to_likelihood); + ranges::actions::transform(profile); return ValueGrid2{std::move(likelihood_values), grid.width(), grid.resolution()}; } diff --git a/beluga/include/beluga/sensor/likelihood_field_prob_model.hpp b/beluga/include/beluga/sensor/likelihood_field_prob_model.hpp index 307883c4ff..2c9cd61e25 100644 --- a/beluga/include/beluga/sensor/likelihood_field_prob_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_prob_model.hpp @@ -18,9 +18,13 @@ #include #include #include +#include +#include #include #include +#include + /** * \file * \brief Implementation of a likelihood field prob sensor model for range finders. @@ -28,11 +32,26 @@ namespace beluga { -/// Parameters used to construct a LikelihoodFieldProbModelParam instance. +/// Parameters used to construct a LikelihoodFieldProbModel instance. /** * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. + * + * Inherits the likelihood field parameters from beluga::LikelihoodFieldModelBaseParam and + * adds the fields configuring the optional beam skipping heuristic, which detects beams that + * disagree with the map across a large fraction of the particle set (e.g. caused by + * unmapped/dynamic obstacles) and excludes them from the weight computation. Beam skipping is + * disabled by default; see https://github.com/Ekumen-OS/beluga/issues/187 for context. */ -using LikelihoodFieldProbModelParam = LikelihoodFieldModelBaseParam; +struct LikelihoodFieldProbModelParam : public LikelihoodFieldModelBaseParam { + /// Whether to enable the beam skipping heuristic. + bool do_beamskip = false; + /// Distance to the nearest obstacle below which a beam is considered to agree with the map. + double beam_skip_distance = 0.5; + /// Fraction of particles that must agree on a beam for it to be kept. + double beam_skip_threshold = 0.3; + /// If the fraction of skipped beams exceeds this value, skipping is disabled for the update. + double beam_skip_error_threshold = 0.9; +}; /// Likelihood field prob sensor model for range finders. /** @@ -54,10 +73,104 @@ class LikelihoodFieldProbModel : public LikelihoodFieldModelBase /// Constructs a LikelihoodFieldProbModel instance. /** - * @copydoc LikelihoodFieldModelBase::LikelihoodFieldModelBase + * \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) - : LikelihoodFieldModelBase(params, grid) {} + : LikelihoodFieldModelBase(params, grid), + do_beamskip_{params.do_beamskip}, + beam_skip_threshold_{params.beam_skip_threshold}, + beam_skip_error_threshold_{params.beam_skip_error_threshold}, + likelihood_threshold_{compute_likelihood_threshold(params)} {} + + /// Precomputes the beam skipping mask from the current particle states. + /** + * Runs the first pass of the beam skipping heuristic: for every beam, it counts the + * fraction of particle states for which the beam endpoint lands close enough to a mapped + * obstacle (within `beam_skip_distance`, expressed here as a likelihood threshold). Beams + * for which that fraction falls below `beam_skip_threshold` are masked out and ignored by + * the subsequent weight computation in `operator()`. If skipping would discard more than + * `beam_skip_error_threshold` of the beams, the mask is reset so that all beams are used, + * preventing filter divergence. This is a no-op when beam skipping is disabled. + * + * Must be called once per update, after motion propagation and before reweighting. + * + * \tparam StateRange A range of particle states (Sophus::SE2d). + * \param points 2D lidar hit points in the reference frame of particle states. + * \param states Range with the (propagated) particle states for the current update. + */ + template + void prepare(const measurement_type& points, StateRange&& states) { + if (!do_beamskip_) { + return; + } + + const std::size_t num_beams = points.size(); + beam_mask_.assign(num_beams, std::uint8_t{1}); + if (num_beams == 0) { + return; + } + + const auto unknown_space_occupancy_prob = static_cast(1. / this->params_.max_laser_distance); + std::vector obs_count(num_beams, 0); + std::size_t num_states = 0; + + for (const auto& state : states) { + ++num_states; + 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(); + for (std::size_t i = 0; i < num_beams; ++i) { + const auto& point = points[i]; + // 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 = this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob); + // The likelihood field is monotonically decreasing in the distance to the nearest + // obstacle, so "distance < beam_skip_distance" is equivalent to "pz > threshold". + if (pz > likelihood_threshold_) { + ++obs_count[i]; + } + } + } + + if (num_states == 0) { + return; + } + + std::size_t skipped = 0; + for (std::size_t i = 0; i < num_beams; ++i) { + const double ratio = static_cast(obs_count[i]) / static_cast(num_states); + beam_mask_[i] = static_cast(ratio > beam_skip_threshold_); + if (!beam_mask_[i]) { + ++skipped; + } + } + + // Safety fallback: if too many beams would be skipped, integrate all of them instead. + const double skipped_ratio = static_cast(skipped) / static_cast(num_beams); + if (skipped_ratio > beam_skip_error_threshold_) { + std::fill(beam_mask_.begin(), beam_mask_.end(), std::uint8_t{1}); + } + } + + /// Returns the current beam skipping mask (one flag per beam, non-zero means the beam is used). + /** + * The mask is populated by `prepare()`. It is empty until the first call, which is + * equivalent to using every beam. Mainly useful for introspection and testing. + * + * A `std::vector` is used instead of `std::vector` to avoid the bit-packed + * specialization: the mask is read once per beam per particle in the reweight hot path, where a + * plain byte load is faster and more cache friendly than the bit-masking `std::vector` does. + */ + [[nodiscard]] const std::vector& beam_mask() const { return beam_mask_; } /// Returns a state weighting function conditioned on 2D lidar hits. /** @@ -74,20 +187,46 @@ class LikelihoodFieldProbModel : public LikelihoodFieldModelBase 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); - })); + double log_weight = 0.0; + for (std::size_t i = 0; i < points.size(); ++i) { + // Skip beams that were masked out by prepare(). When beam skipping is disabled (or + // prepare() was never called) the mask is not consulted and every beam contributes, + // reproducing the plain likelihood field prob behavior. + if (do_beamskip_ && i < beam_mask_.size() && !beam_mask_[i]) { + continue; + } + // 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& point = points[i]; + 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)); + log_weight += std::log(pz); + } + return std::exp(log_weight); }; } + + private: + bool do_beamskip_; ///< Whether the beam skipping heuristic is enabled. + double beam_skip_threshold_; ///< Fraction of particles that must agree for a beam to be kept. + double beam_skip_error_threshold_; ///< Skipped-beam fraction above which skipping is disabled. + float likelihood_threshold_; ///< Likelihood equivalent of `beam_skip_distance`. + std::vector beam_mask_; ///< Per-beam mask computed by `prepare()` (non-zero means used). + + /// Converts `beam_skip_distance` into the equivalent likelihood field value. + /** + * Reuses the base class likelihood profile (the same gaussian used to build the likelihood + * field), so that the "distance to obstacle < beam_skip_distance" agreement test can be + * evaluated directly on the precomputed likelihood field without keeping the distance map around. + */ + static float compute_likelihood_threshold(const param_type& params) { + const auto profile = LikelihoodFieldModelBase::make_likelihood_profile(params); + const double squared_distance = params.beam_skip_distance * params.beam_skip_distance; + return static_cast(profile(squared_distance)); + } }; } // namespace beluga diff --git a/beluga/include/beluga/sensor/primitives.hpp b/beluga/include/beluga/sensor/primitives.hpp index 71dd99c13b..eca9864504 100644 --- a/beluga/include/beluga/sensor/primitives.hpp +++ b/beluga/include/beluga/sensor/primitives.hpp @@ -15,6 +15,10 @@ #ifndef BELUGA_SENSOR_PRIMITIVES_HPP #define BELUGA_SENSOR_PRIMITIVES_HPP +#include +#include +#include + // /** // * \file // * \brief Implementation of sensor primitives to abstract member access. @@ -37,6 +41,26 @@ struct has_likelihood_field().likelihood template inline constexpr bool has_likelihood_field_v = has_likelihood_field::value; +// Primary template which defaults to `false_type`. +// A specialization will override this if the method is detected. +template +struct has_beam_skip : std::false_type {}; + +// Specialization. Uses SFINAE to detect whether a sensor model exposes a +// `prepare(measurement, states)` method, used to precompute a beam skipping mask over the +// whole particle set before per-particle reweighting. The states range is probed with a +// `std::vector`; any range type is accepted by the actual templated method. +template +struct has_beam_skip< + T, + std::void_t().prepare( + std::declval(), + std::declval&>()))>> : std::true_type {}; + +/// Trait variable that indicates whether a type `T` supports beam skipping via `prepare()`. +template +inline constexpr bool has_beam_skip_v = has_beam_skip::value; + } // namespace beluga #endif // BELUGA_SENSOR_PRIMITIVES_HPP 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 3f43dfcb36..2c19f62b9e 100644 --- a/beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_field_prob_model.cpp @@ -43,7 +43,7 @@ TEST(LikelihoodFieldProbModel, ImportanceWeight) { kResolution}; // clang-format on - const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}}; auto sensor_model = UUT{params, grid}; { @@ -86,7 +86,7 @@ TEST(LikelihoodFieldProbModel, GridWithOffset) { 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}; + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}}; auto sensor_model = UUT{params, grid}; { @@ -113,7 +113,7 @@ TEST(LikelihoodFieldProbModel, GridWithRotation) { 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}; + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}}; auto sensor_model = UUT{params, grid}; { @@ -143,7 +143,7 @@ TEST(LikelihoodFieldProbModel, GridWithRotationAndOffset) { origin}; // clang-format on - const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}}; auto sensor_model = UUT{params, grid}; { @@ -171,7 +171,7 @@ TEST(LikelihoodFieldProbModel, GridUpdates) { kResolution, origin}; // clang-format on - const auto params = beluga::LikelihoodFieldProbModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}}; auto sensor_model = UUT{params, std::move(grid)}; { @@ -196,4 +196,151 @@ TEST(LikelihoodFieldProbModel, GridUpdates) { } } +// Builds the standard 5x5 grid used by the beam skipping tests: a single obstacle at the center +// cell, so that the beam {1.25, 1.25} lands exactly on it (pz ~ 1.022) and beams further away +// floor at pz = z_random / max_laser_distance = 0.025. +StaticOccupancyGrid<5, 5> make_beamskip_grid() { + constexpr double kResolution = 0.5; + // clang-format off + return 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 +} + +TEST(LikelihoodFieldProbModelBeamSkip, DisabledMatchesBaseline) { + const auto grid = make_beamskip_grid(); + // do_beamskip defaults to false when omitted from the aggregate initializer. + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}}; + auto sensor_model = UUT{params, grid}; + + // prepare() is a no-op while skipping is disabled: the mask stays empty and weights are unchanged. + const auto points = std::vector>{{1.25, 1.25}, {2.25, 2.25}}; + sensor_model.prepare(points, std::vector(10, grid.origin())); + EXPECT_TRUE(sensor_model.beam_mask().empty()); + + auto state_weighting_function = sensor_model(std::vector>{points}); + // Both beams contribute: 1.022 (obstacle) * 0.025 (floor). + ASSERT_NEAR(1.022 * 0.025, state_weighting_function(grid.origin()), 0.003 * 0.025); +} + +TEST(LikelihoodFieldProbModelBeamSkip, ExcludesDivergentBeam) { + const auto grid = make_beamskip_grid(); + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}, true, 0.5, 0.3, 0.9}; + auto sensor_model = UUT{params, grid}; + + // One beam hits the obstacle (agrees with the map), the other consistently misses it + // (simulated dynamic obstacle). + const auto points = std::vector>{{1.25, 1.25}, {2.25, 2.25}}; + sensor_model.prepare(points, std::vector(10, grid.origin())); + + ASSERT_EQ(sensor_model.beam_mask().size(), 2U); + EXPECT_TRUE(sensor_model.beam_mask()[0]); // obstacle beam is kept + EXPECT_FALSE(sensor_model.beam_mask()[1]); // divergent beam is skipped + + // With the divergent beam skipped, only the obstacle beam contributes (~1.022), which is higher + // than the full product 1.022 * 0.025 the model would yield without skipping. + auto state_weighting_function = sensor_model(std::vector>{points}); + ASSERT_NEAR(1.022, state_weighting_function(grid.origin()), 0.003); +} + +TEST(LikelihoodFieldProbModelBeamSkip, KeepsAgreedBeam) { + const auto grid = make_beamskip_grid(); + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}, true, 0.5, 0.3, 0.9}; + auto sensor_model = UUT{params, grid}; + + // 7 of 10 particles agree on the obstacle beam (0.7 > beam_skip_threshold), so it is kept. + auto states = std::vector(7, grid.origin()); + states.resize(10, Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{10., 10.}}); // 3 particles miss + sensor_model.prepare(std::vector>{{1.25, 1.25}}, states); + + ASSERT_EQ(sensor_model.beam_mask().size(), 1U); + EXPECT_TRUE(sensor_model.beam_mask()[0]); +} + +TEST(LikelihoodFieldProbModelBeamSkip, ThresholdBoundary) { + const auto grid = make_beamskip_grid(); + // beam_skip_error_threshold = 1.0 so the single-beam case is decided purely by beam_skip_threshold + // and never triggers the all-beams-skipped fallback. + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}, true, 0.5, 0.3, 1.0}; + auto sensor_model = UUT{params, grid}; + + const auto agreeing = grid.origin(); + const auto missing = Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{10., 10.}}; + const auto points = std::vector>{{1.25, 1.25}}; + + // 4/10 = 0.4 > 0.3 -> beam is kept. + { + auto states = std::vector(4, agreeing); + states.resize(10, missing); + sensor_model.prepare(points, states); + ASSERT_EQ(sensor_model.beam_mask().size(), 1U); + EXPECT_TRUE(sensor_model.beam_mask()[0]); + } + + // 2/10 = 0.2 < 0.3 -> beam is skipped. + { + auto states = std::vector(2, agreeing); + states.resize(10, missing); + sensor_model.prepare(points, states); + ASSERT_EQ(sensor_model.beam_mask().size(), 1U); + EXPECT_FALSE(sensor_model.beam_mask()[0]); + } +} + +TEST(LikelihoodFieldProbModelBeamSkip, ErrorThresholdFallback) { + const auto grid = make_beamskip_grid(); + // All beams miss the map, so all would be skipped; that exceeds beam_skip_error_threshold (0.9) + // and the heuristic falls back to using every beam. + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}, true, 0.5, 0.3, 0.9}; + auto sensor_model = UUT{params, grid}; + + const auto points = std::vector>{{2.25, 2.25}, {2.30, 2.30}, {2.35, 2.35}}; + sensor_model.prepare(points, std::vector(10, grid.origin())); + + ASSERT_EQ(sensor_model.beam_mask().size(), 3U); + EXPECT_TRUE(sensor_model.beam_mask()[0]); + EXPECT_TRUE(sensor_model.beam_mask()[1]); + EXPECT_TRUE(sensor_model.beam_mask()[2]); + + // The weight matches the model with skipping disabled (all beams integrated). + const auto baseline_params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}}; + auto baseline_model = UUT{baseline_params, grid}; + + const auto weight_with_fallback = sensor_model(std::vector>{points})(grid.origin()); + const auto weight_baseline = baseline_model(std::vector>{points})(grid.origin()); + EXPECT_NEAR(weight_with_fallback, weight_baseline, 1e-9); +} + +TEST(LikelihoodFieldProbModelBeamSkip, DistanceToLikelihoodThreshold) { + const auto grid = make_beamskip_grid(); + // Beam one cell (0.5 m) away from the obstacle, so its likelihood (~0.069) sits between the + // thresholds produced by the two beam_skip_distance values below. + const auto points = std::vector>{{1.75, 1.25}}; + const auto states = std::vector(10, grid.origin()); + // beam_skip_error_threshold = 1.0 keeps the single-beam decision tied to the distance threshold. + + // Small beam_skip_distance -> high likelihood threshold -> the beam does not agree -> skipped. + { + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}, true, 0.3, 0.3, 1.0}; + auto sensor_model = UUT{params, grid}; + sensor_model.prepare(points, states); + ASSERT_EQ(sensor_model.beam_mask().size(), 1U); + EXPECT_FALSE(sensor_model.beam_mask()[0]); + } + + // Larger beam_skip_distance -> lower likelihood threshold -> the beam agrees -> kept. + { + const auto params = beluga::LikelihoodFieldProbModelParam{{2.0, 20.0, 0.5, 0.5, 0.2}, true, 0.8, 0.3, 1.0}; + auto sensor_model = UUT{params, grid}; + sensor_model.prepare(points, states); + ASSERT_EQ(sensor_model.beam_mask().size(), 1U); + EXPECT_TRUE(sensor_model.beam_mask()[0]); + } +} + } // namespace diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 2d4bee2d65..1c645cc141 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -180,6 +180,46 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", "" declare_parameter("sigma_hit", rclcpp::ParameterValue(0.2), descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Whether to enable beam skipping, ignoring beams that disagree with the map across most " + "particles (e.g. caused by unmapped or dynamic obstacles). Only used by the " + "likelihood_field_prob model."; + declare_parameter("do_beamskip", false, descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Distance threshold for beam-skipping to consider hit a likely true static map hit"; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("beam_skip_distance", rclcpp::ParameterValue(0.5), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Agreement threshold above which beam-skipping considers a hit a static map hit"; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1; + descriptor.floating_point_range[0].step = 0; + declare_parameter("beam_skip_threshold", rclcpp::ParameterValue(0.3), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "If more than this fraction of beam disagree with the map, assume localization error and disable beam skipping."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1; + descriptor.floating_point_range[0].step = 0; + declare_parameter("beam_skip_error_threshold", rclcpp::ParameterValue(0.9), descriptor); + } + { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "If false, AMCL will use the last known pose to initialize when a new map is received."; @@ -391,6 +431,10 @@ auto AmclNode::get_sensor_model(std::string_view name, nav_msgs::msg::OccupancyG 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(); + params.do_beamskip = get_parameter("do_beamskip").as_bool(); + params.beam_skip_distance = get_parameter("beam_skip_distance").as_double(); + params.beam_skip_threshold = get_parameter("beam_skip_threshold").as_double(); + params.beam_skip_error_threshold = get_parameter("beam_skip_error_threshold").as_double(); return beluga::LikelihoodFieldProbModel{params, beluga_ros::OccupancyGrid{map}}; } if (name == kBeamSensorModelName) { diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp index fca4acea9d..817eb9eff4 100644 --- a/beluga_amcl/test/test_amcl_node.cpp +++ b/beluga_amcl/test/test_amcl_node.cpp @@ -322,6 +322,32 @@ TEST_F(TestNode, NoBroadcastWhenInitialPoseInvalid) { ASSERT_FALSE(tester_node_->can_transform("map", "odom")); } +TEST_F(TestNode, BeamSkipInitializes) { + // The beam skipping parameters must be declared/parsed and the likelihood_field_prob model + // built with skipping enabled without error. + amcl_node_->set_parameter(rclcpp::Parameter{"laser_model_type", "likelihood_field_prob"}); + amcl_node_->set_parameter(rclcpp::Parameter{"do_beamskip", true}); + amcl_node_->set_parameter(rclcpp::Parameter{"min_particles", 10}); + amcl_node_->set_parameter(rclcpp::Parameter{"max_particles", 30}); + amcl_node_->configure(); + amcl_node_->activate(); + tester_node_->publish_map(); + ASSERT_TRUE(wait_for_initialization()); +} + +TEST_F(TestNode, BeamSkipEstimatesPose) { + // Drives a full update through the two-pass prepare()->reweight() path and checks that a pose + // estimate is produced. + amcl_node_->set_parameter(rclcpp::Parameter{"laser_model_type", "likelihood_field_prob"}); + amcl_node_->set_parameter(rclcpp::Parameter{"do_beamskip", true}); + amcl_node_->configure(); + amcl_node_->activate(); + tester_node_->publish_map(); + ASSERT_TRUE(wait_for_initialization()); + tester_node_->publish_laser_scan(); + ASSERT_TRUE(wait_for_pose_estimate()); +} + TEST_F(TestNode, FirstMapOnly) { amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true}); amcl_node_->set_parameter(rclcpp::Parameter{"always_reset_initial_pose", true}); diff --git a/beluga_example/params/default.ros2.yaml b/beluga_example/params/default.ros2.yaml index e8d8762c22..791e6a1417 100644 --- a/beluga_example/params/default.ros2.yaml +++ b/beluga_example/params/default.ros2.yaml @@ -69,6 +69,16 @@ amcl: z_max: 0.05 # Standard deviation of a gaussian centered arounds obstacles. sigma_hit: 0.2 + # Whether to enable beam skipping (likelihood_field_prob only). When enabled, beams that + # disagree with the map across most particles (e.g. unmapped or dynamic obstacles) are + # ignored. Disabled by default; only beneficial in scenes with unmapped/dynamic obstacles. + do_beamskip: true + # Distance threshold for beam-skipping to consider hit a likely true static map hit. + beam_skip_distance: 0.5 + # Agreement threshold above which beam-skipping considers a hit a static map hit. + beam_skip_threshold: 0.3 + # If more than this fraction of beam disagree with the map, assume localization error and disable beam skipping + beam_skip_error_threshold: 0.9 # Whether to broadcast map to odom transform or not. tf_broadcast: true # Transform tolerance allowed. diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index c38360e642..30eaab8997 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -19,9 +19,12 @@ #include #include #include +#include +#include #include #include #include +#include namespace beluga_ros { @@ -95,8 +98,14 @@ auto Amcl::update( std::visit( [&, this](auto& policy, auto& motion_model, auto& sensor_model) { particles_ |= - beluga::actions::propagate(policy, motion_model(control_action_window_ << base_pose_in_odom)) | // - beluga::actions::reweight(policy, sensor_model(std::move(measurement))) | // + beluga::actions::propagate(policy, motion_model(control_action_window_ << base_pose_in_odom)); + // First pass: let beam-skipping sensor models analyze the propagated particle set and + // precompute which beams to ignore, before the per-particle reweight below. + if constexpr (beluga::has_beam_skip_v>) { + sensor_model.prepare(measurement, beluga::views::states(particles_)); + } + particles_ |= // + beluga::actions::reweight(policy, sensor_model(std::move(measurement))) | // beluga::actions::normalize(policy); }, execution_policy_, motion_model_, sensor_model_);