From e59b8758115f115cc256af26f8602ebb2094a98b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 16 Oct 2026 09:04:42 +0200 Subject: [PATCH] NavMap AMCL: per-particle heading noise; IMU turns, not its absolute yaw MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The prediction drew one heading noise for all the particles, so they never spread in yaw and the correction could not fix the heading: without an IMU, the odometry's turning error (large on a skid-steer robot) was never corrected. Each particle now gets its own. With an IMU, every particle took its whole orientation, yaw included. Without a magnetometer an IMU's absolute yaw is arbitrary and drifts, so this only worked in simulation. The particles now take its roll and pitch, and turn by the change of its yaw (a gyro turns far better than skid-steer odometry), keeping their own heading. Signed-off-by: Francisco Martín Rico --- localizers/easynav_navmap_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 3 + .../AMCLLocalizer.cpp | 39 ++++++--- .../tests/navmap_localizer_predict_tests.cpp | 85 +++++++++++++++++++ 4 files changed, 116 insertions(+), 13 deletions(-) diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index cd1a81b5..576beb51 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/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. | diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index 55ef72ac..280f771d 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #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 last_imu_yaw_; /// Flag indicating if the odometry has been initialized. bool initialized_odom_ = false; diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index db27d392..8a890c66 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/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::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); } } } diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp index ea01604b..5e94c461 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp +++ b/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(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 node_; std::shared_ptr 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); + } +}