Removed old code lines from LineFinderHough::findLines()

src/line_finder_hough.cpp

@@ -47,13 +47,6 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
     std::vector<Eigen::Triplet<unsigned int> > triplet_vector;
         
     //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(); 
-//         }
-//     }
     hough_grid_x_.clear(); 
     
     
@@ -82,11 +75,7 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
             //check validity of the discretized values
             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 ));
-                
                 //Anotate the support of point ipt to cell (jth,kr)
-                //hough_grid_x_.back().coeffRef(jth,kr) = 1; 
                 triplet_vector.push_back(Eigen::Triplet<unsigned int>(jth,kr,1));
             }
         }
@@ -191,75 +180,6 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
         }
     }//closes while    
     
-//     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(); 
 }