From 248aadb2596605fcab861914a542555996e04398 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Jul 2026 18:27:48 +0200 Subject: [PATCH 1/2] Fix std::normal_distribution when stdev is zero MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../AMCLLocalizer.cpp | 21 +++++++++++-------- .../AMCLLocalizer.cpp | 21 +++++++++++-------- 2 files changed, 24 insertions(+), 18 deletions(-) diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 033cc8b..12bcaca 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -242,9 +242,9 @@ AMCLLocalizer::on_initialize() RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -430,7 +430,7 @@ AMCLLocalizer::init_pose_callback( yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); std::normal_distribution standard_normal(0.0, 1.0); - std::normal_distribution yaw_noise(0.0, yaw_stddev); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); const std::size_t N = particles_.size(); @@ -546,13 +546,16 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); std::normal_distribution noise_yaw( 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); tf2::Vector3 noisy_translation( dx + noise_dx(rng_), @@ -680,7 +683,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) { diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 23729b9..62244a5 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -239,9 +239,9 @@ AMCLLocalizer::on_initialize() RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -425,7 +425,7 @@ AMCLLocalizer::init_pose_callback( yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); std::normal_distribution standard_normal(0.0, 1.0); - std::normal_distribution yaw_noise(0.0, yaw_stddev); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); const std::size_t N = particles_.size(); for (std::size_t i = 0; i < N; ++i) { @@ -503,13 +503,16 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) std::mt19937 gen(rd()); for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); std::normal_distribution noise_yaw( 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); tf2::Vector3 noisy_translation( dx + noise_dx(gen), @@ -636,7 +639,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) { From 44a788f30b6b3a623cb396cf77b6f5bcae3e39a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Jul 2026 18:59:58 +0200 Subject: [PATCH 2/2] Avoid creating objects every loop cycle MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../AMCLLocalizer.cpp | 21 +++++++++---------- .../AMCLLocalizer.cpp | 21 +++++++++---------- 2 files changed, 20 insertions(+), 22 deletions(-) diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 12bcaca..b07dfc2 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -545,18 +545,17 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) double rot_len = std::abs(yaw); - for (auto & p : particles_) { - std::normal_distribution noise_dx( - 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); - std::normal_distribution noise_dy( - 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); - std::normal_distribution noise_dz( - 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); - - std::normal_distribution noise_yaw( - 0.0, - std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( dx + noise_dx(rng_), dy + noise_dy(rng_), diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 62244a5..43c5eda 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -502,18 +502,17 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) std::random_device rd; std::mt19937 gen(rd()); - for (auto & p : particles_) { - std::normal_distribution noise_dx( - 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); - std::normal_distribution noise_dy( - 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); - std::normal_distribution noise_dz( - 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); - - std::normal_distribution noise_yaw( - 0.0, - std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( dx + noise_dx(gen), dy + noise_dy(gen),