diff --git a/config/params.yaml b/config/params.yaml
index 92796750537cc5a169224f0398a38d185b4160f4..0c1b8ecf1272a60dd76393dde07d7488a958fcc4 100644
--- a/config/params.yaml
+++ b/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
diff --git a/include/laser_scan_icp_alg.h b/include/laser_scan_icp_alg.h
index 3ad7f4fb0704012c5704217b5f24df4a57c3e067..c7730135cdd7760f25568e7b552d4283b20a738b 100644
--- a/include/laser_scan_icp_alg.h
+++ b/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
diff --git a/src/laser_scan_icp_alg.cpp b/src/laser_scan_icp_alg.cpp
index 3c3d3747232b7a51463f76e0b7977f01b7a86ff7..095b8636ec47f44c6a950759395fbb6c35987acb 100644
--- a/src/laser_scan_icp_alg.cpp
+++ b/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;
diff --git a/src/laser_scan_icp_alg_node.cpp b/src/laser_scan_icp_alg_node.cpp
index 204942017f112c955621da423f3f4d24aa12c07a..b1fc30e5ac591eabb6aef10715f5e46c0d90c775 100644
--- a/src/laser_scan_icp_alg_node.cpp
+++ b/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);
}