Add localization quality estimation metric

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -36,6 +36,7 @@
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <std_srvs/srv/empty.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -153,6 +154,9 @@ class AmclNode : public BaseAMCLNode {
   /// Likelihood field publisher
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr likelihood_field_pub_;
 
+  /// Localization quality publisher
+  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float64>::SharedPtr quality_pub_;
+
   /// Global relocalization service server.
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_localization_server_;
   /// No motion update service server.

beluga_amcl/src/amcl_node.cpp

@@ -58,6 +58,7 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_field.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <std_srvs/srv/empty.hpp>
 
 #include <beluga/motion/differential_drive_model.hpp>
@@ -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<std_msgs::msg::Float64>("localization_quality", rclcpp::SystemDefaultsQoS());
+  quality_pub_->on_activate();
+
   {
     map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
         get_parameter("map_topic").as_string(), rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
@@ -311,13 +337,17 @@ 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&) {
   // Release all resources.
   particle_filter_.reset();
   enable_tf_broadcast_ = false;
   likelihood_field_pub_.reset();
+  quality_pub_.reset();
 }
 
 auto AmclNode::get_initial_estimate() const -> std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> {
@@ -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::Amcl>(
       beluga_ros::OccupancyGrid{map},                                        //
@@ -644,6 +677,10 @@ void AmclNode::sensor_callback(const std::shared_ptr<const MessageT>& 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);
   }
 }
 

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<double>::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<particle_type> particles_;
 
@@ -279,6 +296,9 @@ class Amcl {
   beluga::RollingWindow<Sophus::SE2d, 2> control_action_window_;
 
   bool force_update_{true};
+  double last_quality_{1.0};
+
+  double compute_quality(const Sophus::Matrix3d& actual_covariance);
 };
 
 }  // namespace beluga_ros

beluga_ros/src/amcl.cpp

@@ -14,6 +14,7 @@
 
 #include <beluga_ros/amcl.hpp>
 
+#include <algorithm>
 #include <beluga/actions/assign.hpp>
 #include <beluga/actions/normalize.hpp>
 #include <beluga/actions/propagate.hpp>
@@ -22,6 +23,9 @@
 #include <beluga/views/random_intersperse.hpp>
 #include <beluga/views/take_while_kld.hpp>
 #include <cmath>
+#include <limits>
+#include <random>
+#include <vector>
 
 namespace beluga_ros {
 
@@ -122,7 +126,46 @@ 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<double> dx{0.0, params_.expected_pose_x_stddev};
+  std::normal_distribution<double> dy{0.0, params_.expected_pose_y_stddev};
+  std::normal_distribution<double> dyaw{0.0, params_.expected_pose_yaw_stddev};
+
+  std::vector<Sophus::SE2d> 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<double> 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<double>::epsilon()) {
+      quality = std::min(quality, ref_covariance.coeff(i, i) / actual);
+    }
+  }
+  return std::clamp(quality, 0.0, 1.0);
 }
 
 }  // namespace beluga_ros