Skip to content
Snippets Groups Projects
Commit 9681e4e4 authored by andreucm's avatar andreucm
Browse files

Hough line detection improved. Scan points now allowed to vote for a sinlge line

parent f9ee2867
No related branches found
No related tags found
No related merge requests found
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
//includes from Eigen lib //includes from Eigen lib
#include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry> #include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Sparse>
//namespace laserscanutils; //namespace laserscanutils;
......
...@@ -33,27 +33,37 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params) ...@@ -33,27 +33,37 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points, unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
std::list<laserscanutils::LineSegment> & _line_list) std::list<laserscanutils::LineSegment> & _line_list)
{ {
ScalarT theta, range; ScalarT theta, range, max_value;
unsigned int ii_max_value, jj_max_value;
int kr; int kr;
LineSegment line; LineSegment line;
ScalarT xmax, xmin, ymax, ymin; ScalarT xmax, xmin, ymax, ymin;
Eigen::Vector3s q_pt, p_pt; //auxiliary points Eigen::Vector3s q_pt, p_pt; //auxiliary points
Eigen::MatrixXs pts; //auxiliar array of points Eigen::MatrixXs pts; //auxiliar array of points
std::list<Eigen::Vector3s>::iterator pts_it; std::list<Eigen::Vector3s>::iterator pts_it;
Eigen::SparseMatrix<unsigned int> sum_grid(hough_grid_rows,hough_grid_cols);
std::list<Eigen::SparseMatrix<unsigned int> >::iterator subgrid_it;
Eigen::MatrixXs support_points;
//clear the Houhg Grid //clear the Houhg Grid
for (unsigned int ii = 0; ii < hough_grid_.size(); ii++) // for (unsigned int ii = 0; ii < hough_grid_.size(); ii++)
{ // {
for (unsigned int jj = 0; jj < hough_grid_.at(ii).size(); jj++) // for (unsigned int jj = 0; jj < hough_grid_.at(ii).size(); jj++)
{ // {
hough_grid_.at(ii).at(jj).clear(); // hough_grid_.at(ii).at(jj).clear();
} // }
} // }
hough_grid_x_.clear();
//STEP 1. For each scan point, accumulate hits in the Hough Grid //STEP 1. For each scan point, accumulate hits in the Hough Grid
for (unsigned int ipt = 0; ipt < _points.cols(); ipt++) //loop over all points for (unsigned int ipt = 0; ipt < _points.cols(); ipt++) //loop over all points
{ {
for (unsigned int jth = 0; jth < hough_grid_rows; jth++) //loop over all theta values in the grid //push_back a new Sparse Matrix to hough_grid_. Each SparseMatrix represents the grid for one point.
hough_grid_x_.push_back(Eigen::SparseMatrix<unsigned int>(hough_grid_rows,hough_grid_cols));
//loop over all theta values in the grid
for (unsigned int jth = 0; jth < hough_grid_rows; jth++)
{ {
//compute Real values of theta and range //compute Real values of theta and range
theta = jth*hough_params_.theta_step_; theta = jth*hough_params_.theta_step_;
...@@ -66,80 +76,180 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points, ...@@ -66,80 +76,180 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
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 homogeneous coordinates //Add support to cell(jth,kr), by pushing back the point homogeneous coordinates
hough_grid_.at(jth).at(kr).push_back( Eigen::Vector3s(_points(0,ipt),_points(1,ipt),1 )); //hough_grid_.at(jth).at(kr).push_back( Eigen::Vector3s(_points(0,ipt),_points(1,ipt),1 ));
//Anotate the support of point ipt to cell (jth,kr)
hough_grid_x_.back().coeffRef(jth,kr) = 1;
} }
} }
} }
////STEP 2 .Find cells having a peak of at least min_supports_ points supporting them ////STEP 2 .Find cells having a peak of at least min_supports_ points supporting them
for (unsigned int ii = 1; ii < hough_grid_rows-1; ii++) while ( hough_grid_x_.size() > hough_params_.min_supports_ )
{ {
for (unsigned int jj = 1; jj < hough_grid_cols-1; jj++) //sum all subgrids
sum_grid.setZero();
for ( subgrid_it = hough_grid_x_.begin(); subgrid_it != hough_grid_x_.end(); subgrid_it++ )
{
sum_grid = sum_grid + (*subgrid_it);
}
//Find the max_value at sum_grid -> r,th TODO: improve it by exploiting sparsity (not running over all matrix!)
max_value = 0;
for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
{ {
//check min supports for (unsigned int jj = 0; jj < hough_grid_cols; jj++)
unsigned int cell_supports = hough_grid_.at(ii).at(jj).size();
if( cell_supports >= hough_params_.min_supports_ )
{ {
//check if cell ii,jj is a peak ( 8 neighboring cells should be below in number of supports) if ( sum_grid.coeffRef(ii,jj) > max_value )
if ( ( cell_supports >= hough_grid_.at(ii-1).at(jj-1).size() ) && {
( cell_supports >= hough_grid_.at(ii-1).at(jj).size() ) && max_value = sum_grid.coeffRef(ii,jj);
( cell_supports >= hough_grid_.at(ii-1).at(jj+1).size() ) && ii_max_value = ii;
( cell_supports >= hough_grid_.at(ii).at(jj-1).size() ) && jj_max_value = jj;
( cell_supports >= hough_grid_.at(ii).at(jj+1).size() ) && }
( cell_supports >= hough_grid_.at(ii+1).at(jj-1).size() ) && }
( cell_supports >= hough_grid_.at(ii+1).at(jj).size() ) && }
( cell_supports >= hough_grid_.at(ii+1).at(jj+1).size() ) )
{ //max_value is the number of supporters for the most supported cell. Ceck if it has enough supporters
//find best fitting line with the supporting points if ( max_value < hough_params_.min_supports_ )
pts.resize(3,cell_supports); //each point in a column {
unsigned int kk; break; //Exit while(), since the max_value does not get the min_supports_
for(kk=0, pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++, kk++) }
{ else
pts.block<3,1>(0,kk) << *pts_it; //copy point {
} //resize supporter set
this->fitLine(pts, line); support_points.resize(3,max_value);
//set the line hough params TODO: Compute them from fitLine result, not from the Grid !! //select points supporting the cell of max value
line.np_ = hough_grid_.at(ii).at(jj).size(); //supporters unsigned int ipt = 0; //index over _points
line.theta_ = ii*hough_params_.theta_step_; //theta unsigned int jpt = 0; //index over support_points
line.range_ = jj*hough_params_.range_step_; //range for (ipt = 0, subgrid_it = hough_grid_x_.begin(); ipt < _points.cols(), subgrid_it != hough_grid_x_.end(); ipt++ )
{
//set starting and ending points of the line if ( subgrid_it->coeff(ii_max_value,jj_max_value) != 0 ) //point ipt hit this cell
//TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0) {
xmax=-100; xmin=100; ymax=-100; ymin=100; //copy ipt point to support_points
for (pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++) support_points.block<3,1>(0,jpt) << _points.block<3,1>(0,ipt);
{ jpt++;
//find xmax, xmin, ymax, ymin
if (pts_it->x() > xmax) xmax = pts_it->x();
if (pts_it->y() > ymax) ymax = pts_it->y();
if (pts_it->x() < xmin) xmin = pts_it->x();
if (pts_it->y() < ymin) ymin = pts_it->y();
}
if (ii < hough_grid_rows_half) //theta in [0,PI/2]
{
q_pt << xmin,ymax,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_first_ << p_pt;
q_pt << xmax,ymin,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_last_ << p_pt;
}
else //theta in [PI/2,PI]
{
q_pt << xmin,ymin,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_first_ << p_pt;
q_pt << xmax,ymax,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_last_ << p_pt;
}
//push back the line to the list //remove this subgrid
_line_list.push_back(line); subgrid_it = hough_grid_x_.erase(subgrid_it);
} }
else //point ipt didn't hit the cell, just carry on
{
subgrid_it ++;
}
}
//fit a line with the supporter points (support_points)
this->fitLine(support_points, line);
//set the line hough params TODO: Compute them from fitLine result, not from the Grid !!
line.np_ = max_value; //supporters
line.theta_ = ii_max_value*hough_params_.theta_step_; //theta
line.range_ = jj_max_value*hough_params_.range_step_; //range
//set starting and ending points of the line
//TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0)
xmax=-100; xmin=100; ymax=-100; ymin=100;
for (jpt = 0; jpt<support_points.cols(); jpt++ )
{
//find xmax, xmin, ymax, ymin
if (support_points(0,jpt) > xmax) xmax = support_points(0,jpt);
if (support_points(1,jpt) > ymax) ymax = support_points(1,jpt);
if (support_points(0,jpt) < xmin) xmin = support_points(0,jpt);
if (support_points(1,jpt) < ymin) ymin = support_points(1,jpt);
} }
if (ii_max_value < hough_grid_rows_half) //theta in [0,PI/2]
{
q_pt << xmin,ymax,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_first_ << p_pt;
q_pt << xmax,ymin,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_last_ << p_pt;
}
else //theta in [PI/2,PI]
{
q_pt << xmin,ymin,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_first_ << p_pt;
q_pt << xmax,ymax,1;
pointProjectionToLine(line.abc_, q_pt, p_pt);
line.point_last_ << p_pt;
}
//push back the line to the list
_line_list.push_back(line);
} }
} }
// for (unsigned int ii = 1; ii < hough_grid_rows-1; ii++)
// {
// for (unsigned int jj = 1; jj < hough_grid_cols-1; jj++)
// {
// //check min supports
// unsigned int cell_supports = hough_grid_.at(ii).at(jj).size();
// if( cell_supports >= hough_params_.min_supports_ )
// {
// //check if cell ii,jj is a peak ( 8 neighboring cells should be below in number of supports)
// if ( ( cell_supports >= hough_grid_.at(ii-1).at(jj-1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii-1).at(jj).size() ) &&
// ( cell_supports >= hough_grid_.at(ii-1).at(jj+1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii).at(jj-1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii).at(jj+1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii+1).at(jj-1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii+1).at(jj).size() ) &&
// ( cell_supports >= hough_grid_.at(ii+1).at(jj+1).size() ) )
// {
// //find best fitting line with the supporting points
// pts.resize(3,cell_supports); //each point in a column
// unsigned int kk;
// for(kk=0, pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++, kk++)
// {
// pts.block<3,1>(0,kk) << *pts_it; //copy point
// }
// this->fitLine(pts, line);
//
// //set the line hough params TODO: Compute them from fitLine result, not from the Grid !!
// line.np_ = hough_grid_.at(ii).at(jj).size(); //supporters
// line.theta_ = ii*hough_params_.theta_step_; //theta
// line.range_ = jj*hough_params_.range_step_; //range
//
// //set starting and ending points of the line
// //TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0)
// xmax=-100; xmin=100; ymax=-100; ymin=100;
// for (pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++)
// {
// //find xmax, xmin, ymax, ymin
// if (pts_it->x() > xmax) xmax = pts_it->x();
// if (pts_it->y() > ymax) ymax = pts_it->y();
// if (pts_it->x() < xmin) xmin = pts_it->x();
// if (pts_it->y() < ymin) ymin = pts_it->y();
// }
// if (ii < hough_grid_rows_half) //theta in [0,PI/2]
// {
// q_pt << xmin,ymax,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_first_ << p_pt;
// q_pt << xmax,ymin,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_last_ << p_pt;
// }
// else //theta in [PI/2,PI]
// {
// q_pt << xmin,ymin,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_first_ << p_pt;
// q_pt << xmax,ymax,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_last_ << p_pt;
// }
//
// //push back the line to the list
// _line_list.push_back(line);
// }
// }
// }
// }
//return the number of lines detected //return the number of lines detected
return _line_list.size(); return _line_list.size();
......
...@@ -35,6 +35,7 @@ class LineFinderHough : public LineFinder ...@@ -35,6 +35,7 @@ class LineFinderHough : public LineFinder
//Hough grid in theta-range space. Each cell holds a list of the point coordinates supporting the cell //Hough grid in theta-range space. Each cell holds a list of the point coordinates supporting the cell
std::vector<std::vector<std::list<Eigen::Vector3s > > > hough_grid_; std::vector<std::vector<std::list<Eigen::Vector3s > > > hough_grid_;
std::list<Eigen::SparseMatrix<unsigned int> > hough_grid_x_; //hough grid is a list of grids. Each one corresponds to the votes of a single point
//dereived Hough params //dereived Hough params
unsigned int hough_grid_rows; unsigned int hough_grid_rows;
......
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