From 65005a8e59e8c2c5ceb05af659160847a16bb37a Mon Sep 17 00:00:00 2001 From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c> Date: Wed, 10 Feb 2016 13:58:59 +0000 Subject: [PATCH] Working version. Filling start and end points to Hough lines. Not yet tested. --- src/entities.h | 4 +- src/line_detector.cpp | 90 +++++++++++++++++++++++++++---------------- 2 files changed, 58 insertions(+), 36 deletions(-) diff --git a/src/entities.h b/src/entities.h index c55dd4f..1917aa2 100644 --- a/src/entities.h +++ b/src/entities.h @@ -17,8 +17,8 @@ namespace laserscanutils ScalarT error_; //sum of all distances from used points to line unsigned int first_; //index of the range vector of the first point used unsigned int last_; //index of the range vector of the last point used - 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 + Eigen::Vector3s point_first_; //homogeneous coordinates of the starting 2D point + Eigen::Vector3s point_last_; //homogeneous coordinates of the ending 2D point unsigned int np_; // number of points of the line double range_; //range component in polar coordinates double theta_; //theta component in polar coordinates diff --git a/src/line_detector.cpp b/src/line_detector.cpp index 2760abc..f674add 100644 --- a/src/line_detector.cpp +++ b/src/line_detector.cpp @@ -209,19 +209,22 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX std::list<laserscanutils::Line> & _line_list ) { double theta, range; - unsigned int kr; + 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; + std::list<std::pair<double,double>::iterator pt_it; //iterator over the points of a given cell list //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_); + unsigned int hough_grid_rows = (unsigned int)ceil(M_PI/_alg_params.theta_step_);//[0,PI] + unsigned int hough_grid_rows_half = (unsigned int)ceil(0.5*M_PI/_alg_params.theta_step_);//middle row index + unsigned int hough_grid_cols = (unsigned int)ceil(2*_alg_params.range_max_/_alg_params.range_step_);//[-rmax,+rmax] + unsigned int hough_grid_cols_half = (unsigned int)ceil(_alg_params.range_max_/_alg_params.range_step_);//middle col index hough_grid.resize(hough_grid_rows); for (unsigned int ii = 0; ii < hough_grid_rows; ii++) { @@ -236,13 +239,13 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX for (unsigned int jth = 0; jth < hough_grid_rows; jth++) //loop over all theta values in the grid { //compute Real values of theta and range - theta = jth*(M_PI/2.); + theta = jth*M_PI; 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_); + kr = (int)floor(range/_alg_params.range_step_) + (int)hough_grid_cols_half ; - //check validity of the dicretized values: TODO: take into account negative ranges + //check validity of the dicretized values if( ( kr >= 0 ) && ( kr < hough_grid_cols ) ) { //Add support to cell(jth,kr), by pushing back the point coordinates @@ -252,46 +255,65 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX } } -// //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); -// } -// } -// } - //Check cells having a list with >= min_supports_ members - std::list<std::pair<unsigned int,unsigned int> > best_cells; //list of the indexes corresponding to the cells above the threshold 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_ ) { - //push ii,jj pair as candidate - best_cells.push_back( std::pair<unsigned int,unsigned int>(ii,jj) ); + //set the line hough params + line.np_ = hough_grid.at(ii).at(jj).size(); //supporters + line.theta_ = ii*_alg_params.theta_step_; //theta + line.range_ = jj*_alg_params.range_step_; //range + + //find xmax, xmin, ymax, ymin + for (pt_it = hough_grid.at(ii).at(jj).begin(); pt_it != hough_grid.at(ii).at(jj).end(); pt_it++) + { + if (pt_it->first > xmax) xmax = pt_it->first; + if (pt_it->second > ymax) ymax = pt_it->second; + if (pt_it->first < xmin) xmin = pt_it->first; + if (pt_it->second < ymin) ymin = pt_it->second; + } + + //set the limiting points of the line + if (ii < hough_grid_rows_half) //first and third quartile + { + line.point_first_ << ,,1; + line.point_last_ << ,,1; + + } + else //second and fourth quartile + { + + } + + //push back the line to the list + _line_list.push_back(line); } } } + + //Check cells having a list with >= min_supports_ members +// std::list<std::pair<unsigned int,unsigned int> > best_cells; //list of the indexes corresponding to the cells above the threshold +// 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_ ) +// { +// //push ii,jj pair as candidate +// best_cells.push_back( std::pair<unsigned int,unsigned int>(ii,jj) ); +// } +// } +// } //clustering over candidates - std::list<std::pair<unsigned int,unsigned int> >::iterator it_best_cells; - for (it_best_cells = best_cells.begin(); it_best_cells != best_cells.end(); it_best_cells++) - { - - } +// std::list<std::pair<unsigned int,unsigned int> >::iterator it_best_cells; +// for (it_best_cells = best_cells.begin(); it_best_cells != best_cells.end(); it_best_cells++) +// { +// +// } //get the 10 most supported lines // std::list<unsigned int> peak_values; //list of the ten highest peak values in the hough_grid. Last the highest. -- GitLab