Hough Line extraction debugged and checked with TNO rosbag. It remains to...

Hough Line extraction debugged and checked with TNO rosbag. It remains to modify entry type of laser points

src/line_detector.cpp

@@ -218,8 +218,8 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
     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(); 
+    //clear line list 
+    _line_list.clear(); //TODO: TO BE DONE OUTSIDE!!
     
     //resize hough_grid according range and theta steps and bounds
     unsigned int hough_grid_rows = (unsigned int)ceil(M_PI/_alg_params.theta_step_);//[0,PI]
@@ -240,7 +240,7 @@ 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;
+                theta = jth*_alg_params.theta_step_;
                 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
@@ -269,6 +269,7 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
                 line.range_ = jj*_alg_params.range_step_; //range
 
                 //find xmax, xmin, ymax, ymin
+                xmax=-100; xmin=100; ymax=-100; ymin=100; 
                 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; 
@@ -280,13 +281,14 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
                 //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;
+                    line.point_first_ << xmin,ymax,1;
+                    line.point_last_ << xmax,ymin,1;
                     
                 }
                 else //second and fourth quartile
                 {
-                    
+                    line.point_first_ << xmin,ymin,1;
+                    line.point_last_ << xmax,ymax,1;                    
                 }
                 
                 //push back the line to the list
@@ -352,8 +354,6 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
 }
     
 
-
-
 laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2)
 {
     // lines should be normalized