Updates to laser_scan_utils update

config/params.yaml

@@ -2,38 +2,47 @@ rate: 10
 range_std_dev: 0.1
 angle_std_dev: 0.05
 
-max_iterations:                      50
-max_correspondence_dist:             1
-use_corr_tricks:                     1
-outliers_maxPerc:                    0.9
-use_point_to_line_distance:          1
-outliers_adaptive_order:             0.7
-outliers_adaptive_mult:              1.5
-do_compute_covariance:               1
-
-max_angular_correction_deg:          3.14
-max_linear_correction:               2.0
-epsilon_xy:                          0
-epsilon_theta:                       0
-sigma:                               0.1
-restart:                             0
-restart_threshold_mean_error:        0
-restart_dt:                          0
-restart_dtheta:                      0
-clustering_threshold:                0
-orientation_neighbourhood:           0
-do_alpha_test:                       0
-do_alpha_test_thresholdDeg:          0
-do_visibility_test:                  0
-outliers_remove_doubles:             0
-debug_verify_tricks:                 0
-gpm_theta_bin_size_deg:              0
-gpm_extend_range_deg:                0
-gpm_interval:                        0
-min_reading:                         0.023
-max_reading:                         40.0
-use_ml_weights:                      0
-use_sigma_weights:                   0
-
-cov_factor:                          1
+verbose:                             false # prints debug messages
 
+# ALGORITHM OPTIONS
+use_point_to_line_distance:          true  # use PlICP (true) or use vanilla ICP (false).
+max_angular_correction_deg:          20    # Maximum angular displacement between scans (deg)
+max_linear_correction:               2.0   # Maximum translation between scans (m)
+max_correspondence_dist:             0.5   # Maximum distance for a correspondence to be valid
+use_corr_tricks:                     true  # Use smart tricks for finding correspondences. Only influences speed; not convergence.
+debug_verify_tricks:                 false # Checks that find_correspondences_tricks give the right answer
+
+# STOP CRITERIA
+max_iterations:                      50    # maximum iterations
+epsilon_xy:                          0.001 # distance change
+epsilon_theta:                       0.001  # angle change
+
+# RESTART
+restart:                             false # Restart algorithm
+restart_threshold_mean_error:        0     # Threshold for restarting
+restart_dt:                          0     # Displacement for restarting
+restart_dtheta:                      0     # Displacement for restarting
+
+# DISCARDING POINTS/CORRESPONDENCES
+min_reading:                         0.023 # discard rays outside of this interval
+max_reading:                         40.0  # discard rays outside of this interval
+outliers_maxPerc:                    0.9   # Percentage of correspondences to consider; if 0.9, always discard the top 10% of correspondences with more error
+outliers_adaptive_order:             0.7   # Parameters describing a simple adaptive algorithm for discarding
+outliers_adaptive_mult:              1.5   # Parameters describing a simple adaptive algorithm for discarding
+outliers_remove_doubles:             false # Do not allow two different correspondences to share a point
+do_visibility_test:                  false # If initial guess, visibility test can discard points that are not visible
+do_alpha_test:                       false # Discard correspondences based on the angles
+do_alpha_test_thresholdDeg:          0     # 
+
+# POINT ORIENTATION
+clustering_threshold:                0.0   # Max-distance clustering for point orientation
+orientation_neighbourhood:           0     # Number of neighbour rays used to estimate the orientation
+
+# WEIGHTS
+use_ml_weights:                      false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
+use_sigma_weights:                   false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
+sigma:                               0.1   # Noise of the scan
+
+# COVARIANCE
+do_compute_covariance:               true
+cov_factor:                          1     # Factor multiplying the cov output of csm

include/laser_scan_icp_alg.h

@@ -109,17 +109,16 @@ class LaserScanIcpAlgorithm
      * \brief Function to set ICP parameters.
      * 
      */
-    void set_icp_params(const double& _cov_factor, const int& _use_point_to_line_distance, const double& _max_correspondence_dist, 
-                          const int& _max_iterations, const int& _use_corr_tricks, const double& _outliers_maxPerc, 
-                          const double& _outliers_adaptive_order, const double& _outliers_adaptive_mult, const int& _do_compute_covariance, 
+    void set_icp_params(const bool& _verbose, const double& _cov_factor, const bool& _use_point_to_line_distance, const double& _max_correspondence_dist, 
+                          const int& _max_iterations, const bool& _use_corr_tricks, const double& _outliers_maxPerc, 
+                          const double& _outliers_adaptive_order, const double& _outliers_adaptive_mult, const bool& _do_compute_covariance, 
                           const double& _max_angular_correction_deg, const double& _max_linear_correction, const double& _epsilon_xy, 
-                          const double& _epsilon_theta, const double& _sigma, const int& _restart, 
+                          const double& _epsilon_theta, const double& _sigma, const bool& _restart, 
                           const double& _restart_threshold_mean_error, const double& _restart_dt, const double& _restart_dtheta, 
-                          const double& _clustering_threshold, const int& _orientation_neighbourhood, const int& _do_alpha_test, 
-                          const double& _do_alpha_test_thresholdDeg, const int& _do_visibility_test, const int& _outliers_remove_doubles, 
-                          const int& _debug_verify_tricks, const double& _gpm_theta_bin_size_deg, const double& _gpm_extend_range_deg, 
-                          const double& _gpm_interval, const double& _min_reading, const double& _max_reading, 
-                          const int& _use_ml_weights, const int& _use_sigma_weights);
+                          const double& _clustering_threshold, const int& _orientation_neighbourhood, const bool& _do_alpha_test, 
+                          const double& _do_alpha_test_thresholdDeg, const bool& _do_visibility_test, const bool& _outliers_remove_doubles, 
+                          const bool& _debug_verify_tricks, const double& _min_reading, const double& _max_reading, 
+                          const bool& _use_ml_weights, const bool& _use_sigma_weights);
 
    /**
     * \brief define config type

src/laser_scan_icp_alg.cpp

@@ -59,18 +59,18 @@ bool LaserScanIcpAlgorithm::process_icp(const double& _rot_angle_estimated, Eige
   return (out.valid == 1);
 }
 
-void LaserScanIcpAlgorithm::set_icp_params(const double& _cov_factor, const int& _use_point_to_line_distance, const double& _max_correspondence_dist, 
-                          const int& _max_iterations, const int& _use_corr_tricks, const double& _outliers_maxPerc, 
-                          const double& _outliers_adaptive_order, const double& _outliers_adaptive_mult, const int& _do_compute_covariance, 
+void LaserScanIcpAlgorithm::set_icp_params(const bool& _verbose, const double& _cov_factor, const bool& _use_point_to_line_distance, const double& _max_correspondence_dist, 
+                          const int& _max_iterations, const bool& _use_corr_tricks, const double& _outliers_maxPerc, 
+                          const double& _outliers_adaptive_order, const double& _outliers_adaptive_mult, const bool& _do_compute_covariance, 
                           const double& _max_angular_correction_deg, const double& _max_linear_correction, const double& _epsilon_xy, 
-                          const double& _epsilon_theta, const double& _sigma, const int& _restart, 
+                          const double& _epsilon_theta, const double& _sigma, const bool& _restart, 
                           const double& _restart_threshold_mean_error, const double& _restart_dt, const double& _restart_dtheta, 
-                          const double& _clustering_threshold, const int& _orientation_neighbourhood, const int& _do_alpha_test, 
-                          const double& _do_alpha_test_thresholdDeg, const int& _do_visibility_test, const int& _outliers_remove_doubles, 
-                          const int& _debug_verify_tricks, const double& _gpm_theta_bin_size_deg, const double& _gpm_extend_range_deg, 
-                          const double& _gpm_interval, const double& _min_reading, const double& _max_reading, 
-                          const int& _use_ml_weights, const int& _use_sigma_weights)
+                          const double& _clustering_threshold, const int& _orientation_neighbourhood, const bool& _do_alpha_test, 
+                          const double& _do_alpha_test_thresholdDeg, const bool& _do_visibility_test, const bool& _outliers_remove_doubles, 
+                          const bool& _debug_verify_tricks, const double& _min_reading, const double& _max_reading, 
+                          const bool& _use_ml_weights, const bool& _use_sigma_weights)
 {
+  this->icp_params_.verbose = _verbose;
   this->icp_params_.cov_factor = _cov_factor;
   this->icp_params_.use_point_to_line_distance = _use_point_to_line_distance;
   this->icp_params_.max_correspondence_dist = _max_correspondence_dist;
@@ -96,9 +96,6 @@ void LaserScanIcpAlgorithm::set_icp_params(const double& _cov_factor, const int&
   this->icp_params_.do_visibility_test = _do_visibility_test;
   this->icp_params_.outliers_remove_doubles = _outliers_remove_doubles;
   this->icp_params_.debug_verify_tricks = _debug_verify_tricks;
-  this->icp_params_.gpm_theta_bin_size_deg = _gpm_theta_bin_size_deg;
-  this->icp_params_.gpm_extend_range_deg = _gpm_extend_range_deg;
-  this->icp_params_.gpm_interval = _gpm_interval;
   this->icp_params_.min_reading = _min_reading;
   this->icp_params_.max_reading = _max_reading;
   this->icp_params_.use_ml_weights = _use_ml_weights;

src/laser_scan_icp_alg_node.cpp

@@ -51,39 +51,43 @@ void LaserScanIcpAlgNode::mainNodeThread(void)
 }
 
 void LaserScanIcpAlgNode::set_icp_params(void)
-{
-  double cov_factor;
-  int use_point_to_line_distance;
-  double max_correspondence_dist;
-  int max_iterations;
-  int use_corr_tricks;
-  double outliers_maxPerc;
-  double outliers_adaptive_order;
-  double outliers_adaptive_mult;
-  int do_compute_covariance;
+{ 
+  bool verbose;
+  bool use_point_to_line_distance;
   double max_angular_correction_deg;
   double max_linear_correction;
+  int max_iterations;
   double epsilon_xy;
   double epsilon_theta;
-  double sigma;
-  int restart;
+  double max_correspondence_dist;
+  bool use_corr_tricks;
+  bool debug_verify_tricks;
+  bool restart;
   double restart_threshold_mean_error;
   double restart_dt;
   double restart_dtheta;
-  double clustering_threshold;
-  int orientation_neighbourhood;
-  int do_alpha_test;
-  double do_alpha_test_thresholdDeg;
-  int do_visibility_test;
-  int outliers_remove_doubles;
-  int debug_verify_tricks;
-  double gpm_theta_bin_size_deg;
-  double gpm_extend_range_deg;
-  double gpm_interval;
   double min_reading;
   double max_reading;
-  int use_ml_weights;
-  int use_sigma_weights;
+  double outliers_maxPerc;
+  double outliers_adaptive_order;
+  double outliers_adaptive_mult;
+  bool outliers_remove_doubles;
+  bool do_visibility_test;
+  bool do_alpha_test;
+  double do_alpha_test_thresholdDeg;
+  double clustering_threshold;
+  int orientation_neighbourhood;
+  bool use_ml_weights;
+  bool use_sigma_weights;
+  double sigma;
+  bool do_compute_covariance;
+  double cov_factor;
+
+  if(!this->private_node_handle_.getParam("verbose", verbose))
+  {
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'verbose' not found. Setting it to false.");
+    verbose = false;
+  }
 
   if(!this->private_node_handle_.getParam("cov_factor", cov_factor))
   {
@@ -92,13 +96,13 @@ void LaserScanIcpAlgNode::set_icp_params(void)
   }
   if(!this->private_node_handle_.getParam("use_point_to_line_distance", use_point_to_line_distance))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_point_to_line_distance' not found. Setting it to 1.");
-    use_point_to_line_distance = 1;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_point_to_line_distance' not found. Setting it to true.");
+    use_point_to_line_distance = true;
   }
   if(!this->private_node_handle_.getParam("max_correspondence_dist", max_correspondence_dist))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'max_correspondence_dist' not found. Setting it to 1.");
-    max_correspondence_dist = 1;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'max_correspondence_dist' not found. Setting it to 0.5.");
+    max_correspondence_dist = 0.5;
   }
   if(!this->private_node_handle_.getParam("max_iterations", max_iterations))
   {
@@ -107,8 +111,8 @@ void LaserScanIcpAlgNode::set_icp_params(void)
   }
   if(!this->private_node_handle_.getParam("use_corr_tricks", use_corr_tricks))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_corr_tricks' not found. Setting it to 1.");
-    use_corr_tricks = 1;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_corr_tricks' not found. Setting it to true.");
+    use_corr_tricks = true;
   }
   if(!this->private_node_handle_.getParam("outliers_maxPerc", outliers_maxPerc))
   {
@@ -127,13 +131,13 @@ void LaserScanIcpAlgNode::set_icp_params(void)
   }
   if(!this->private_node_handle_.getParam("do_compute_covariance", do_compute_covariance))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'do_compute_covariance' not found. Setting it to 1.");
-    do_compute_covariance = 1;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'do_compute_covariance' not found. Setting it to true.");
+    do_compute_covariance = true;
   }
   if(!this->private_node_handle_.getParam("max_angular_correction_deg", max_angular_correction_deg))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'max_angular_correction_deg' not found. Setting it to 3.14.");
-    max_angular_correction_deg = 3.14;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'max_angular_correction_deg' not found. Setting it to 20.");
+    max_angular_correction_deg = 20;
   }
   if(!this->private_node_handle_.getParam("max_linear_correction", max_linear_correction))
   {
@@ -157,8 +161,8 @@ void LaserScanIcpAlgNode::set_icp_params(void)
   }
   if(!this->private_node_handle_.getParam("restart", restart))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'restart' not found. Setting it to 0.");
-    restart = 0;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'restart' not found. Setting it to false.");
+    restart = false;
   }
   if(!this->private_node_handle_.getParam("restart_threshold_mean_error", restart_threshold_mean_error))
   {
@@ -187,8 +191,8 @@ void LaserScanIcpAlgNode::set_icp_params(void)
   }
   if(!this->private_node_handle_.getParam("do_alpha_test", do_alpha_test))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'do_alpha_test' not found. Setting it to 0.");
-    do_alpha_test = 0;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'do_alpha_test' not found. Setting it to false.");
+    do_alpha_test = false;
   }
   if(!this->private_node_handle_.getParam("do_alpha_test_thresholdDeg", do_alpha_test_thresholdDeg))
   {
@@ -197,33 +201,18 @@ void LaserScanIcpAlgNode::set_icp_params(void)
   }
   if(!this->private_node_handle_.getParam("do_visibility_test", do_visibility_test))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'do_visibility_test' not found. Setting it to 0.");
-    do_visibility_test = 0;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'do_visibility_test' not found. Setting it to false.");
+    do_visibility_test = false;
   }
   if(!this->private_node_handle_.getParam("outliers_remove_doubles", outliers_remove_doubles))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'outliers_remove_doubles' not found. Setting it to 0.");
-    outliers_remove_doubles = 0;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'outliers_remove_doubles' not found. Setting it to false.");
+    outliers_remove_doubles = false;
   }
   if(!this->private_node_handle_.getParam("debug_verify_tricks", debug_verify_tricks))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'debug_verify_tricks' not found. Setting it to 0.");
-    debug_verify_tricks = 0;
-  }
-  if(!this->private_node_handle_.getParam("gpm_theta_bin_size_deg", gpm_theta_bin_size_deg))
-  {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'gpm_theta_bin_size_deg' not found. Setting it to 0.");
-    gpm_theta_bin_size_deg = 0;
-  }
-  if(!this->private_node_handle_.getParam("gpm_extend_range_deg", gpm_extend_range_deg))
-  {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'gpm_extend_range_deg' not found. Setting it to 0.");
-    gpm_extend_range_deg = 0;
-  }
-  if(!this->private_node_handle_.getParam("gpm_interval", gpm_interval))
-  {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'gpm_interval' not found. Setting it to 0.");
-    gpm_interval = 0;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'debug_verify_tricks' not found. Setting it to false.");
+    debug_verify_tricks = false;
   }
   if(!this->private_node_handle_.getParam("min_reading", min_reading))
   {
@@ -237,20 +226,20 @@ void LaserScanIcpAlgNode::set_icp_params(void)
   }
   if(!this->private_node_handle_.getParam("use_ml_weights", use_ml_weights))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_ml_weights' not found. Setting it to 0.");
-    use_ml_weights = 0;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_ml_weights' not found. Setting it to false.");
+    use_ml_weights = false;
   }
   if(!this->private_node_handle_.getParam("use_sigma_weights", use_sigma_weights))
   {
-    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_sigma_weights' not found. Setting it to 0.");
-    use_sigma_weights = 0;
+    ROS_WARN("LaserScanIcpAlgNode::set_icp_params: param 'use_sigma_weights' not found. Setting it to false.");
+    use_sigma_weights = false;
   }
 
-  this->alg_.set_icp_params(cov_factor, use_point_to_line_distance, max_correspondence_dist, max_iterations, use_corr_tricks, outliers_maxPerc,
+  this->alg_.set_icp_params(verbose, cov_factor, use_point_to_line_distance, max_correspondence_dist, max_iterations, use_corr_tricks, outliers_maxPerc,
                             outliers_adaptive_order, outliers_adaptive_mult, do_compute_covariance, max_angular_correction_deg, max_linear_correction, epsilon_xy,
                             epsilon_theta, sigma, restart, restart_threshold_mean_error, restart_dt, restart_dtheta,
                             clustering_threshold, orientation_neighbourhood, do_alpha_test, do_alpha_test_thresholdDeg, do_visibility_test, outliers_remove_doubles,
-                            debug_verify_tricks, gpm_theta_bin_size_deg, gpm_extend_range_deg, gpm_interval, min_reading, max_reading,
+                            debug_verify_tricks, min_reading, max_reading,
                             use_ml_weights, use_sigma_weights);
 }