diff --git a/src/icp.cpp b/src/icp.cpp index 8a18328e53bbe2dddc2f9821f37cc36851da1c49..4f454270a7303bfb1954fbf81e7f8900bdaec291 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 908356258b46b55786fdc85ae033fb11c6b2b2ae..d93e738788c21de238f70eec279f5f59be8eff80 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