NavMap AMCL: per-particle heading noise; IMU turns, not its absolute yaw

localizers/easynav_navmap_localizer/README.md

@@ -72,7 +72,7 @@ This package does not create service servers or clients.
 
 | Key | Type | Access | Notes |
 |---|---|---|---|
-| `imu` | `IMUPerceptions` | **Read** | Used to get the latest IMU orientation when available. |
+| `imu` | `IMUPerceptions` | **Read** | When available, the particles take its roll and pitch, and turn by the change of its yaw (not by the odometry's); its absolute yaw is never used (without a magnetometer it is arbitrary and drifts). Without it, the orientation follows the NavMap surface and the odometry's turns. |
 | `points` | `PointPerceptions` | **Read** | 3D point cloud bundles used in the correction step. |
 | `map.navmap` | `::navmap::NavMap` | **Read** | Triangle mesh map providing surface geometry and elevation. |
 | `map.bonxai` | `Bonxai::ProbabilisticMap` | **Read** | Probabilistic occupancy map used for scoring. |

localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp

@@ -23,6 +23,7 @@
 #include <random>
 #include <limits>
 #include <memory>
+#include <optional>
 
 #include "Eigen/Geometry"
 #include "bonxai/probabilistic_map.hpp"
@@ -233,6 +234,8 @@ class AMCLLocalizer : public LocalizerMethodBase
 
   /// Previous odometry transform (used to compute deltas).
   tf2::Transform last_odom_{tf2::Transform::getIdentity()};
+  /// IMU yaw at the last prediction: only its change is used.
+  std::optional<double> last_imu_yaw_;
 
   /// Flag indicating if the odometry has been initialized.
   bool initialized_odom_ = false;

localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp

@@ -736,6 +736,18 @@ void AMCLLocalizer::predict(NavState & nav_state)
   double trans_len = std::sqrt(dx * dx + dy * dy + dz * dz);
 
   double r, p, yaw; tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, yaw);
+  // The IMU: roll and pitch (gravity) are absolute; of its yaw only the change is used (a gyro
+  // turns far better than skid-steer odometry, and without a magnetometer its absolute yaw is
+  // arbitrary and drifts)
+  double imu_roll = 0.0, imu_pitch = 0.0;
+  if (imu_q_opt.has_value()) {
+    double imu_yaw;
+    tf2::Matrix3x3(*imu_q_opt).getRPY(imu_roll, imu_pitch, imu_yaw);
+    if (last_imu_yaw_.has_value()) {
+      yaw = std::remainder(imu_yaw - *last_imu_yaw_, 2.0 * M_PI);
+    }
+    last_imu_yaw_ = imu_yaw;
+  }
   double rot_len = std::abs(yaw);
 
   // Zero-mean noise; a zero deviation (e.g. the robot still) is no noise: std::normal_distribution
@@ -748,12 +760,15 @@ void AMCLLocalizer::predict(NavState & nav_state)
   const double sd_dz = std::abs(dz) * noise_translation_;
   const double sd_yaw = rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_;
 
-  double noisy_y = yaw + noise(sd_yaw);
-
   for (auto & p : particles_) {
-    tf2::Vector3 noisy_t(dx + noise(sd_dx), dy + noise(sd_dy), dz + noise(sd_dz));
-    tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_y);
-    p.pose = p.pose * tf2::Transform(noisy_q, noisy_t);
+    // Each particle its own noise: they must spread in heading too for the correction to fix it
+    double pr, pp, py; tf2::Matrix3x3(p.pose.getRotation()).getRPY(pr, pp, py);
+    const tf2::Vector3 noisy_t(dx + noise(sd_dx), dy + noise(sd_dy), dz + noise(sd_dz));
+    tf2::Vector3 Pw = p.pose * noisy_t;  // along the robot's current (tilted) heading
+    const double new_yaw = py + yaw + noise(sd_yaw);
+    tf2::Quaternion flat_q; flat_q.setRPY(0.0, 0.0, new_yaw);
+    p.pose.setOrigin(Pw);
+    p.pose.setRotation(flat_q);
 
     if (have_navmap) {
       ::navmap::NavMap::LocateOpts opts;
@@ -762,7 +777,6 @@ void AMCLLocalizer::predict(NavState & nav_state)
       }
       opts.planar_eps = 1e-4f; opts.height_eps = 0.50f; opts.use_downward_ray = true;
 
-      tf2::Vector3 Pw = p.pose.getOrigin();
       std::size_t sidx = 0; ::navmap::NavCelId cid = std::numeric_limits<uint32_t>::max();
       Eigen::Vector3f bary, hit_eig;
 
@@ -776,21 +790,22 @@ void AMCLLocalizer::predict(NavState & nav_state)
         tf2::Vector3 hit = to_tf(hit_eig);
         p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), hit.z()));
         if (imu_q_opt.has_value()) {
-          p.pose.setRotation(*imu_q_opt);
+          tf2::Quaternion q; q.setRPY(imu_roll, imu_pitch, new_yaw);
+          p.pose.setRotation(q);
         } else {
           const auto & cel = navmap.navcels[cid];
           tf2::Vector3 n_world = to_tf(cel.normal);
           double nlen = n_world.length();
           if (nlen > 1e-9) {
             n_world /= nlen;
-            double rr, pp, yy; tf2::Matrix3x3(p.pose.getRotation()).getRPY(rr, pp, yy);
-            p.pose.setRotation(frame_from_normal_and_yaw(n_world, yy));
+            if (n_world.z() < 0.0) {n_world = -n_world;}
+            p.pose.setRotation(frame_from_normal_and_yaw(n_world, new_yaw));
           }
         }
         p.last_cid = cid; p.last_surface = sidx;
