diff --git a/src/laser_scan_utils.h b/src/laser_scan_utils.h
index c078c45b8fd5fbcf28cb0b92c060c40a43996016..4ba571945b71b5bbdf114bac11f6a0588c5d5ad5 100644
--- a/src/laser_scan_utils.h
+++ b/src/laser_scan_utils.h
@@ -5,6 +5,7 @@
//includes from Eigen lib
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
//namespace laserscanutils;
diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp
index 092ad5c201a4e1af8a418bc1b5039148aa4edb9b..2202db3ae2ffca4daab58722bcbac79117420422 100644
--- a/src/line_finder_hough.cpp
+++ b/src/line_finder_hough.cpp
@@ -33,27 +33,37 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
std::list<laserscanutils::LineSegment> & _line_list)
{
- ScalarT theta, range;
+ ScalarT theta, range, max_value;
+ unsigned int ii_max_value, jj_max_value;
int kr;
LineSegment line;
ScalarT xmax, xmin, ymax, ymin;
Eigen::Vector3s q_pt, p_pt; //auxiliary points
Eigen::MatrixXs pts; //auxiliar array of points
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
- for (unsigned int ii = 0; ii < hough_grid_.size(); ii++)
- {
- for (unsigned int jj = 0; jj < hough_grid_.at(ii).size(); jj++)
- {
- hough_grid_.at(ii).at(jj).clear();
- }
- }
+// for (unsigned int ii = 0; ii < hough_grid_.size(); ii++)
+// {
+// for (unsigned int jj = 0; jj < hough_grid_.at(ii).size(); jj++)
+// {
+// hough_grid_.at(ii).at(jj).clear();
+// }
+// }
+ hough_grid_x_.clear();
+
//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 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
theta = jth*hough_params_.theta_step_;
@@ -66,80 +76,180 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
if( ( kr >= 0 ) && ( kr < hough_grid_cols ) )
{
//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
- 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
- unsigned int cell_supports = hough_grid_.at(ii).at(jj).size();
- if( cell_supports >= hough_params_.min_supports_ )
+ for (unsigned int jj = 0; jj < hough_grid_cols; jj++)
{
- //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;
- }
+ if ( sum_grid.coeffRef(ii,jj) > max_value )
+ {
+ max_value = sum_grid.coeffRef(ii,jj);
+ ii_max_value = ii;
+ jj_max_value = jj;
+ }
+ }
+ }
+
+ //max_value is the number of supporters for the most supported cell. Ceck if it has enough supporters
+ if ( max_value < hough_params_.min_supports_ )
+ {
+ break; //Exit while(), since the max_value does not get the min_supports_
+ }
+ else
+ {
+ //resize supporter set
+ support_points.resize(3,max_value);
+
+ //select points supporting the cell of max value
+ unsigned int ipt = 0; //index over _points
+ unsigned int jpt = 0; //index over support_points
+ for (ipt = 0, subgrid_it = hough_grid_x_.begin(); ipt < _points.cols(), subgrid_it != hough_grid_x_.end(); ipt++ )
+ {
+ if ( subgrid_it->coeff(ii_max_value,jj_max_value) != 0 ) //point ipt hit this cell
+ {
+ //copy ipt point to support_points
+ support_points.block<3,1>(0,jpt) << _points.block<3,1>(0,ipt);
+ jpt++;
- //push back the line to the list
- _line_list.push_back(line);
+ //remove this subgrid
+ 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 _line_list.size();
diff --git a/src/line_finder_hough.h b/src/line_finder_hough.h
index e2a2c4c9d86219bfefa6c91f0ec53a0f40fa64dd..2f43810c672334129bdf790a21bd13a2322afb48 100644
--- a/src/line_finder_hough.h
+++ b/src/line_finder_hough.h
@@ -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
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
unsigned int hough_grid_rows;