diff --git a/src/line_detector.cpp b/src/line_detector.cpp index acc816425f5004e4eebf0994f3151ee89f094552..650930c624626a913368cbf1e8d08918c1a528c3 100644 --- a/src/line_detector.cpp +++ b/src/line_detector.cpp @@ -218,8 +218,8 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX 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(); + //clear line list + _line_list.clear(); //TODO: TO BE DONE OUTSIDE!! //resize hough_grid according range and theta steps and bounds unsigned int hough_grid_rows = (unsigned int)ceil(M_PI/_alg_params.theta_step_);//[0,PI] @@ -240,7 +240,7 @@ 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; + theta = jth*_alg_params.theta_step_; 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 @@ -269,6 +269,7 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX line.range_ = jj*_alg_params.range_step_; //range //find xmax, xmin, ymax, ymin + xmax=-100; xmin=100; ymax=-100; ymin=100; 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; @@ -280,13 +281,14 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX //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; + line.point_first_ << xmin,ymax,1; + line.point_last_ << xmax,ymin,1; } else //second and fourth quartile { - + line.point_first_ << xmin,ymin,1; + line.point_last_ << xmax,ymax,1; } //push back the line to the list @@ -352,8 +354,6 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX } - - laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2) { // lines should be normalized