First implementation of Hough transform for line detection. Not fully debugged

src/entities.cpp

@@ -7,12 +7,14 @@
 void laserscanutils::Line::print() const
 {
     std::cout << "Line Parameters : " << std::endl 
-              << "   (a,b,c): " << vector_.transpose() << std::endl
-              << "   error: " << error_ << std::endl
-              << "   first,last: " << first_ << " , " << last_ << std::endl
-              << "   first point: " << point_first_.transpose() << std::endl
-              << "   last point: " << point_last_.transpose() << std::endl
-              << "   number of points: " << np_ << std::endl;
+//               << "   (a,b,c): " << vector_.transpose() << std::endl
+//               << "   error: " << error_ << std::endl
+//               << "   first,last: " << first_ << " , " << last_ << std::endl
+//               << "   first point: " << point_first_.transpose() << std::endl
+//               << "   last point: " << point_last_.transpose() << std::endl
+              << "   number of points: " << np_ << std::endl
+              << "   range: " << range_ << std::endl
+              << "   theta: " << theta_ << std::endl;
 }
 
 float laserscanutils::Line::length()

src/entities.h

@@ -20,10 +20,14 @@ namespace laserscanutils
         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
         unsigned int np_; // number of points of the line
+        double range_; //range component in polar coordinates
+        double theta_; //theta component in polar coordinates
         //TODO: add an Eigen::Map to the supporting points ... but who ensures memory allocation of such points ???
 
         //just a print method
         void print() const;
+        
+        //returns length
         float length();
     };
     

src/line_detector.cpp

@@ -203,6 +203,82 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
     return _line_list.size();
 }
 
+
+unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixXd> & _laser_cloud,
+                                                const ExtractLineParamsHough & _alg_params, 
+                                                std::list<laserscanutils::Line> & _line_list )
+{
+    double theta, range; 
+    unsigned 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; 
+    
+    //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_);
+    hough_grid.resize(hough_grid_rows);
+    for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
+    {
+        hough_grid[ii].resize(hough_grid_cols);
+    }
+    
+    //For each input point, register all the supports to the hough_grid
+    for (unsigned int laser_id = 0; laser_id < _laser_cloud.size(); laser_id++) //loop on the laser devices
+    {
+        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++)
+            {
+                //compute Real values of theta and range
+                theta = jth*(M_PI/2.);
+                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_);
+                
+                //check validity of the dicretized values: TODO: take into account negative ranges
+                if( ( kr >= 0 ) && ( kr < hough_grid_cols ) )
+                {
+                    //Add support to cell(jth,kr), by pushing back the point coordinates
+                    hough_grid.at(jth).at(kr).push_back( std::pair<double,double>(_laser_cloud.at(laser_id)(0,ipt),_laser_cloud.at(laser_id)(1,ipt)) );
+                }
+            }
+        }
+    }
+    
+    //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);
+            }
+        }
+    }
+    
+    //return the number of lines detected
+    return _line_list.size(); 
+}
+    
+
+
+
 laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2)
 {
     // lines should be normalized

src/line_detector.h

@@ -28,7 +28,19 @@ namespace laserscanutils
         //just a print method
         void print() const;
     };
-
+    
+    /** \brief set of tunning parameters for the Hough transform line detection
+     * 
+     * set of tunning parameters for the Hough transform line detection
+     * 
+     **/
+    struct ExtractLineParamsHough
+    {
+        double range_max_; //maximum allowed range for lines
+        double range_step_; //range step in the voting grid
+        double theta_step_; //theta step in the voting grid
+        unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line
+    };
     
     /** \brief Find the best fittig line given a set of points
      * 
@@ -53,6 +65,27 @@ namespace laserscanutils
                               const ExtractLineParams & _alg_params, 
                               const std::vector<float> & _ranges, 
                               std::list<laserscanutils::Line> & _line_list);
+
+    /** \brief Extract lines using Hough transform. Result as a list of Line
+     *
+     * Extract lines from a set of scans.
+     * Returns Lines as a std::list<laserscanutils::Line>
+     * Requires: 
+     *    \param _laser_cloud Each component of std::vector is the 2D point data corresponding to one laser device, alreday in vehicle base coordinates.
+     *    \param _laser_data TODO: list of pairs, each one grouping ScanParams and a vector of ranges
+     * Provides: 
+     *    \param _line_list output lines extracted from the scan
+     *    \return Number of lines extracted.
+     * 
+     *
+     */
+//     unsigned int extractLinesHough(const std::list<std::pair<laserscanutils::ScanParams,const std::vector<float> > > _laser_data,
+//                               const ExtractLineParamsHough & _alg_params, 
+//                               std::list<laserscanutils::Line> & _line_list);
+    unsigned int extractLinesHough( const std::vector<Eigen::MatrixXd> & _laser_cloud,
+                                    const ExtractLineParamsHough & _alg_params, 
+                                    std::list<laserscanutils::Line> & _line_list);
+
     
     /** \brief Returns the angle between two lines
      *

src/scan_basics.cpp

@@ -92,6 +92,40 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
     //_points.conservativeResize(Eigen::NoChange_t, ii_ok);  //does not compile ... why ?
 }
 
+void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points)
+{
+    ScalarT azimuth = _params.angle_min_;
+    unsigned int ii = 0;
+
+    //resize to all points case
+    _points.resize(3,_ranges.size());
+
+    //for each range, check correctness of value and translate from polar to xy coordinates
+    while (ii <_ranges.size())
+    {
+        if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !std::isnan(_ranges[ii]) && !std::isinf(_ranges[ii]) )
+        {
+            //transform from polar to euclidean
+            _points(0,ii) = _ranges[ii] * cos(azimuth);
+            _points(1,ii) = _ranges[ii] * sin(azimuth);
+            _points(2,ii) = 1;
+
+            ii++;
+        }
+        else 
+        {
+            //erase the current range 
+            _ranges.erase(_ranges.begin()+ii);
+        }
+
+        //increment azimuth
+        azimuth += _params.angle_step_;
+    }
+
+    //resize the output matrix to the number of correct points, while keeping values
+    _points.conservativeResize(3, _ranges.size());
+}
+
 laserscanutils::ScalarT laserscanutils::pi2pi(const ScalarT& angle)
 {
     return (angle > 0 ? fmod(angle + M_PI, 2 * M_PI) - M_PI : fmod(angle - M_PI, 2 * M_PI) + M_PI);

src/scan_basics.h

@@ -61,6 +61,21 @@ namespace laserscanutils
     **/
     void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, const ScalarT& _jump_angle_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps);
 
+    /** \brief Transforms from ranges (polar) to euclidean (xy)
+     *
+     * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data.
+     * Invalid values (inf's and nan's) and out of range measurements are not transformed.
+     * Valid measurements are returned, by reference, in homogeneous coordinates.
+     * Ranges vector is returned with all not correct elements removed.
+     * Requires: 
+     *      \param _params laser_scan_utils structure of the laser scan parameters.
+     *      \param _ranges raw range measurements from the laser scanner device. Size Nr. Type float to match ROS LaserScan message
+     * Outputs: 
+     *      \param _points matrix of homogeneous coordinates of hit points. Size 3xNp, where Np can be <= than Nr (due to possible inf's and nan's from raw data)
+    **/
+    void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points);
+
+
     /** \brief Fits angle in (-pi, pi]
      *
      * Fits angle in (-pi, pi]