Merge pull request #26 from cmrobotics/feat/NAV-1599/laser_importance…

…_param

Laser importance param

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -421,7 +421,7 @@ class AmclNode : public nav2_util::LifecycleNode
   double z_max_;
   double z_short_;
   double z_rand_;
-  double k_l_;
+  double laser_importance_factor_;
   double max_particle_gen_prob_ext_pose_;
   double ext_pose_search_tolerance_sec_;
   std::string scan_topic_{"scan"};

nav2_amcl/include/nav2_amcl/pf/pf.hpp

@@ -35,6 +35,12 @@
 extern "C" {
 #endif
 
+
+// laser and external pose based particle scores have different scales
+// so when external pose is used we need to multiply laser by the factor
+// to give it equal importance with external pose
+#define LASER_TO_EXTERNAL_POSE_BALANCE_FACTOR 250.0
+
 // Forward declarations
 struct _pf_t;
 struct _rtk_fig_t;
@@ -136,7 +142,6 @@ typedef struct _pf_t
 
   double ext_x, ext_y, ext_yaw;
   double cov_matrix[9], eigen_matrix[9];
-  double k_l; // constant, it is used as an data source importance factor
   double max_particle_gen_prob_ext_pose;
   int ext_pose_is_valid;
 } pf_t;
@@ -147,7 +152,7 @@ pf_t * pf_alloc(
   int min_samples, int max_samples,
   double alpha_slow, double alpha_fast,
   pf_init_model_fn_t random_pose_fn,
-  void * random_pose_data, double k_l,
+  void * random_pose_data,
   double max_particle_gen_prob_ext_pose);
 
 // Free an existing filter

nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp

@@ -66,6 +66,7 @@ class Laser
   double z_hit_;
   double z_rand_;
   double sigma_hit_;
