From d41a0551f18f91ef2f2304591a97c7eb1de6cd10 Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Mon, 1 Nov 2021 13:34:03 +0100 Subject: [PATCH] refactor: Precompute length of measurement vector --- ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp b/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp index b328a56..bc4a4f4 100644 --- a/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp +++ b/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp @@ -90,7 +90,7 @@ void GaussianLandmarkModel::sensor_update(ParticleFilter * pf, const LandmarkLis double p = 1; for (const auto & measurement : data.landmarks) { double pz_arg = std::numeric_limits::max(); - + auto measurement_length = measurement.pose.getOrigin().length(); // Only landmarks withing max_range around the laser have a chance to influence the particle // weight. We can use this to quickly prune the map with a KDTree. double max_range = measurement.pose.getOrigin().length() + max_confidence_range_; @@ -115,8 +115,7 @@ void GaussianLandmarkModel::sensor_update(ParticleFilter * pf, const LandmarkLis auto landmark_pose_LASER = laser_pose.inverseTimes(landmark.pose); // range difference - auto r_hat_diff = - landmark_pose_LASER.getOrigin().length() - measurement.pose.getOrigin().length(); + auto r_hat_diff = landmark_pose_LASER.getOrigin().length() - measurement_length; // bearring difference auto t_hat_diff = landmark_pose_LASER.getOrigin().angle(measurement.pose.getOrigin());