From d72f1acf0cc2985dd30d12ea933a9679f150227d Mon Sep 17 00:00:00 2001 From: Patricio Palma Date: Tue, 19 May 2026 20:01:20 +0000 Subject: [PATCH 1/7] In reference to https://github.com/Ekumen-OS/beluga/discussions/562 Parameters for expected standard deviation x, y and yaw were added. Added compute_quality function. Signed-off-by: Patricio Palma --- beluga_ros/include/beluga_ros/amcl.hpp | 20 +++++++++++ beluga_ros/src/amcl.cpp | 46 +++++++++++++++++++++++++- 2 files changed, 65 insertions(+), 1 deletion(-) diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 46c612cb69..ded5e7cc8e 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -95,6 +95,15 @@ struct AmclParams { /// \brief Spatial resolution around the z-axis to create buckets for KLD resampling. double spatial_resolution_theta = 10 * Sophus::Constants::pi() / 180; + + /// \brief Expected standard deviation of x in a healthy particle distribution, used for quality estimation [m]. + double expected_pose_x_stddev = 0.1; + + /// \brief Expected standard deviation of y in a healthy particle distribution, used for quality estimation [m]. + double expected_pose_y_stddev = 0.1; + + /// \brief Expected standard deviation of yaw in a healthy particle distribution, used for quality estimation [rad]. + double expected_pose_yaw_stddev = 0.05; }; /// Implementation of the 2D Adaptive Monte Carlo Localization (AMCL) algorithm. @@ -262,6 +271,14 @@ class Amcl { /// Force a manual update of the particles on the next iteration of the filter. void force_update() { force_update_ = true; } + /// Returns the localization quality score from the last filter update. + /** + * The score is in [0, 1], where close to 1 means the filter covariance is within the expected + * and values approaching 0 indicate the filter may be diverging. + * Returns 1.0 before the first successful update. + */ + [[nodiscard]] double quality() const { return last_quality_; } + private: beluga::TupleVector particles_; @@ -279,6 +296,9 @@ class Amcl { beluga::RollingWindow control_action_window_; bool force_update_{true}; + double last_quality_{1.0}; + + double compute_quality(const Sophus::Matrix3d& actual_covariance); }; } // namespace beluga_ros diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index c38360e642..f6aef1fe12 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -21,7 +21,11 @@ #include #include #include +#include #include +#include +#include +#include namespace beluga_ros { @@ -122,7 +126,47 @@ auto Amcl::update( } force_update_ = false; - return beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); + auto estimate = + beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); + last_quality_ = compute_quality(estimate.second); + return estimate; +} + +double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { + const std::size_t N = params_.min_particles; + + // a known seed is required to provide the exact same reference each time quality is computed. + std::mt19937 gen{42}; + std::normal_distribution dx{0.0, params_.expected_pose_x_stddev}; + std::normal_distribution dy{0.0, params_.expected_pose_y_stddev}; + std::normal_distribution dyaw{0.0, params_.expected_pose_yaw_stddev}; + + std::vector ref_states; + ref_states.reserve(N); + for (std::size_t i = 0; i < N; ++i) { + ref_states.emplace_back(Sophus::SO2d{dyaw(gen)}, Sophus::Vector2d{dx(gen), dy(gen)}); + } + + std::visit( + [&](const auto& motion_model) { + auto sampling_fn = motion_model(control_action_window_); + for (auto& state : ref_states) { + state = sampling_fn(state, gen); + } + }, + motion_model_); + + const std::vector uniform_weights(N, 1.0); + const auto [ref_mean, ref_covariance] = beluga::estimate(ref_states, uniform_weights); + + double quality = 1.0; + for (int i = 0; i < 3; ++i) { + const double actual = actual_covariance.coeff(i, i); + if (actual > std::numeric_limits::epsilon()) { + quality = std::min(quality, ref_covariance.coeff(i, i) / actual); + } + } + return std::clamp(quality, 0.0, 1.0); } } // namespace beluga_ros From 6c839dce9fc441fdfc300f3dc163f87b4597c8cd Mon Sep 17 00:00:00 2001 From: Patricio Palma Date: Tue, 19 May 2026 20:06:50 +0000 Subject: [PATCH 2/7] In reference to https://github.com/Ekumen-OS/beluga/discussions/562 "localization_quality" publisher added. Parameter descriptors for expected stddev in x, y, and yaw have been added. Signed-off-by: Patricio Palma --- beluga_amcl/include/beluga_amcl/amcl_node.hpp | 4 ++ beluga_amcl/src/amcl_node.cpp | 37 +++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index fd553bfb19..a9186c355f 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -153,6 +154,9 @@ class AmclNode : public BaseAMCLNode { /// Likelihood field publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr likelihood_field_pub_; + /// Localization quality publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_pub_; + /// Global relocalization service server. rclcpp::Service::SharedPtr global_localization_server_; /// No motion update service server. diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 2d4bee2d65..f84761dd0a 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -201,6 +202,28 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", "" "increase resource usage and potentially degrade performance."; declare_parameter("debug", false, descriptor); } + + { + const auto defaults = beluga_ros::AmclParams{}; + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0.0; + descriptor.floating_point_range[0].to_value = 1.0; + descriptor.floating_point_range[0].step = 0.0; + + descriptor.description = + "Expected standard deviation of x in a healthy particle distribution, used for quality estimation [m]."; + declare_parameter("expected_pose_x_stddev", rclcpp::ParameterValue(defaults.expected_pose_x_stddev), descriptor); + + descriptor.description = + "Expected standard deviation of y in a healthy particle distribution, used for quality estimation [m]."; + declare_parameter("expected_pose_y_stddev", rclcpp::ParameterValue(defaults.expected_pose_y_stddev), descriptor); + + descriptor.description = + "Expected standard deviation of yaw in a healthy particle distribution, used for quality estimation [rad]."; + declare_parameter( + "expected_pose_yaw_stddev", rclcpp::ParameterValue(defaults.expected_pose_yaw_stddev), descriptor); + } } AmclNode::~AmclNode() { @@ -215,6 +238,9 @@ void AmclNode::do_activate(const rclcpp_lifecycle::State&) { likelihood_field_pub_->on_activate(); } + quality_pub_ = create_publisher("localization_quality", rclcpp::SystemDefaultsQoS()); + quality_pub_->on_activate(); + { map_sub_ = create_subscription( get_parameter("map_topic").as_string(), rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -311,6 +337,9 @@ void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) { if (likelihood_field_pub_) { likelihood_field_pub_->on_deactivate(); } + if (quality_pub_) { + quality_pub_->on_deactivate(); + } } void AmclNode::do_cleanup(const rclcpp_lifecycle::State&) { @@ -318,6 +347,7 @@ void AmclNode::do_cleanup(const rclcpp_lifecycle::State&) { particle_filter_.reset(); enable_tf_broadcast_ = false; likelihood_field_pub_.reset(); + quality_pub_.reset(); } auto AmclNode::get_initial_estimate() const -> std::optional> { @@ -423,6 +453,9 @@ auto AmclNode::make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr map) params.spatial_resolution_x = get_parameter("spatial_resolution_x").as_double(); params.spatial_resolution_y = get_parameter("spatial_resolution_y").as_double(); params.spatial_resolution_theta = get_parameter("spatial_resolution_theta").as_double(); + params.expected_pose_x_stddev = get_parameter("expected_pose_x_stddev").as_double(); + params.expected_pose_y_stddev = get_parameter("expected_pose_y_stddev").as_double(); + params.expected_pose_yaw_stddev = get_parameter("expected_pose_yaw_stddev").as_double(); return std::make_unique( beluga_ros::OccupancyGrid{map}, // @@ -644,6 +677,10 @@ void AmclNode::sensor_callback(const std::shared_ptr& sensor_msg tf2::toMsg(base_pose_in_map, message.pose.pose); tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance); pose_pub_->publish(message); + + auto quality_msg = std_msgs::msg::Float64{}; + quality_msg.data = particle_filter_->quality(); + quality_pub_->publish(quality_msg); } } From f731fc257bc69454692f8bd225d84454dbcd988f Mon Sep 17 00:00:00 2001 From: Patricio Palma Date: Wed, 20 May 2026 15:59:17 +0000 Subject: [PATCH 3/7] Fix for format error Signed-off-by: Patricio Palma --- beluga_ros/src/amcl.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index f6aef1fe12..8736d5185d 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -21,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -126,8 +126,7 @@ auto Amcl::update( } force_update_ = false; - auto estimate = - beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); + auto estimate = beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); last_quality_ = compute_quality(estimate.second); return estimate; } From ea0db5c53e2455844a276d1c6204bf2a78c254e7 Mon Sep 17 00:00:00 2001 From: Patricio Palma Date: Fri, 22 May 2026 17:47:10 -0400 Subject: [PATCH 4/7] Fix format error Signed-off-by: Patricio Palma --- beluga_ros/src/amcl.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index 8736d5185d..691743f27c 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -132,7 +132,7 @@ auto Amcl::update( } double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { - const std::size_t N = params_.min_particles; + const std::size_t n = params_.min_particles; // a known seed is required to provide the exact same reference each time quality is computed. std::mt19937 gen{42}; @@ -141,8 +141,8 @@ double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { std::normal_distribution dyaw{0.0, params_.expected_pose_yaw_stddev}; std::vector ref_states; - ref_states.reserve(N); - for (std::size_t i = 0; i < N; ++i) { + ref_states.reserve(n); + for (std::size_t i = 0; i < n; ++i) { ref_states.emplace_back(Sophus::SO2d{dyaw(gen)}, Sophus::Vector2d{dx(gen), dy(gen)}); } @@ -155,7 +155,7 @@ double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { }, motion_model_); - const std::vector uniform_weights(N, 1.0); + const std::vector uniform_weights(n, 1.0); const auto [ref_mean, ref_covariance] = beluga::estimate(ref_states, uniform_weights); double quality = 1.0; From 62117bcf771f2ac9b2937ca31c9de28ca66fe72f Mon Sep 17 00:00:00 2001 From: Patricio Palma Date: Wed, 27 May 2026 05:08:59 +0000 Subject: [PATCH 5/7] Use beluga::MultivariateNormalDistribution to generate reference states. Generate reference states on Amcl class construction. Fix parameter value range. Signed-off-by: Patricio Palma --- beluga_amcl/src/amcl_node.cpp | 2 +- beluga_ros/include/beluga_ros/amcl.hpp | 2 ++ beluga_ros/src/amcl.cpp | 31 +++++++++++++------------- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index f84761dd0a..9f1795a3de 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -208,7 +208,7 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", "" auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.floating_point_range.resize(1); descriptor.floating_point_range[0].from_value = 0.0; - descriptor.floating_point_range[0].to_value = 1.0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); descriptor.floating_point_range[0].step = 0.0; descriptor.description = diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index ded5e7cc8e..2d007a4907 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -297,6 +298,7 @@ class Amcl { bool force_update_{true}; double last_quality_{1.0}; + std::vector ref_states_; double compute_quality(const Sophus::Matrix3d& actual_covariance); }; diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index 691743f27c..ecc7b39502 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -47,6 +47,17 @@ Amcl::Amcl( if (params_.selective_resampling) { resample_policy_ = resample_policy_ && beluga::policies::on_effective_size_drop; } + + Sophus::Matrix3d covariance = Sophus::Matrix3d::Zero(); + covariance(0, 0) = params_.expected_pose_x_stddev * params_.expected_pose_x_stddev; + covariance(1, 1) = params_.expected_pose_y_stddev * params_.expected_pose_y_stddev; + covariance(2, 2) = params_.expected_pose_yaw_stddev * params_.expected_pose_yaw_stddev; + auto dist = beluga::MultivariateNormalDistribution{covariance}; + std::mt19937 gen{42}; + ref_states_.reserve(params_.min_particles); + for (std::size_t i = 0; i < params_.min_particles; ++i) { + ref_states_.emplace_back(dist(gen)); + } } void Amcl::update_map(beluga_ros::OccupancyGrid map) { @@ -132,30 +143,20 @@ auto Amcl::update( } double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { - const std::size_t n = params_.min_particles; - - // a known seed is required to provide the exact same reference each time quality is computed. - std::mt19937 gen{42}; - std::normal_distribution dx{0.0, params_.expected_pose_x_stddev}; - std::normal_distribution dy{0.0, params_.expected_pose_y_stddev}; - std::normal_distribution dyaw{0.0, params_.expected_pose_yaw_stddev}; - std::vector ref_states; - ref_states.reserve(n); - for (std::size_t i = 0; i < n; ++i) { - ref_states.emplace_back(Sophus::SO2d{dyaw(gen)}, Sophus::Vector2d{dx(gen), dy(gen)}); - } + ref_states.reserve(ref_states_.size()); + std::mt19937 gen{42}; std::visit( [&](const auto& motion_model) { auto sampling_fn = motion_model(control_action_window_); - for (auto& state : ref_states) { - state = sampling_fn(state, gen); + for (const auto& state : ref_states_) { + ref_states.push_back(sampling_fn(state, gen)); } }, motion_model_); - const std::vector uniform_weights(n, 1.0); + const std::vector uniform_weights(ref_states.size(), 1.0); const auto [ref_mean, ref_covariance] = beluga::estimate(ref_states, uniform_weights); double quality = 1.0; From 9c8d15853bf119331e7af79ca65c7461560f9d00 Mon Sep 17 00:00:00 2001 From: Patricio Palma Date: Mon, 1 Jun 2026 20:05:12 +0000 Subject: [PATCH 6/7] Provides localization quality metric in separate publishers for x, y, yaw. Signed-off-by: Patricio Palma --- beluga_amcl/include/beluga_amcl/amcl_node.hpp | 6 ++-- beluga_amcl/src/amcl_node.cpp | 31 ++++++++++++++----- beluga_ros/include/beluga_ros/amcl.hpp | 16 +++++----- beluga_ros/src/amcl.cpp | 8 ++--- 4 files changed, 41 insertions(+), 20 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index a9186c355f..87c85397dd 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -154,8 +154,10 @@ class AmclNode : public BaseAMCLNode { /// Likelihood field publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr likelihood_field_pub_; - /// Localization quality publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_pub_; + /// Per-axis localization quality publishers (x, y, yaw). + rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_x_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_y_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_yaw_pub_; /// Global relocalization service server. rclcpp::Service::SharedPtr global_localization_server_; diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 9f1795a3de..bc736d051e 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -238,8 +238,12 @@ void AmclNode::do_activate(const rclcpp_lifecycle::State&) { likelihood_field_pub_->on_activate(); } - quality_pub_ = create_publisher("localization_quality", rclcpp::SystemDefaultsQoS()); - quality_pub_->on_activate(); + quality_x_pub_ = create_publisher("localization_quality_x", rclcpp::SystemDefaultsQoS()); + quality_x_pub_->on_activate(); + quality_y_pub_ = create_publisher("localization_quality_y", rclcpp::SystemDefaultsQoS()); + quality_y_pub_->on_activate(); + quality_yaw_pub_ = create_publisher("localization_quality_yaw", rclcpp::SystemDefaultsQoS()); + quality_yaw_pub_->on_activate(); { map_sub_ = create_subscription( @@ -337,8 +341,14 @@ void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) { if (likelihood_field_pub_) { likelihood_field_pub_->on_deactivate(); } - if (quality_pub_) { - quality_pub_->on_deactivate(); + if (quality_x_pub_) { + quality_x_pub_->on_deactivate(); + } + if (quality_y_pub_) { + quality_y_pub_->on_deactivate(); + } + if (quality_yaw_pub_) { + quality_yaw_pub_->on_deactivate(); } } @@ -347,7 +357,9 @@ void AmclNode::do_cleanup(const rclcpp_lifecycle::State&) { particle_filter_.reset(); enable_tf_broadcast_ = false; likelihood_field_pub_.reset(); - quality_pub_.reset(); + quality_x_pub_.reset(); + quality_y_pub_.reset(); + quality_yaw_pub_.reset(); } auto AmclNode::get_initial_estimate() const -> std::optional> { @@ -678,9 +690,14 @@ void AmclNode::sensor_callback(const std::shared_ptr& sensor_msg tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance); pose_pub_->publish(message); + const auto quality = particle_filter_->quality(); auto quality_msg = std_msgs::msg::Float64{}; - quality_msg.data = particle_filter_->quality(); - quality_pub_->publish(quality_msg); + quality_msg.data = quality[0]; + quality_x_pub_->publish(quality_msg); + quality_msg.data = quality[1]; + quality_y_pub_->publish(quality_msg); + quality_msg.data = quality[2]; + quality_yaw_pub_->publish(quality_msg); } } diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 2d007a4907..024ba965b9 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -15,6 +15,7 @@ #ifndef BELUGA_ROS_AMCL_HPP #define BELUGA_ROS_AMCL_HPP +#include #include #include #include @@ -272,13 +273,14 @@ class Amcl { /// Force a manual update of the particles on the next iteration of the filter. void force_update() { force_update_ = true; } - /// Returns the localization quality score from the last filter update. + /// Returns the per-axis localization quality scores from the last filter update. /** - * The score is in [0, 1], where close to 1 means the filter covariance is within the expected - * and values approaching 0 indicate the filter may be diverging. - * Returns 1.0 before the first successful update. + * Each element is in [0, 1]: index 0 = x, 1 = y, 2 = yaw. + * A value close to 1 means the filter covariance for that axis is within the expected range; + * values approaching 0 indicate the filter may be diverging on that axis. + * Returns {1.0, 1.0, 1.0} before the first successful update. */ - [[nodiscard]] double quality() const { return last_quality_; } + [[nodiscard]] std::array quality() const { return last_quality_; } private: beluga::TupleVector particles_; @@ -297,10 +299,10 @@ class Amcl { beluga::RollingWindow control_action_window_; bool force_update_{true}; - double last_quality_{1.0}; + std::array last_quality_{1.0, 1.0, 1.0}; std::vector ref_states_; - double compute_quality(const Sophus::Matrix3d& actual_covariance); + std::array compute_quality(const Sophus::Matrix3d& actual_covariance); }; } // namespace beluga_ros diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index ecc7b39502..418ea9f981 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -142,7 +142,7 @@ auto Amcl::update( return estimate; } -double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { +std::array Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { std::vector ref_states; ref_states.reserve(ref_states_.size()); @@ -159,14 +159,14 @@ double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { const std::vector uniform_weights(ref_states.size(), 1.0); const auto [ref_mean, ref_covariance] = beluga::estimate(ref_states, uniform_weights); - double quality = 1.0; + std::array quality{1.0, 1.0, 1.0}; for (int i = 0; i < 3; ++i) { const double actual = actual_covariance.coeff(i, i); if (actual > std::numeric_limits::epsilon()) { - quality = std::min(quality, ref_covariance.coeff(i, i) / actual); + quality[i] = std::clamp(ref_covariance.coeff(i, i) / actual, 0.0, 1.0); } } - return std::clamp(quality, 0.0, 1.0); + return quality; } } // namespace beluga_ros From dc1d4d2c0d9a056783e5acb5f404ceaf888b8975 Mon Sep 17 00:00:00 2001 From: Patricio Palma Date: Tue, 2 Jun 2026 01:12:39 +0000 Subject: [PATCH 7/7] Remove motion_model of compute_quality(). Documentation for input parameters now states "typical stddev" for x, y and yaw. Signed-off-by: Patricio Palma --- beluga_amcl/src/amcl_node.cpp | 9 ++++--- beluga_ros/include/beluga_ros/amcl.hpp | 10 +++++--- beluga_ros/src/amcl.cpp | 35 +++++--------------------- 3 files changed, 18 insertions(+), 36 deletions(-) diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index bc736d051e..61fe8c2e59 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -212,15 +212,18 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", "" descriptor.floating_point_range[0].step = 0.0; descriptor.description = - "Expected standard deviation of x in a healthy particle distribution, used for quality estimation [m]."; + "Typical standard deviation of x in the output covariance matrix of a healthy filter, used for quality " + "estimation [m]."; declare_parameter("expected_pose_x_stddev", rclcpp::ParameterValue(defaults.expected_pose_x_stddev), descriptor); descriptor.description = - "Expected standard deviation of y in a healthy particle distribution, used for quality estimation [m]."; + "Typical standard deviation of y in the output covariance matrix of a healthy filter, used for quality " + "estimation [m]."; declare_parameter("expected_pose_y_stddev", rclcpp::ParameterValue(defaults.expected_pose_y_stddev), descriptor); descriptor.description = - "Expected standard deviation of yaw in a healthy particle distribution, used for quality estimation [rad]."; + "Typical standard deviation of yaw in the output covariance matrix of a healthy filter, used for quality " + "estimation [rad]."; declare_parameter( "expected_pose_yaw_stddev", rclcpp::ParameterValue(defaults.expected_pose_yaw_stddev), descriptor); } diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 024ba965b9..6b50936552 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -98,13 +98,16 @@ struct AmclParams { /// \brief Spatial resolution around the z-axis to create buckets for KLD resampling. double spatial_resolution_theta = 10 * Sophus::Constants::pi() / 180; - /// \brief Expected standard deviation of x in a healthy particle distribution, used for quality estimation [m]. + /// \brief Typical standard deviation of x in the output covariance matrix of a healthy filter, used for quality + /// estimation [m]. double expected_pose_x_stddev = 0.1; - /// \brief Expected standard deviation of y in a healthy particle distribution, used for quality estimation [m]. + /// \brief Typical standard deviation of y in the output covariance matrix of a healthy filter, used for quality + /// estimation [m]. double expected_pose_y_stddev = 0.1; - /// \brief Expected standard deviation of yaw in a healthy particle distribution, used for quality estimation [rad]. + /// \brief Typical standard deviation of yaw in the output covariance matrix of a healthy filter, used for quality + /// estimation [rad]. double expected_pose_yaw_stddev = 0.05; }; @@ -300,7 +303,6 @@ class Amcl { bool force_update_{true}; std::array last_quality_{1.0, 1.0, 1.0}; - std::vector ref_states_; std::array compute_quality(const Sophus::Matrix3d& actual_covariance); }; diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index 418ea9f981..85e9cc84af 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include namespace beluga_ros { @@ -47,17 +46,6 @@ Amcl::Amcl( if (params_.selective_resampling) { resample_policy_ = resample_policy_ && beluga::policies::on_effective_size_drop; } - - Sophus::Matrix3d covariance = Sophus::Matrix3d::Zero(); - covariance(0, 0) = params_.expected_pose_x_stddev * params_.expected_pose_x_stddev; - covariance(1, 1) = params_.expected_pose_y_stddev * params_.expected_pose_y_stddev; - covariance(2, 2) = params_.expected_pose_yaw_stddev * params_.expected_pose_yaw_stddev; - auto dist = beluga::MultivariateNormalDistribution{covariance}; - std::mt19937 gen{42}; - ref_states_.reserve(params_.min_particles); - for (std::size_t i = 0; i < params_.min_particles; ++i) { - ref_states_.emplace_back(dist(gen)); - } } void Amcl::update_map(beluga_ros::OccupancyGrid map) { @@ -143,27 +131,16 @@ auto Amcl::update( } std::array Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { - std::vector ref_states; - ref_states.reserve(ref_states_.size()); - - std::mt19937 gen{42}; - std::visit( - [&](const auto& motion_model) { - auto sampling_fn = motion_model(control_action_window_); - for (const auto& state : ref_states_) { - ref_states.push_back(sampling_fn(state, gen)); - } - }, - motion_model_); - - const std::vector uniform_weights(ref_states.size(), 1.0); - const auto [ref_mean, ref_covariance] = beluga::estimate(ref_states, uniform_weights); - + const std::array expected_variance = { + params_.expected_pose_x_stddev * params_.expected_pose_x_stddev, + params_.expected_pose_y_stddev * params_.expected_pose_y_stddev, + params_.expected_pose_yaw_stddev * params_.expected_pose_yaw_stddev, + }; std::array quality{1.0, 1.0, 1.0}; for (int i = 0; i < 3; ++i) { const double actual = actual_covariance.coeff(i, i); if (actual > std::numeric_limits::epsilon()) { - quality[i] = std::clamp(ref_covariance.coeff(i, i) / actual, 0.0, 1.0); + quality[i] = std::clamp(expected_variance[i] / actual, 0.0, 1.0); } } return quality;