diff --git a/src/entities.cpp b/src/entities.cpp index 6cb9112764aa5545cc2a730aa6722c51af382646..da6d3d37fb6b9c6e47fb6aaf1073617ed886e95f 100644 --- a/src/entities.cpp +++ b/src/entities.cpp @@ -7,12 +7,14 @@ void laserscanutils::Line::print() const { std::cout << "Line Parameters : " << std::endl - << " (a,b,c): " << vector_.transpose() << std::endl - << " error: " << error_ << std::endl - << " first,last: " << first_ << " , " << last_ << std::endl - << " first point: " << point_first_.transpose() << std::endl - << " last point: " << point_last_.transpose() << std::endl - << " number of points: " << np_ << std::endl; +// << " (a,b,c): " << vector_.transpose() << std::endl +// << " error: " << error_ << std::endl +// << " first,last: " << first_ << " , " << last_ << std::endl +// << " first point: " << point_first_.transpose() << std::endl +// << " last point: " << point_last_.transpose() << std::endl + << " number of points: " << np_ << std::endl + << " range: " << range_ << std::endl + << " theta: " << theta_ << std::endl; } float laserscanutils::Line::length() diff --git a/src/entities.h b/src/entities.h index 65dc9c0697000071ea04d74a701c5661af6b835a..c55dd4f1415369f985bde8c2f10cdcb108742f74 100644 --- a/src/entities.h +++ b/src/entities.h @@ -20,10 +20,14 @@ namespace laserscanutils Eigen::Vector3s point_first_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0 Eigen::Vector3s point_last_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0 unsigned int np_; // number of points of the line + double range_; //range component in polar coordinates + double theta_; //theta component in polar coordinates //TODO: add an Eigen::Map to the supporting points ... but who ensures memory allocation of such points ??? //just a print method void print() const; + + //returns length float length(); }; diff --git a/src/line_detector.cpp b/src/line_detector.cpp index 4827be2ee7459c97e11468385f552e65d7bbde93..63f64a7d49c2cf2053c7ee960b08e6ce812dcd32 100644 --- a/src/line_detector.cpp +++ b/src/line_detector.cpp @@ -203,6 +203,82 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa return _line_list.size(); } + +unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixXd> & _laser_cloud, + const ExtractLineParamsHough & _alg_params, + std::list<laserscanutils::Line> & _line_list ) +{ + double theta, range; + unsigned int kr; + Line line; + + //A 2D array of lists. Each cell is a (r,theta) discretization in the line parameter space. + //Each list keeps the laser point coordinates that support that cell + std::vector<std::vector<std::list<std::pair<double,double> > > > hough_grid; + + //clear line list + _line_list.clear(); + + //resize hough_grid according range and theta steps and bounds + unsigned int hough_grid_rows = (unsigned int)ceil((M_PI/2.)/_alg_params.theta_step_); + unsigned int hough_grid_cols = (unsigned int)ceil(_alg_params.range_max_/_alg_params.range_step_); + hough_grid.resize(hough_grid_rows); + for (unsigned int ii = 0; ii < hough_grid_rows; ii++) + { + hough_grid[ii].resize(hough_grid_cols); + } + + //For each input point, register all the supports to the hough_grid + for (unsigned int laser_id = 0; laser_id < _laser_cloud.size(); laser_id++) //loop on the laser devices + { + for (unsigned int ipt = 0; ipt < _laser_cloud.at(laser_id).cols(); ipt++) //loop over all points of laser_id + { + for (unsigned int jth = 0; jth < hough_grid_rows; jth++) + { + //compute Real values of theta and range + theta = jth*(M_PI/2.); + range = _laser_cloud.at(laser_id)(0,ipt)*cos(theta) + _laser_cloud.at(laser_id)(1,ipt)*sin(theta); //r=xcos(th)+ysin(th) + + //discretize range + kr = (unsigned int)floor(range/_alg_params.range_step_); + + //check validity of the dicretized values: TODO: take into account negative ranges + if( ( kr >= 0 ) && ( kr < hough_grid_cols ) ) + { + //Add support to cell(jth,kr), by pushing back the point coordinates + hough_grid.at(jth).at(kr).push_back( std::pair<double,double>(_laser_cloud.at(laser_id)(0,ipt),_laser_cloud.at(laser_id)(1,ipt)) ); + } + } + } + } + + //Check cells having a list with >= min_supports_ members + for (unsigned int ii = 0; ii < hough_grid_rows; ii++) + { + for (unsigned int jj = 0; jj < hough_grid_cols; jj++) + { + if( hough_grid.at(ii).at(jj).size() >= _alg_params.min_supports_ ) + { + //set the line params + line.np_ = hough_grid.at(ii).at(jj).size(); + line.theta_ = ii*_alg_params.theta_step_; + line.range_ = jj*_alg_params.range_step_; + //line.point_first_ << ; + //line.point_last_ << ; + + //push back the line to the list + _line_list.push_back(line); + } + } + } + + //return the number of lines detected + return _line_list.size(); +} + + + + laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2) { // lines should be normalized diff --git a/src/line_detector.h b/src/line_detector.h index 869747c5efb869af6d0adef6de5f7646a822e0b1..6fb88ab4a3b30820ae7fdcef943ac8a5be450b0f 100644 --- a/src/line_detector.h +++ b/src/line_detector.h @@ -28,7 +28,19 @@ namespace laserscanutils //just a print method void print() const; }; - + + /** \brief set of tunning parameters for the Hough transform line detection + * + * set of tunning parameters for the Hough transform line detection + * + **/ + struct ExtractLineParamsHough + { + double range_max_; //maximum allowed range for lines + double range_step_; //range step in the voting grid + double theta_step_; //theta step in the voting grid + unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line + }; /** \brief Find the best fittig line given a set of points * @@ -53,6 +65,27 @@ namespace laserscanutils const ExtractLineParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Line> & _line_list); + + /** \brief Extract lines using Hough transform. Result as a list of Line + * + * Extract lines from a set of scans. + * Returns Lines as a std::list<laserscanutils::Line> + * Requires: + * \param _laser_cloud Each component of std::vector is the 2D point data corresponding to one laser device, alreday in vehicle base coordinates. + * \param _laser_data TODO: list of pairs, each one grouping ScanParams and a vector of ranges + * Provides: + * \param _line_list output lines extracted from the scan + * \return Number of lines extracted. + * + * + */ +// unsigned int extractLinesHough(const std::list<std::pair<laserscanutils::ScanParams,const std::vector<float> > > _laser_data, +// const ExtractLineParamsHough & _alg_params, +// std::list<laserscanutils::Line> & _line_list); + unsigned int extractLinesHough( const std::vector<Eigen::MatrixXd> & _laser_cloud, + const ExtractLineParamsHough & _alg_params, + std::list<laserscanutils::Line> & _line_list); + /** \brief Returns the angle between two lines * diff --git a/src/scan_basics.cpp b/src/scan_basics.cpp index 193f2bcf801f65baaf58b502fb3879dc3341dcdb..48a5429307be809cd66a67aeb95364e84d183cba 100644 --- a/src/scan_basics.cpp +++ b/src/scan_basics.cpp @@ -92,6 +92,40 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & //_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ? } +void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points) +{ + ScalarT azimuth = _params.angle_min_; + unsigned int ii = 0; + + //resize to all points case + _points.resize(3,_ranges.size()); + + //for each range, check correctness of value and translate from polar to xy coordinates + while (ii <_ranges.size()) + { + if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !std::isnan(_ranges[ii]) && !std::isinf(_ranges[ii]) ) + { + //transform from polar to euclidean + _points(0,ii) = _ranges[ii] * cos(azimuth); + _points(1,ii) = _ranges[ii] * sin(azimuth); + _points(2,ii) = 1; + + ii++; + } + else + { + //erase the current range + _ranges.erase(_ranges.begin()+ii); + } + + //increment azimuth + azimuth += _params.angle_step_; + } + + //resize the output matrix to the number of correct points, while keeping values + _points.conservativeResize(3, _ranges.size()); +} + laserscanutils::ScalarT laserscanutils::pi2pi(const ScalarT& angle) { return (angle > 0 ? fmod(angle + M_PI, 2 * M_PI) - M_PI : fmod(angle - M_PI, 2 * M_PI) + M_PI); diff --git a/src/scan_basics.h b/src/scan_basics.h index 42f8b955df671354c6d0967b1fbdf87fbb0d0194..37e1cfdce92233d6bc2db92d0312c94df6f7ee19 100644 --- a/src/scan_basics.h +++ b/src/scan_basics.h @@ -61,6 +61,21 @@ namespace laserscanutils **/ void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, const ScalarT& _jump_angle_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps); + /** \brief Transforms from ranges (polar) to euclidean (xy) + * + * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data. + * Invalid values (inf's and nan's) and out of range measurements are not transformed. + * Valid measurements are returned, by reference, in homogeneous coordinates. + * Ranges vector is returned with all not correct elements removed. + * Requires: + * \param _params laser_scan_utils structure of the laser scan parameters. + * \param _ranges raw range measurements from the laser scanner device. Size Nr. Type float to match ROS LaserScan message + * Outputs: + * \param _points matrix of homogeneous coordinates of hit points. Size 3xNp, where Np can be <= than Nr (due to possible inf's and nan's from raw data) + **/ + void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points); + + /** \brief Fits angle in (-pi, pi] * * Fits angle in (-pi, pi]