From 517e1529f87389ea1a9897bf26ef14a09ea61118 Mon Sep 17 00:00:00 2001
From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Mon, 11 Jan 2016 14:44:16 +0000
Subject: [PATCH] Working copy of Hough Line detector
---
src/line_detector.cpp | 72 +++++++++++++++++++++++++++++++++++++------
1 file changed, 62 insertions(+), 10 deletions(-)
diff --git a/src/line_detector.cpp b/src/line_detector.cpp
index 63f64a7..2760abc 100644
--- a/src/line_detector.cpp
+++ b/src/line_detector.cpp
@@ -233,7 +233,7 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
{
for (unsigned int ipt = 0; ipt < _laser_cloud.at(laser_id).cols(); ipt++) //loop over all points of laser_id
{
- for (unsigned int jth = 0; jth < hough_grid_rows; jth++)
+ 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.);
@@ -252,26 +252,78 @@ 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_ )
{
- //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);
+ //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++)
+ {
+
+ }
+
+ //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<std::pair<unsigned int,unsigned int> > peak_indexes; //list of the indexes corresponding to the list above
+// std::list<unsigned int>::iterator it_peak_values;
+// std::list<std::pair<unsigned int,unsigned int> >::iterator it_peak_indexes;
+//
+// for (unsigned int ii = 0; ii < hough_grid_rows; ii++) //loop over all theta values
+// {
+// for (unsigned int jj = 0; jj < hough_grid_cols; jj++) //loop over all range values
+// {
+// //set iterators at the beginning of the lists
+// it_peak_values = peak_values.begin();
+// it_peak_indexes = peak_indexes.begin();
+//
+// //fins if cell ii,jj has mor support that others in the peak_values list
+// 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);
+// }
+// }
+// }
+
+
//return the number of lines detected
return _line_list.size();
}
--
GitLab