From fa6e6320856f3bf3388b850ad3930a90c5f8a7d5 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 16 Dec 2021 13:33:25 +0100
Subject: [PATCH] documented icp params and changed types
---
src/icp.cpp | 187 +++++++++++++++++++++++-----------------------------
src/icp.h | 119 +++++++++++++++++++++------------
2 files changed, 162 insertions(+), 144 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index 8a18328..4f45427 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -44,8 +44,8 @@ class LDWrapper
laser_data->theta[i] = scan_params.angle_min_ + i*scan_params.angle_step_;
if (scan_params.range_min_ <= it and
- it <= scan_params.range_max_ and
- 0 <= it and it <= 100)
+ it <= scan_params.range_max_ and
+ 0 <= it and it <= 100)
{
laser_data->readings[i] = it;
laser_data->valid[i] = 1;
@@ -95,105 +95,86 @@ icpOutput ICP::align(const LaserScan &_current_ls,
const icpParams &_icp_params,
Eigen::Vector3s &_transf_ref_current)
{
- // Uncomment to enable debug messages from the CSM library
- //sm_debug_write(true);
-
- LDWrapper laser_scan_current = LDWrapper(_current_ls, _current_scan_params);
- LDWrapper laser_scan_ref = LDWrapper(_ref_ls, _ref_scan_params);
-
- int num_rays = _current_ls.ranges_raw_.size();
-
- sm_params csm_input{};
- sm_result csm_output{};
-
- csm_input.min_reading = fmax(_current_scan_params.range_min_, _ref_scan_params.range_min_);
- csm_input.max_reading = fmin(_current_scan_params.range_max_, _ref_scan_params.range_max_);
- csm_input.sigma = _ref_scan_params.range_std_dev_;
-
- csm_input.laser_ref = laser_scan_ref.laser_data;
- csm_input.laser_sens = laser_scan_current.laser_data;
-
- csm_input.first_guess[0] = _transf_ref_current(0);
- csm_input.first_guess[1] = _transf_ref_current(1);
- csm_input.first_guess[2] = _transf_ref_current(2);
-
- csm_input.use_point_to_line_distance = _icp_params.use_point_to_line_distance;
- csm_input.max_correspondence_dist = _icp_params.max_correspondence_dist;
- csm_input.max_iterations = _icp_params.max_iterations;
- csm_input.use_corr_tricks = _icp_params.use_corr_tricks;
- csm_input.outliers_maxPerc = _icp_params.outliers_maxPerc;
- csm_input.outliers_adaptive_order = _icp_params.outliers_adaptive_order;
- csm_input.outliers_adaptive_mult = _icp_params.outliers_adaptive_mult;
- csm_input.do_compute_covariance = _icp_params.do_compute_covariance;
-
- csm_input.max_angular_correction_deg = _icp_params.max_angular_correction_deg;
- csm_input.max_linear_correction = _icp_params.max_linear_correction;
- csm_input.epsilon_xy = _icp_params.epsilon_xy;
- csm_input.epsilon_theta = _icp_params.epsilon_theta;
- csm_input.sigma = _icp_params.sigma;
- csm_input.restart = _icp_params.restart;
- csm_input.restart_threshold_mean_error = _icp_params.restart_threshold_mean_error;
- csm_input.restart_dt = _icp_params.restart_dt;
- csm_input.restart_dtheta = _icp_params.restart_dtheta;
- csm_input.clustering_threshold = _icp_params.clustering_threshold;
- csm_input.orientation_neighbourhood = _icp_params.orientation_neighbourhood;
- csm_input.do_alpha_test = _icp_params.do_alpha_test;
- csm_input.do_alpha_test_thresholdDeg = _icp_params.do_alpha_test_thresholdDeg;
- csm_input.do_visibility_test = _icp_params.do_visibility_test;
- csm_input.outliers_remove_doubles = _icp_params.outliers_remove_doubles;
- csm_input.debug_verify_tricks = _icp_params.debug_verify_tricks;
- csm_input.gpm_theta_bin_size_deg = _icp_params.gpm_theta_bin_size_deg;
- csm_input.gpm_extend_range_deg = _icp_params.gpm_extend_range_deg;
- csm_input.gpm_interval = _icp_params.gpm_interval;
-
- //Not implemented (yet) in CSM
- // csm_input.laser_x = _icp_params.laser_x;
- // csm_input.laser_y = _icp_params.laser_y;
- // csm_input.laser_theta = _icp_params.laser_theta;
-
- csm_input.min_reading = _icp_params.min_reading;
- csm_input.max_reading = _icp_params.max_reading;
- csm_input.use_ml_weights = _icp_params.use_ml_weights;
- csm_input.use_sigma_weights = _icp_params.use_sigma_weights;
-
- // Not implemented (yet) in CSM
- // csm_input.hsm_linear_cell_size = _icp_params.hsm_linear_cell_size;
- // csm_input.hsm_angular_cell_size_deg = _icp_params.hsm_angular_cell_size_deg;
- // csm_input.hsm_num_angular_hypotheses = _icp_params.hsm_num_angular_hypotheses;
- // csm_input.hsm_xc_directions_min_distance_deg = _icp_params.hsm_xc_directions_min_distance_deg;
- // csm_input.hsm_xc_ndirections = _icp_params.hsm_xc_ndirections;
- // csm_input.hsm_angular_hyp_min_distance_deg = _icp_params.hsm_angular_hyp_min_distance_deg;
- // csm_input.hsm_linear_xc_max_npeaks = _icp_params.hsm_linear_xc_max_npeaks;
- // csm_input.hsm_linear_xc_peaks_min_distance = _icp_params.hsm_linear_xc_peaks_min_distance;
-
- sm_icp(&csm_input, &csm_output);
-
- icpOutput result{};
- result.nvalid = csm_output.nvalid;
- result.valid = csm_output.valid;
- result.error = csm_output.error;
-
- if (result.valid == 1)
- {
- result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
- result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
- result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
-
- if (csm_input.do_compute_covariance)
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- result.res_covar(i, j) = _icp_params.cov_factor *
- csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
- // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
- }
- else
- {
- //std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
- result.res_transf = _transf_ref_current;
- result.res_covar = Eigen::Matrix3s::Identity();
- }
-
- return result;
+ // enable/disable debug messages from the CSM library
+ sm_debug_write(_icp_params.verbose);
+
+ LDWrapper laser_scan_current = LDWrapper(_current_ls, _current_scan_params);
+ LDWrapper laser_scan_ref = LDWrapper(_ref_ls, _ref_scan_params);
+
+ int num_rays = _current_ls.ranges_raw_.size();
+
+ sm_params csm_input{};
+ sm_result csm_output{};
+
+ csm_input.min_reading = fmax(_current_scan_params.range_min_, _ref_scan_params.range_min_);
+ csm_input.max_reading = fmin(_current_scan_params.range_max_, _ref_scan_params.range_max_);
+ csm_input.sigma = _ref_scan_params.range_std_dev_;
+
+ csm_input.laser_ref = laser_scan_ref.laser_data;
+ csm_input.laser_sens = laser_scan_current.laser_data;
+
+ csm_input.first_guess[0] = _transf_ref_current(0);
+ csm_input.first_guess[1] = _transf_ref_current(1);
+ csm_input.first_guess[2] = _transf_ref_current(2);
+
+ csm_input.use_point_to_line_distance = _icp_params.use_point_to_line_distance ? 1 : 0;
+ csm_input.max_correspondence_dist = _icp_params.max_correspondence_dist;
+ csm_input.max_iterations = _icp_params.max_iterations;
+ csm_input.use_corr_tricks = _icp_params.use_corr_tricks ? 1 : 0;
+ csm_input.outliers_maxPerc = _icp_params.outliers_maxPerc;
+ csm_input.outliers_adaptive_order = _icp_params.outliers_adaptive_order;
+ csm_input.outliers_adaptive_mult = _icp_params.outliers_adaptive_mult;
+ csm_input.do_compute_covariance = _icp_params.do_compute_covariance ? 1 : 0;
+
+ csm_input.max_angular_correction_deg = _icp_params.max_angular_correction_deg;
+ csm_input.max_linear_correction = _icp_params.max_linear_correction;
+ csm_input.epsilon_xy = _icp_params.epsilon_xy;
+ csm_input.epsilon_theta = _icp_params.epsilon_theta;
+ csm_input.sigma = _icp_params.sigma;
+ csm_input.restart = _icp_params.restart ? 1 : 0;
+ csm_input.restart_threshold_mean_error = _icp_params.restart_threshold_mean_error;
+ csm_input.restart_dt = _icp_params.restart_dt;
+ csm_input.restart_dtheta = _icp_params.restart_dtheta;
+ csm_input.clustering_threshold = _icp_params.clustering_threshold;
+ csm_input.orientation_neighbourhood = _icp_params.orientation_neighbourhood;
+ csm_input.do_alpha_test = _icp_params.do_alpha_test ? 1 : 0;
+ csm_input.do_alpha_test_thresholdDeg = _icp_params.do_alpha_test_thresholdDeg;
+ csm_input.do_visibility_test = _icp_params.do_visibility_test ? 1 : 0;
+ csm_input.outliers_remove_doubles = _icp_params.outliers_remove_doubles ? 1 : 0;
+ csm_input.debug_verify_tricks = _icp_params.debug_verify_tricks ? 1 : 0;
+ csm_input.min_reading = _icp_params.min_reading;
+ csm_input.max_reading = _icp_params.max_reading;
+ csm_input.use_ml_weights = _icp_params.use_ml_weights ? 1 : 0;
+ csm_input.use_sigma_weights = _icp_params.use_sigma_weights ? 1 : 0;
+
+ sm_icp(&csm_input, &csm_output);
+
+ icpOutput result{};
+ result.nvalid = csm_output.nvalid;
+ result.valid = csm_output.valid == 1;
+ result.error = csm_output.error;
+
+ if (result.valid)
+ {
+ result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
+ result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
+ result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
+
+ if (csm_input.do_compute_covariance)
+ for (int i = 0; i < 3; ++i)
+ for (int j = 0; j < 3; ++j)
+ result.res_covar(i, j) = _icp_params.cov_factor *
+ csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
+ // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
+ }
+ else
+ {
+ //std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
+ result.res_transf = _transf_ref_current;
+ result.res_covar = Eigen::Matrix3s::Identity();
+ }
+
+ return result;
}
//Legacy code
@@ -213,8 +194,8 @@ void ICP::printTwoLaserData(sm_params ¶ms)
for (int ii=0; ii<params.laser_ref->nrays-1; ++ii)
{
std::cout << "Theta: " << params.laser_ref->theta[ii] << "; Readings: "
- << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
- << '\n';
+ << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
+ << '\n';
}
}
diff --git a/src/icp.h b/src/icp.h
index 9083562..d93e738 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -31,59 +31,96 @@
namespace laserscanutils{
struct icpOutput{
- int valid; /** 1 if the result is valid */
+ bool valid; /** if the result is valid */
Eigen::Vector3s res_transf;
Eigen::Matrix3s res_covar;
int nvalid; /** Number of valid correspondence in the end */
double error; /** Total correspondence error */
};
-struct icpParams{
- double cov_factor;
+struct icpParams
+{
+ bool verbose; // prints debug messages
- 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;
+ // Algorithm options ---------------------------------------------------
+ bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false).
- int do_compute_covariance;
+ /** Maximum angular displacement between scans (deg)*/
double max_angular_correction_deg;
+ /** Maximum translation between scans (m) */
double max_linear_correction;
- double epsilon_xy;
- double epsilon_theta;
- double sigma;
- int restart;
- double restart_threshold_mean_error;
- double restart_dt;
- double restart_dtheta;
+
+ // Stopping criteria
+ int max_iterations; // maximum iterations
+ double epsilon_xy; // distance change
+ double epsilon_theta; // angle change
+
+ /** Maximum distance for a correspondence to be valid */
+ double max_correspondence_dist;
+ /** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
+ bool use_corr_tricks;
+ /** Checks that find_correspondences_tricks give the right answer */
+ bool debug_verify_tricks;
+
+ // Restart algorithm
+ bool restart;
+ double restart_threshold_mean_error; // Threshold for restarting
+ double restart_dt; // Displacement for restarting
+ double restart_dtheta; // Displacement for restarting
+
+ // Discarding points or correpondences ---------------------------------------------------
+ /** discard rays outside of this interval */
+ double min_reading, max_reading;
+ /** Percentage of correspondences to consider: if 0.9,
+ always discard the top 10% of correspondences with more error */
+ double outliers_maxPerc;
+
+ /** Parameters describing a simple adaptive algorithm for discarding.
+ 1) Order the errors.
+ 2) Choose the percentile according to outliers_adaptive_order.
+ (if it is 0.7, get the 70% percentile)
+ 3) Define an adaptive threshold multiplying outliers_adaptive_mult
+ with the value of the error at the chosen percentile.
+ 4) Discard correspondences over the threshold.
+
+ This is useful to be conservative; yet remove the biggest errors.
+ */
+ double outliers_adaptive_order; // 0.7
+ double outliers_adaptive_mult; // 2
+
+ /** Do not allow two different correspondences to share a point */
+ bool outliers_remove_doubles;
+
+ /** If initial guess, visibility test can be done to discard points that are not visible */
+ bool do_visibility_test;
+
+ /** Discard correspondences based on the angles */
+ bool do_alpha_test;
+ double do_alpha_test_thresholdDeg;
+
+ // Point orientation ------------------------------------------------------------------
+ /** For now, a very simple max-distance clustering algorithm is used */
double clustering_threshold;
+ /** Number of neighbour rays used to estimate the orientation.*/
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 laser_x;
- //double laser_y;
- //double laser_theta;
- double min_reading;
- double max_reading;
- int use_ml_weights;
- int use_sigma_weights;
- //double hsm_linear_cell_size;
- //double hsm_angular_cell_size_deg;
- //double hsm_num_angular_hypotheses;
- //double hsm_xc_directions_min_distance_deg;
- //double hsm_xc_ndirections;
- //double hsm_angular_hyp_min_distance_deg;
- //double hsm_linear_xc_max_npeaks;
- //double hsm_linear_xc_peaks_min_distance;
+
+ // Weights ---------------------------------------------------------------------------
+ /** If the field "true_alpha" is used to compute the incidence
+ beta, and the factor (1/cos^2(beta)) used to weight the impact
+ of each correspondence. This works fabolously if doing localization,
+ that is the first scan has no noise.
+ If "true_alpha" is not available, it uses "alpha".
+ */
+ bool use_ml_weights;
+ /* If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
+ bool use_sigma_weights;
+ /** Noise in the scan */
+ double sigma;
+
+ // Covariance ------------------------------------------------------------------------
+ bool do_compute_covariance; // Compute the matching covariance (method in http://purl.org/censi/2006/icpcov)
+ double cov_factor; // Factor multiplying the cov output of csm
+
};
class ICP
--
GitLab