diff --git a/.gitignore b/.gitignore index c99a3e22087bd637b9d4de70dec55977cd422a95..07dd1e8673fdf510e0dbdb93de44891b7d519fa9 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ lib/ .settings/language.settings.xml .project .cproject +.clangd diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake index 7ad6ce1974a96153a652df1199f1ae7885ac8ed2..953f9c5113d9139cefaaa508043f5d623072cb8d 100644 --- a/cmake_modules/Findcsm.cmake +++ b/cmake_modules/Findcsm.cmake @@ -61,4 +61,4 @@ if(NOT csm_FOUND) csm_report_not_found("Something went wrong while setting up csm.") endif(NOT csm_FOUND) # Set the include directories for csm (itself). -set(csm_FOUND TRUE) \ No newline at end of file +set(csm_FOUND TRUE) diff --git a/src/icp.cpp b/src/icp.cpp index c137f9bfdb6095e689b646496a64bfa2ac3e1fbf..aa43c13911d56725e6bd1c40d75c73104e62830e 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -1,4 +1,6 @@ #include "icp.h" +#include <algorithm> + using namespace laserscanutils; unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); std::mt19937 generator (seed); @@ -11,7 +13,6 @@ public: { int num_rays = scan.ranges_raw_.size(); laser_data = ld_alloc_new(num_rays); - int i = 0; for(auto it : scan.ranges_raw_){ laser_data->theta[i] = scan_params.angle_min_ + i*scan_params.angle_step_; @@ -27,9 +28,6 @@ public: ++i; } - // for(int i = 0; i < num_rays; ++i){ - // laser_data->theta[i] = laser_data->theta[i] * 0.0175; - // } laser_data->min_theta = laser_data->theta[0]; laser_data->max_theta = laser_data->theta[num_rays-1]; @@ -56,72 +54,139 @@ ICP::~ICP() } -icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_last_transf) -{ - // Uncomment to enable debug messages from the CSM library - // sm_debug_write(true); - - LDWrapper last = LDWrapper(_last_ls, scan_params); - LDWrapper origin = LDWrapper(_origin_ls, scan_params); - - int num_rays = _last_ls.ranges_raw_.size(); - - sm_params csm_input{}; - sm_result csm_output{}; - - csm_input.min_reading = scan_params.range_min_; - csm_input.max_reading = scan_params.range_max_; - csm_input.sigma = scan_params.range_std_dev_; - - csm_input.laser_ref = origin.laser_data; - csm_input.laser_sens = last.laser_data; - - csm_input.first_guess[0] = _last_transf(0); - csm_input.first_guess[1] = _last_transf(1); - csm_input.first_guess[2] = _last_transf(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 = 1; - - 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]; - result.res_transf(1) = csm_output.x[1]; - result.res_transf(2) = csm_output.x[2]; - - for(int i = 0; i < 3; ++i) - for(int j = 0; j < 3; ++j) - result.res_covar(i,j) = - //gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING - csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; // This does the same - } - else - { - std::cout << "ICP valid != 1, providing first guess transformation and identity covariance\n"; - result.res_transf = _last_transf; - result.res_covar = Eigen::Matrix3s::Identity(); - } - - // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n'; - // std::cout << "Number of iterations: " << csm_output.iterations << '\n'; - // std::cout << "Error: " << csm_output.error << '\n'; +icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, const LaserScanParams &_current_scan_params, const LaserScanParams &_ref_scan_params,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 = max(_current_scan_params.range_min_, _ref_scan_params.range_min_); + csm_input.max_reading = min(_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; + + // std::cout<< "=========================================================================================================================================\n"; + // std::cout << "csm max_angular_correction_deg " << csm_input.max_angular_correction_deg << " icp max_angular_correction_deg " << _icp_params.max_angular_correction_deg << "\n"; + // std::cout << "csm max_linear_correction " << csm_input.max_linear_correction << " icp max_linear_correction " << _icp_params.max_linear_correction << "\n"; + // std::cout << "csm epsilon_xy " << csm_input.epsilon_xy << " icp epsilon_xy " << _icp_params.epsilon_xy << "\n"; + // std::cout << "csm epsilon_theta " << csm_input.epsilon_theta << " icp epsilon_theta " << _icp_params.epsilon_theta << "\n"; + // std::cout << "csm sigma " << csm_input.sigma << " icp sigma " << _icp_params.sigma << "\n"; + // std::cout << "csm restart " << csm_input.restart << " icp restart " << _icp_params.restart << "\n"; + // std::cout << "csm restart_threshold_mean_error " << csm_input.restart_threshold_mean_error << " icp restart_threshold_mean_error " << _icp_params.restart_threshold_mean_error << "\n"; + // std::cout << "csm restart_dt " << csm_input.restart_dt << " icp restart_dt " << _icp_params.restart_dt << "\n"; + // std::cout << "csm restart_dtheta " << csm_input.restart_dtheta << " icp restart_dtheta " << _icp_params.restart_dtheta << "\n"; + // std::cout << "csm clustering_threshold " << csm_input.clustering_threshold << " icp clustering_threshold " << _icp_params.clustering_threshold << "\n"; + // std::cout << "csm orientation_neighbourhood " << csm_input.orientation_neighbourhood << " icp orientation_neighbourhood " << _icp_params.orientation_neighbourhood << "\n"; + // std::cout << "csm do_alpha_test " << csm_input.do_alpha_test << " icp do_alpha_test " << _icp_params.do_alpha_test << "\n"; + // std::cout << "csm do_alpha_test_thresholdDeg " << csm_input.do_alpha_test_thresholdDeg << " icp do_alpha_test_thresholdDeg " << _icp_params.do_alpha_test_thresholdDeg << "\n"; + // std::cout << "csm do_visibility_test " << csm_input.do_visibility_test << " icp do_visibility_test " << _icp_params.do_visibility_test << "\n"; + // std::cout << "csm outliers_remove_doubles " << csm_input.outliers_remove_doubles << " icp outliers_remove_doubles " << _icp_params.outliers_remove_doubles << "\n"; + // std::cout << "csm debug_verify_tricks " << csm_input.debug_verify_tricks << " icp debug_verify_tricks " << _icp_params.debug_verify_tricks << "\n"; + // std::cout << "csm gpm_theta_bin_size_deg " << csm_input.gpm_theta_bin_size_deg << " icp gpm_theta_bin_size_deg " << _icp_params.gpm_theta_bin_size_deg << "\n"; + // std::cout << "csm gpm_extend_range_deg " << csm_input.gpm_extend_range_deg << " icp gpm_extend_range_deg " << _icp_params.gpm_extend_range_deg << "\n"; + // std::cout << "csm gpm_interval " << csm_input.gpm_interval << " icp gpm_interval " << _icp_params.gpm_interval << "\n"; + // std::cout << "00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000" + // "000000000000000000000000000000000\n"; + + //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]; + result.res_transf(1) = csm_output.x[1]; + result.res_transf(2) = csm_output.x[2]; + + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + result.res_covar(i, j) = + // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT + // COMPILING + csm_output.cov_x_m + ->data[i * csm_output.cov_x_m->tda + j]; // This does the same + } else { + std::cout << "ICP valid != 1, providing first guess transformation and " + "identity covariance\n"; + result.res_transf = _transf_ref_current; + result.res_covar = Eigen::Matrix3s::Identity(); + } + + // std::cout << "Number of valid correspondences: " << csm_output.nvalid << + // '\n'; std::cout << "Number of iterations: " << csm_output.iterations << + // '\n'; std::cout << "Error: " << csm_output.error << '\n'; + + return result; +} - return result; +//Legacy code +icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_transf_guess) +{ + return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _transf_guess); } void ICP::printLaserData(LDP &laser_data) diff --git a/src/icp.h b/src/icp.h index d6345fc992ea5657445b02f2fdd04b5d614eacbb..389a616024efadd9af042fe8be0dd2a22495f046 100644 --- a/src/icp.h +++ b/src/icp.h @@ -1,10 +1,10 @@ #ifndef ICP_H_ #define ICP_H_ -#include <chrono> -#include <random> #include "laser_scan.h" +#include <chrono> #include <csm/csm_all.h> +#include <random> // using namespace CSM; namespace laserscanutils{ @@ -19,12 +19,48 @@ struct icpOutput{ struct icpParams{ int use_point_to_line_distance; - int max_correspondence_dist; + 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; + double max_angular_correction_deg; + 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; + 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 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; }; class ICP @@ -33,12 +69,19 @@ class ICP ICP(); ~ICP(); - static icpOutput align(const LaserScan &_last_ls, const LaserScan &_reference_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_last_transf); - - static void printTwoLaserData(sm_params ¶ms); - static void printLaserData(LDP &laser_data); -}; + static icpOutput align(const LaserScan &_current_ls, + const LaserScan &_ref_ls, + const LaserScanParams &_current_scan_params, + const LaserScanParams &_ref_scan_params, + const icpParams &_icp_params, + Eigen::Vector3s &_transf_ref_current); + static icpOutput align(const LaserScan &_last_ls, const LaserScan &_reference_ls, + const LaserScanParams &scan_params, const icpParams &icp_params, + Eigen::Vector3s &_last_transf); + static void printTwoLaserData(sm_params & params); + static void printLaserData(LDP & laser_data); + }; } #endif diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp index 7c0a34e06ad1d27ae02a115cc8450c9f7b97e239..3747de309ea2c1b26b6f7fbe8894217fb2d26991 100644 --- a/src/laser_scan.cpp +++ b/src/laser_scan.cpp @@ -2,7 +2,17 @@ namespace laserscanutils { - +void LaserScanParams::print() const{ + std::cout << "DBG PRINTING LASERSCANPARAMS ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl; + std::cout << "DBG angle_min_: " << angle_min_ << std::endl; + std::cout << "DBG angle_max_: " << angle_max_ << std::endl; + std::cout << "DBG angle_step_: " << angle_step_ << std::endl; + std::cout << "DBG scan_time_: " << scan_time_ << std::endl; + std::cout << "DBG range_min_: " << range_min_ << std::endl; + std::cout << "DBG range_max_: " << range_max_ << std::endl; + std::cout << "DBG range_std_dev_: " << range_std_dev_ << std::endl; + std::cout << "DBG angle_std_dev_: " << angle_std_dev_ << std::endl; +} LaserScan::LaserScan() : is_raw_processed_(false) { @@ -28,7 +38,7 @@ LaserScan::~LaserScan() bool LaserScan::isRawProcessed() const { - return is_raw_processed_; + return is_raw_processed_; } bool LaserScan::checkScanCorrectness(unsigned int _idx, unsigned int _idx_range) const @@ -40,12 +50,12 @@ bool LaserScan::checkScanCorrectness(unsigned int _idx, unsigned int _idx_range) //set loop bounds unsigned int ii_init = std::max( (unsigned int)0 , (_idx-_idx_range) ); unsigned int ii_end = std::min( (unsigned int)(ranges_.size()-1) , (_idx+_idx_range) ); - + //proceed for (unsigned int ii=ii_init; ii<=ii_end; ii++ ) if (ranges_[ii] < 0) return false; - + //return return true; } @@ -55,16 +65,16 @@ bool LaserScan::checkScanJumps(unsigned int _idx, unsigned int _idx_range) const //first of all check if scan has been raw processed if ( ! is_raw_processed_ ) return true; - + //set loop bounds unsigned int ii_init = std::max( (unsigned int)0 , (_idx-_idx_range) ); unsigned int ii_end = std::min( (unsigned int)(ranges_.size()-1) , (_idx+_idx_range) ); - + //proceed for (unsigned int ii=ii_init; ii<=ii_end; ii++ ) if ( jumps_mask_[ii] ) return true; - + //return return false; } @@ -94,65 +104,55 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _ //invalid range if (!isValidRange(ranges_raw_[ii], _scan_params)) { - // fix if it is a single value between two valid values - if (ii > 0 && ii < ranges_raw_.size() && isValidRange(ranges_raw_[ii-1], _scan_params) && isValidRange(ranges_raw_[ii+1], _scan_params)) - { - ranges_[ii] = (ranges_raw_[ii-1] + ranges_raw_[ii+1])/2.0; - } - // definitely invalid range - else - { - ranges_[ii] = -1.; - jumps_mask_[ii] = true; - points_all_.col(ii) << 0, 0, 0; - //prev_range = 0; + //set as valid range + ranges_[ii] = ranges_raw_[ii]; - //increment azimuth with angle step - azimuth += _scan_params.angle_step_; - continue; - } - } - // valid range - ranges_[ii] = ranges_raw_[ii]; + //transform the laser hit from polar to 3D euclidean homogeneous + point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1; - //transform the laser hit from polar to 3D euclidean homogeneous - point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1; + //apply device mounting point calibration (p_r = T * p_l) + point_ref = _device_T * point_laser; - //apply device mounting point calibration (p_r = T * p_l) - point_ref = _device_T * point_laser; + //set to points_all_ as a 2D homogeneous + points_all_.col(ii) << point_ref(0), point_ref(1), 1; - //set to points_all_ as a 2D homogeneous - points_all_.col(ii) << point_ref(0), point_ref(1), 1; + //set to points_ as a 2D homogeneous + points_.col(ii_ok) << point_ref(0), point_ref(1), 1; - //set to points_ as a 2D homogeneous - points_.col(ii_ok) << point_ref(0), point_ref(1), 1; - - //check jump. - //Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times - if (fabs(ranges_[ii] - prev_range) > _jump_th) //jump condition - { - jumps_indexes_.push_back(ii_ok); //indexes over points_ - jumps_mask_[ii] = true; //masks over ranges_ - } - else - jumps_mask_[ii] = false; + //check jump. + //Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times + if (fabs(ranges_[ii] - prev_range) > _jump_th) //jump condition + { + jumps_indexes_.push_back(ii_ok); //indexes over points_ + jumps_mask_[ii] = true; //masks over ranges_ + } + else + jumps_mask_[ii] = false; - //increment ok counter - ii_ok++; + //increment ok counter + ii_ok++; - //keep current range as previous for the next iteration - prev_range = ranges_raw_[ii]; + //keep current range as previous for the next iteration + prev_range = ranges_raw_[ii]; + } + else //invalid raw value + { + ranges_[ii] = -1.; + jumps_mask_[ii] = true; + points_all_.col(ii) << 0, 0, 0; + //prev_range = 0; + } - //increment azimuth with angle step + //increment azimuth with angle step azimuth += _scan_params.angle_step_; } - + //push back the last index to jumps_indexes_, to properly close the list. This will be used by findSegments() jumps_indexes_.push_back(ii_ok); - + //resize the output matrix to the number of correct points, while keeping values points_.conservativeResize(3, ii_ok); - + //raise the flag is_raw_processed_ = true; } @@ -345,21 +345,21 @@ void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<lase //in case ths input scan is not yet raw processed, do it if (!is_raw_processed_) ranges2xy(_scan_params, Eigen::Matrix4s::Identity(), _jump_th_segment); - - //run over all jumps (except the last, which indicates the closing index) + + //run over all jumps (except the last, which indicates the closing index) for (auto jumps_it = jumps_indexes_.begin(); jumps_it != std::prev(jumps_indexes_.end()); jumps_it++) { //new segment in the list _segment_list.push_back(ScanSegment()); - //check how many points + //check how many points num_points = (*std::next(jumps_it)) - (*jumps_it); //fill points _segment_list.back().points_.resize(3, num_points); for (unsigned int ii = 0; ii < num_points; ii++) _segment_list.back().points_.col(ii) << this->points_.col((*jumps_it) + ii); - + //set segment attributes _segment_list.back().idx_first_ = (*jumps_it); _segment_list.back().idx_last_ = (*jumps_it + num_points -1); @@ -383,4 +383,3 @@ void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<lase } }//namespace -