+  double importance_factor_;
 
   /*
    * @brief Reallocate weights
@@ -122,7 +123,7 @@ class BeamModel : public Laser
    */
   BeamModel(
     double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
-    double lambda_short, double chi_outlier, size_t max_beams, map_t * map);
+    double lambda_short, double chi_outlier, size_t max_beams, map_t * map, double laser_importance_factor);
 
   /*
    * @brief Run a sensor update on laser
@@ -152,7 +153,7 @@ class LikelihoodFieldModel : public Laser
    */
   LikelihoodFieldModel(
     double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
-    size_t max_beams, map_t * map);
+    size_t max_beams, map_t * map, double laser_importance_factor);
 
   /*
    * @brief Run a sensor update on laser
@@ -186,7 +187,7 @@ class LikelihoodFieldModelProb : public Laser
     double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
     bool do_beamskip, double beam_skip_distance,
     double beam_skip_threshold, double beam_skip_error_threshold,
-    size_t max_beams, map_t * map);
+    size_t max_beams, map_t * map, double laser_importance_factor);
 
   /*
    * @brief Run a sensor update on laser

nav2_amcl/src/amcl_node.cpp

@@ -230,8 +230,8 @@ AmclNode::AmclNode()
     "Topic to subscribe to in order to receive the map to localize on");
 
   add_parameter(
-    "k_l", rclcpp::ParameterValue(200.0f),
-    "Constant to balance the importance of the external pose data");
+    "laser_importance_factor", rclcpp::ParameterValue(1.0f),
+    "Importance factor for laser scan, [0.0, 1.0]");
 
   add_parameter(
     "std_warn_level_x", rclcpp::ParameterValue(0.2),
@@ -1183,19 +1183,19 @@ AmclNode::createLaserObject()
   if (sensor_model_type_ == "beam") {
     return new nav2_amcl::BeamModel(
       z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_,
-      0.0, max_beams_, map_);
+      0.0, max_beams_, map_, laser_importance_factor_);
   }
 
   if (sensor_model_type_ == "likelihood_field_prob") {
     return new nav2_amcl::LikelihoodFieldModelProb(
       z_hit_, z_rand_, sigma_hit_,
       laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_,
-      beam_skip_error_threshold_, max_beams_, map_);
+      beam_skip_error_threshold_, max_beams_, map_, laser_importance_factor_);
   }
 
   return new nav2_amcl::LikelihoodFieldModel(
     z_hit_, z_rand_, sigma_hit_,
-    laser_likelihood_max_dist_, max_beams_, map_);
+    laser_likelihood_max_dist_, max_beams_, map_, laser_importance_factor_);
 }
 
 void
@@ -1251,7 +1251,7 @@ AmclNode::initParameters()
   get_parameter("always_reset_initial_pose", always_reset_initial_pose_);
   get_parameter("scan_topic", scan_topic_);
   get_parameter("map_topic", map_topic_);
-  get_parameter("k_l", k_l_);
+  get_parameter("laser_importance_factor", laser_importance_factor_);
   get_parameter("std_warn_level_x", std_warn_level_x_);
   get_parameter("std_warn_level_y", std_warn_level_y_);
   get_parameter("std_warn_level_yaw", std_warn_level_yaw_);
@@ -1523,7 +1523,7 @@ AmclNode::initParticleFilter()
   pf_ = pf_alloc(
     min_particles_, max_particles_, alpha_slow_, alpha_fast_,
     (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
-    reinterpret_cast<void *>(map_), k_l_, max_particle_gen_prob_ext_pose_);
+    reinterpret_cast<void *>(map_), max_particle_gen_prob_ext_pose_);
   pf_->pop_err = pf_err_;
   pf_->pop_z = pf_z_;
 

nav2_amcl/src/pf/pf.c

@@ -49,7 +49,7 @@ pf_t * pf_alloc(
   int min_samples, int max_samples,
   double alpha_slow, double alpha_fast,
   pf_init_model_fn_t random_pose_fn,
-  void * random_pose_data, double k_l,
+  void * random_pose_data,
   double max_particle_gen_prob_ext_pose)
 {
   int i, j;
@@ -104,7 +104,6 @@ pf_t * pf_alloc(
 
   pf->w_slow = 0.0;
   pf->w_fast = 0.0;
-  pf->k_l = k_l;
   pf->max_particle_gen_prob_ext_pose = max_particle_gen_prob_ext_pose;
   pf->ext_pose_is_valid = 0;
 
@@ -417,7 +416,7 @@ void pf_update_resample(pf_t * pf)
       // fprintf(stderr, "AMCL: laser weight - %f, ext pose likelihood - %f\n", set_a->samples[i].weight, ext_pose_likelihood);
 
       // See Improved LiDAR Probabilistic Localization for Autonomous Vehicles Using GNSS, #3.3 for details
-      set_a->samples[i].weight = set_a->samples[i].weight * pf->k_l + ext_pose_likelihood;
+      set_a->samples[i].weight = set_a->samples[i].weight * LASER_TO_EXTERNAL_POSE_BALANCE_FACTOR + ext_pose_likelihood;
 
       total_weight += set_a->samples[i].weight;
     }

nav2_amcl/src/sensors/laser/beam_model.cpp

@@ -29,16 +29,18 @@ namespace nav2_amcl
 
 BeamModel::BeamModel(
   double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
-  double lambda_short, double chi_outlier, size_t max_beams, map_t * map)
+  double lambda_short, double chi_outlier, size_t max_beams, map_t * map, double importance_factor)
 : Laser(max_beams, map)
 {
   z_hit_ = z_hit;
   z_rand_ = z_rand;
   sigma_hit_ = sigma_hit;
   z_short_ = z_short;
   z_max_ = z_max;
+  importance_factor_ = importance_factor;
   lambda_short_ = lambda_short;
   chi_outlier_ = chi_outlier;
+
 }
 
 // Determine the probability for the given pose
@@ -109,7 +111,7 @@ BeamModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
       p += pz * pz * pz;
     }
 
-    sample->weight *= p;
+    sample->weight *= pow(p, self->importance_factor_); // Accroding to Probabilistic Robotics, 6.3.4
     total_weight += sample->weight;
   }
 

nav2_amcl/src/sensors/laser/likelihood_field_model.cpp

@@ -29,12 +29,13 @@ namespace nav2_amcl
 
 LikelihoodFieldModel::LikelihoodFieldModel(
   double z_hit, double z_rand, double sigma_hit,
-  double max_occ_dist, size_t max_beams, map_t * map)
+  double max_occ_dist, size_t max_beams, map_t * map, double importance_factor)
 : Laser(max_beams, map)
 {
   z_hit_ = z_hit;
   z_rand_ = z_rand;
   sigma_hit_ = sigma_hit;
+  importance_factor_ = importance_factor;
   map_update_cspace(map, max_occ_dist);
 }
 
@@ -124,7 +125,7 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
       p += pz * pz * pz;
     }
 
-    sample->weight *= p;
+    sample->weight *= pow(p, self->importance_factor_); // Accroding to Probabilistic Robotics, 6.3.4
     total_weight += sample->weight;
   }
 

nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp

@@ -34,12 +34,13 @@ LikelihoodFieldModelProb::LikelihoodFieldModelProb(
   double beam_skip_distance,
   double beam_skip_threshold,
   double beam_skip_error_threshold,
-  size_t max_beams, map_t * map)
+  size_t max_beams, map_t * map, double importance_factor)
 : Laser(max_beams, map)
 {
   z_hit_ = z_hit;
   z_rand_ = z_rand;
   sigma_hit_ = sigma_hit;
+  importance_factor_ = importance_factor;
   do_beamskip_ = do_beamskip;
   beam_skip_distance_ = beam_skip_distance;
   beam_skip_threshold_ = beam_skip_threshold;
@@ -180,7 +181,7 @@ LikelihoodFieldModelProb::sensorFunction(LaserData * data, pf_sample_set_t * set
       // TODO(?): outlier rejection for short readings
 
       if (!do_beamskip) {
-        log_p += log(pz);
+        log_p += log(pz) * self->importance_factor_; // Accroding to Probabilistic Robotics, 6.3.4
       } else {
         self->temp_obs_[j][beam_ind] = pz;
       }
@@ -225,7 +226,7 @@ LikelihoodFieldModelProb::sensorFunction(LaserData * data, pf_sample_set_t * set
 
       for (beam_ind = 0; beam_ind < self->max_beams_; beam_ind++) {
         if (error || obs_mask[beam_ind]) {
-          log_p += log(self->temp_obs_[j][beam_ind]);
+          log_p += log(self->temp_obs_[j][beam_ind]) * self->importance_factor_; // Accroding to Probabilistic Robotics, 6.3.4
         }
       }