From 65005a8e59e8c2c5ceb05af659160847a16bb37a Mon Sep 17 00:00:00 2001
From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Wed, 10 Feb 2016 13:58:59 +0000
Subject: [PATCH] Working version. Filling start and end points to Hough lines.
 Not yet tested.

---
 src/entities.h        |  4 +-
 src/line_detector.cpp | 90 +++++++++++++++++++++++++++----------------
 2 files changed, 58 insertions(+), 36 deletions(-)

diff --git a/src/entities.h b/src/entities.h
index c55dd4f..1917aa2 100644
--- a/src/entities.h
+++ b/src/entities.h
@@ -17,8 +17,8 @@ namespace laserscanutils
         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 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_last_; //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 coordinates of the ending 2D point
         unsigned int np_; // number of points of the line
         double range_; //range component in polar coordinates
         double theta_; //theta component in polar coordinates
diff --git a/src/line_detector.cpp b/src/line_detector.cpp
index 2760abc..f674add 100644
--- a/src/line_detector.cpp
+++ b/src/line_detector.cpp
@@ -209,19 +209,22 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
                                                 std::list<laserscanutils::Line> & _line_list )
 {
     double theta, range; 
-    unsigned int kr; 
+    int kr; 
     Line line;
     
     //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
     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(); 
     
     //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_cols = (unsigned int)ceil(_alg_params.range_max_/_alg_params.range_step_);
+    unsigned int hough_grid_rows = (unsigned int)ceil(M_PI/_alg_params.theta_step_);//[0,PI]
+    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);
     for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
     {
@@ -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
             {
                 //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)
                 
                 //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 ) )
                 {
                     //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
         }
     }
     
-//     //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_ )
             {
-                //push ii,jj pair as candidate
-                best_cells.push_back( std::pair<unsigned int,unsigned int>(ii,jj) );
+                //set the line hough params
+                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
-    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++)
-    {
-        
-    }
+//     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.
-- 
GitLab