Working copy of Hough Line detector

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(); 
 }