-      } else {
-        p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), Pw.z()));
-        if (imu_q_opt.has_value()) {p.pose.setRotation(*imu_q_opt);}
+      } else if (imu_q_opt.has_value()) {
+        tf2::Quaternion q; q.setRPY(imu_roll, imu_pitch, new_yaw);
+        p.pose.setRotation(q);
       }
     }
   }

localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp

@@ -27,6 +27,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/utils.hpp"
 #include "easynav_common/types/NavState.hpp"
+#include "easynav_sensors/types/IMUPerception.hpp"
 #include "easynav_localizer/LocalizerNode.hpp"
 #include "easynav_navmap_localizer/AMCLLocalizer.hpp"
 
@@ -115,6 +116,34 @@ class NavmapAmclPredictTest : public ::testing::Test
     return {x / n, y / n, std::atan2(s, c)};
   }
 
+  // Spread of the particles' yaw (circular standard deviation)
+  double yaw_spread() const
+  {
+    double s = 0.0, c = 0.0;
+    for (const auto & p : amcl_->particles_) {
+      const double yaw = tf2::getYaw(p.pose.getRotation());
+      s += std::sin(yaw);
+      c += std::cos(yaw);
+    }
+    const double n = static_cast<double>(amcl_->particles_.size());
+    const double r = std::hypot(s, c) / n;
+    return std::sqrt(std::max(0.0, -2.0 * std::log(std::max(r, 1e-12))));
+  }
+
+  // An IMU reading with this orientation
+  void imu(double roll, double pitch, double yaw)
+  {
+    easynav::IMUPerception p;
+    tf2::Quaternion q;
+    q.setRPY(roll, pitch, yaw);
+    p.data.orientation.x = q.x();
+    p.data.orientation.y = q.y();
+    p.data.orientation.z = q.z();
+    p.data.orientation.w = q.w();
+    p.valid = true;
+    nav_state_.set("imu", p);
+  }
+
   std::shared_ptr<easynav::LocalizerNode> node_;
   std::shared_ptr<TestAmcl> amcl_;
   easynav::NavState nav_state_;
@@ -170,3 +199,59 @@ TEST_F(NavmapAmclPredictTest, AMixOfStillAndMovingCyclesStaysFinite)
   }
   EXPECT_TRUE(particles_finite());
 }
+
+TEST_F(NavmapAmclPredictTest, EachParticleGetsItsOwnHeadingNoise)
+{
+  // Odometry turning: the particles must spread in yaw, so the correction can choose among them
+  node_->set_parameter(rclcpp::Parameter("loc.noise_rotation", 0.3));
+  amcl_->initialize(node_, "loc");
+  const double before = yaw_spread();
+  for (int i = 0; i < 20; ++i) {
+    predict(0.0, 0.0, 0.1);
+  }
+  EXPECT_GT(yaw_spread(), 2.0 * before + 0.01);
+}
+
+TEST_F(NavmapAmclPredictTest, TheImuGivesRollPitchAndTurnsButNotItsAbsoluteYaw)
+{
+  // An IMU whose yaw is 1.2 rad off the map (no magnetometer) and tilted on a slope
+  imu(0.1, -0.2, 1.2);
+  const double yaw0 = mean()[2];
+  predict(0.0, 0.0, 0.0);
+  EXPECT_NEAR(std::remainder(mean()[2] - yaw0, 2.0 * M_PI), 0.0, 0.05);  // not snapped to 1.2
+  for (const auto & p : amcl_->particles_) {
+    double r, pi, y;
+    tf2::Matrix3x3(p.pose.getRotation()).getRPY(r, pi, y);
+    ASSERT_NEAR(r, 0.1, 1e-6);
+    ASSERT_NEAR(pi, -0.2, 1e-6);
+  }
+  // The IMU turns 0.5 rad while the (slipping) odometry says 0.1: the particles follow the IMU
+  for (int i = 1; i <= 5; ++i) {
+    imu(0.1, -0.2, 1.2 + 0.1 * i);
+    predict(0.0, 0.0, 0.02);
+  }
+  EXPECT_NEAR(std::remainder(mean()[2] - yaw0 - 0.5, 2.0 * M_PI), 0.0, 0.1);
+  // Across the IMU's +-pi wrap too
+  imu(0.0, 0.0, M_PI - 0.05);
+  predict(0.0, 0.0, 0.0);
+  const double y1 = mean()[2];
+  imu(0.0, 0.0, -M_PI + 0.05);
+  predict(0.0, 0.0, 0.0);
+  EXPECT_NEAR(std::remainder(mean()[2] - y1 - 0.1, 2.0 * M_PI), 0.0, 0.05);
+}
+
+TEST_F(NavmapAmclPredictTest, MovingOnATiltedRobotStaysOnTheSurface)
+{
+  imu(0.0, -0.3, 0.0);  // pitched up 0.3 rad (an incline the flat map does not have)
+  const auto before = mean();
+  for (int i = 0; i < 5; ++i) {
+    predict(0.1, 0.0, 0.0);
+  }
+  ASSERT_TRUE(particles_finite());
+  const auto after = mean();
+  // Along the tilted heading: about cos(0.3) of the distance in XY, z back on the map surface
+  EXPECT_NEAR(after[0] - before[0], 0.5 * std::cos(0.3), 0.1);
+  for (const auto & p : amcl_->particles_) {
+    ASSERT_NEAR(p.pose.getOrigin().z(), 0.0, 1e-3);
+  }
+}