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 938022e920cddb287f773480596ffe497d8a0306..761f180a353273abb241a3c096736df2ff3ef6f6 100644 --- a/cmake_modules/Findcsm.cmake +++ b/cmake_modules/Findcsm.cmake @@ -46,7 +46,7 @@ macro(csm_report_not_found REASON_MSG) # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by # FindPackage() use the camelcase library name, not uppercase. if (csm_FIND_QUIETLY) - message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN}) + message(STATUS "Ignoring csm dependency- " ${REASON_MSG} ${ARGN}) else (csm_FIND_REQUIRED) message(FATAL_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN}) else() diff --git a/src/icp.cpp b/src/icp.cpp index 2abc4fc29104909902fa59ce3d2b6b1c87f85ba2..f03a0f0f62a17344f02051a53bb47916f8f9b9f9 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -126,15 +126,15 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con } //Legacy code -icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_last_transf) +icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_transf_guess) { // 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); + LDWrapper current = LDWrapper(_current_ls, scan_params); + LDWrapper ref = LDWrapper(_ref_ls, scan_params); - int num_rays = _last_ls.ranges_raw_.size(); + int num_rays = _current_ls.ranges_raw_.size(); sm_params csm_input{}; sm_result csm_output{}; @@ -143,12 +143,12 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con 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.laser_ref = ref.laser_data; + csm_input.laser_sens = current.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.first_guess[0] = _transf_guess(0); + csm_input.first_guess[1] = _transf_guess(1); + csm_input.first_guess[2] = _transf_guess(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; @@ -183,7 +183,7 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con else { std::cout << "ICP valid != 1, providing first guess transformation and identity covariance\n"; - result.res_transf = _last_transf; + result.res_transf = _transf_guess; result.res_covar = Eigen::Matrix3s::Identity(); } diff --git a/src/icp.h b/src/icp.h index 740e28e72f73f109cb4346eaf0b538517264416b..8fa7e7994e5d99ef2e6879e51683f8b6771fb93b 100644 --- a/src/icp.h +++ b/src/icp.h @@ -19,7 +19,7 @@ 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; diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp index b4eb9d0cecab1dd3232fe08eac43bb0f5a83133b..6df84b75674f842af6adce9a5add6a94e96a51dc 100644 --- a/src/laser_scan.cpp +++ b/src/laser_scan.cpp @@ -2,7 +2,17 @@ namespace laserscanutils { - +void LaserScanParams::print() const{ + std::cout << "PRINTING LASERSCANPARAMS ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl; + std::cout << "angle_min_: " << angle_min_ << std::endl; + std::cout << "angle_max_: " << angle_max_ << std::endl; + std::cout << "angle_step_: " << angle_step_ << std::endl; + std::cout << "scan_time_: " << scan_time_ << std::endl; + std::cout << "range_min_: " << range_min_ << std::endl; + std::cout << "range_max_: " << range_max_ << std::endl; + std::cout << "range_std_dev_: " << range_std_dev_ << std::endl; + std::cout << "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; } @@ -95,20 +105,20 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _ { //set as 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; - + //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_ as a 2D homogeneous points_.col(ii_ok) << point_ref(0), point_ref(1), 1; - - //check jump. + + //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 { @@ -117,10 +127,10 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _ } else jumps_mask_[ii] = false; - + //increment ok counter ii_ok++; - + //keep current range as previous for the next iteration prev_range = ranges_raw_[ii]; } @@ -131,17 +141,17 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _ points_all_.col(ii) << 0, 0, 0; //prev_range = 0; } - + //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; } @@ -323,21 +333,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); @@ -361,4 +371,3 @@ void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<lase } }//namespace -