Skip to content
Snippets Groups Projects
Commit 65005a8e authored by acoromin's avatar acoromin
Browse files

Working version. Filling start and end points to Hough lines. Not yet tested.

parent 517e1529
No related branches found
No related tags found
No related merge requests found
...@@ -17,8 +17,8 @@ namespace laserscanutils ...@@ -17,8 +17,8 @@ namespace laserscanutils
ScalarT error_; //sum of all distances from used points to line 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 first_; //index of the range vector of the first point used
unsigned int last_; //index of the range vector of the last 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_first_; //homogeneous coordinates of the starting 2D point
Eigen::Vector3s point_last_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0 Eigen::Vector3s point_last_; //homogeneous coordinates of the ending 2D point
unsigned int np_; // number of points of the line unsigned int np_; // number of points of the line
double range_; //range component in polar coordinates double range_; //range component in polar coordinates
double theta_; //theta component in polar coordinates double theta_; //theta component in polar coordinates
......
...@@ -209,19 +209,22 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX ...@@ -209,19 +209,22 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
std::list<laserscanutils::Line> & _line_list ) std::list<laserscanutils::Line> & _line_list )
{ {
double theta, range; double theta, range;
unsigned int kr; int kr;
Line line; Line line;
//A 2D array of lists. Each cell is a (r,theta) discretization in the line parameter space. //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 //Each list keeps the laser point coordinates that support that cell
std::vector<std::vector<std::list<std::pair<double,double> > > > hough_grid; 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 //clear line list
_line_list.clear(); _line_list.clear();
//resize hough_grid according range and theta steps and bounds //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_rows = (unsigned int)ceil(M_PI/_alg_params.theta_step_);//[0,PI]
unsigned int hough_grid_cols = (unsigned int)ceil(_alg_params.range_max_/_alg_params.range_step_); 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); hough_grid.resize(hough_grid_rows);
for (unsigned int ii = 0; ii < hough_grid_rows; ii++) for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
{ {
...@@ -236,13 +239,13 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX ...@@ -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 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 //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) 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 //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 ) ) if( ( kr >= 0 ) && ( kr < hough_grid_cols ) )
{ {
//Add support to cell(jth,kr), by pushing back the point coordinates //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 ...@@ -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 //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 ii = 0; ii < hough_grid_rows; ii++)
{ {
for (unsigned int jj = 0; jj < hough_grid_cols; jj++) for (unsigned int jj = 0; jj < hough_grid_cols; jj++)
{ {
if( hough_grid.at(ii).at(jj).size() >= _alg_params.min_supports_ ) if( hough_grid.at(ii).at(jj).size() >= _alg_params.min_supports_ )
{ {
//push ii,jj pair as candidate //set the line hough params
best_cells.push_back( std::pair<unsigned int,unsigned int>(ii,jj) ); 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 //clustering over candidates
std::list<std::pair<unsigned int,unsigned int> >::iterator 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++) // for (it_best_cells = best_cells.begin(); it_best_cells != best_cells.end(); it_best_cells++)
{ // {
//
} // }
//get the 10 most supported lines //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. // std::list<unsigned int> peak_values; //list of the ten highest peak values in the hough_grid. Last the highest.
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment