From 15d0062d72d27026fa5c2d37126764e4fd72d054 Mon Sep 17 00:00:00 2001 From: Antonio Park Date: Mon, 18 Mar 2024 17:48:58 +0900 Subject: [PATCH] Move lines for pre-computation to outside a loop Signed-off-by: Kyungsik Park --- .../sensors/laser/likelihood_field_model.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp index 491e64ecf1..1148c2828d 100644 --- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp +++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp @@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) self = reinterpret_cast(data->laser); + // Pre-compute a couple of things + double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; + double z_rand_mult = 1.0 / data->range_max; + + step = (data->range_count - 1) / (self->max_beams_ - 1); + + // Step size must be at least 1 + if (step < 1) { + step = 1; + } + total_weight = 0.0; // Compute the sample weights @@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) p = 1.0; - // Pre-compute a couple of things - double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; - double z_rand_mult = 1.0 / data->range_max; - - step = (data->range_count - 1) / (self->max_beams_ - 1); - - // Step size must be at least 1 - if (step < 1) { - step = 1; - } - for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; obs_bearing = data->ranges[i][1];