detecció linies amb window size variable

src/examples/test_extract_corners.cpp

@@ -95,7 +95,8 @@ int main(int argc, char** argv)
 
     laserscanutils::ExtractCornerParams alg_params;
     alg_params.line_params_.jump_dist_ut_ = 1.0; 
-    alg_params.line_params_.window_sz_ = 6;
+    alg_params.line_params_.window_sz_ = 0.5;
+    alg_params.line_params_.min_window_points_ = 5;
     alg_params.line_params_.k_sigmas_ut_ = 3;
     alg_params.line_params_.concatenate_ii_ut_ = 5; 
     alg_params.line_params_.concatenate_angle_ut_ = 0.1; 

src/line_detector.cpp

@@ -35,52 +35,60 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
     Line line;
     std::list<Line>::iterator line_it1, line_it2;
     std::list<unsigned int> jumps;
+    std::vector<float> ranges = _ranges;
     std::list<unsigned int>::iterator jj_jump;
     bool update_iterators;
-    unsigned int ii1,ii2; 
+    unsigned int ii, window_points;
 
     //STEP 1: transform to euclidean coordinates and detect jumps
-    ranges2xy(_params, _ranges, _alg_params.jump_dist_ut_, points, jumps);
+    ranges2xy(_params, ranges, _alg_params.jump_dist_ut_, points, jumps);
 //     std::cout << "jumps: " << jumps.size() << std::endl;
 //     std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
     
     // STEP 2: find line segments running over the scan
-    ii1 = 0; 
-    ii2=_alg_params.window_sz_;
+    ii = 0;
+    //ii2=_alg_params.window_sz_;
+    window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])));
     jj_jump = jumps.begin();
-    while ( ii2 < points.cols() )
+
+
+    while ( ii+window_points < points.cols() )
     {
-        //check if a jump exists between ii1 and ii2
-        if ( jj_jump != jumps.end() )
+        //check if a jump exists between ii and ii+window_points
+        if ( jj_jump != jumps.end()  &&  ii < (*jj_jump) && (ii+window_points >= (*jj_jump)) )
+        {
+            ii = *jj_jump;
+            jj_jump++;
+        }
+        else
         {
-            if (ii1 < (*jj_jump) && (ii2 >= (*jj_jump)) ) 
+            //Found the best fitting line over points within the window [ii,ii+window_points]
+            fitLine(points.block(0,ii,3,window_points), line);
+
+            //if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
+            if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas_ut_ )
             {
-                ii1 = *jj_jump; 
-                ii2 = *jj_jump + _alg_params.window_sz_;
-                jj_jump++; 
-                continue;//move to while loop
+                line.first_ = ii;
+                line.last_ = ii + window_points;
+                line.point_first_ = points.col(line.first_);
+                line.point_last_ = points.col(line.last_);
+                line.np_ = line.last_ - line.first_ + 1;
+                _line_list.push_back(line);
             }
-        }
-        
-        //Found the best fitting line over points within the window [ii1,ii2]
-        fitLine(points.block(0,ii1,3,_alg_params.window_sz_), line);
 
-        //if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
-        if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas_ut_ )
-        {
-            line.first_ = ii1;
-            line.last_ = ii2;
-            line.point_first_ = points.col(line.first_);
-            line.point_last_ = points.col(line.last_);
-            line.np_ = line.last_ - line.first_ + 1;
-            _line_list.push_back(line);
+            //increment window iterators
+            ii++;
         }
-        
-        //increment window iterators
-        ii1++; 
-        ii2++;
+        window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])));
+        std::cout << "min window points: " << _alg_params.min_window_points_ << std::endl;
+        std::cout << "range: " << ranges[ii] << std::endl;
+        std::cout << "l_step: " << _params.angle_step_ * ranges[ii] << std::endl;
+        std::cout << "window_sz: " << _alg_params.window_sz_ << std::endl;
+        std::cout << "window size / l_step: " << (_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
+        std::cout << "(unsigned int) window size / l_step: " << (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
+        std::cout << "window_points: " << window_points << std::endl << std::endl;
     }
-//  std::cout << "Lines fitted: " << _line_list.size() << std::endl;
+  std::cout << "Lines fitted: " << _line_list.size() << std::endl;
 
     //STEP 3: concatenate lines
     if ( _line_list.size() < 2 ) return _line_list.size(); //In case just less than two lines found, return
@@ -134,7 +142,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
             line_it2++;            
         }
     }
-    //std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
+    std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
 
     return _line_list.size();
 }

src/line_detector.h

@@ -17,8 +17,9 @@ namespace laserscanutils
     struct ExtractLineParams
     {
         //members
-        double jump_dist_ut_; //Upper threshold in consecutive ranges to consider a jump
-        unsigned int window_sz_; //number of points to fit lines in the first pass
+        ScalarT jump_dist_ut_; //Upper threshold in consecutive ranges to consider a jump
+        ScalarT window_sz_; // size (m) of the window of points to fit lines in the first pass
+        unsigned int min_window_points_; // minimum number of points to fit lines in the first pass
         ScalarT k_sigmas_ut_;//Uppet threshold of how many std_dev are tolerated to count that a point is supporting a line
         unsigned int concatenate_ii_ut_;//Upper threshold for ray index difference between consecutive lines to consider concatenation
         ScalarT concatenate_angle_ut_; //Upper threshold for angle between consecutive lines to consider concatenation

src/scan_basics.cpp

@@ -12,11 +12,11 @@ void laserscanutils::ScanParams::print() const
                 << "   Range std dev: " << range_std_dev_ << std::endl;
 }
 
-void laserscanutils::ranges2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps)
+void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps)
 {
     ScalarT azimuth = 0.0;
     ScalarT prev_range;
-    unsigned int ii, ii_ok; 
+    unsigned int ii = 0;
     
     //resize to all points case
     _points.resize(3,_ranges.size());
@@ -25,28 +25,30 @@ void laserscanutils::ranges2xy(const ScanParams & _params, const std::vector<flo
     _jumps.clear(); 
     
     //for each range, check correctness of value and translate from polar to xy coordinates 
-    for(ii=0, ii_ok=0; ii<_ranges.size(); ++ii)
+    while (ii <_ranges.size())
     {
-        if ( _ranges[ii] >=_params.range_min_ && _ranges[ii] <= _params.range_max_ && !isnan(_ranges[ii]) && !isinf(_ranges[ii]) )
+        if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !isnan(_ranges[ii]) && !isinf(_ranges[ii]) )
         {
             //transform from polar to euclidean
             azimuth = _params.angle_min_+ _params.angle_step_*ii;
-            _points(0,ii_ok) = _ranges[ii] * cos(azimuth);
-            _points(1,ii_ok) = _ranges[ii] * sin(azimuth);
-            _points(2,ii_ok) = 1; 
+            _points(0,ii) = _ranges[ii] * cos(azimuth);
+            _points(1,ii) = _ranges[ii] * sin(azimuth);
+            _points(2,ii) = 1;
             
             //check jump
-            if ( (ii_ok != 0) && (fabs(_ranges[ii]-prev_range) > _jump_threshold) )
-                _jumps.push_back(ii_ok);
+            if ( (ii != 0) && (fabs(_ranges[ii]-prev_range) > _jump_threshold) )
+                _jumps.push_back(ii);
 
             //keep current range and update counter
             prev_range = _ranges[ii];
-            ii_ok ++;
+            ii ++;
         }
+        else
+            _ranges.erase(_ranges.begin()+ii);
     }
     
     //resize the output matrix to the number of correct points, while keeping values
-    _points.conservativeResize(3, ii_ok); 
+    _points.conservativeResize(3, _ranges.size());
     //_points.conservativeResize(Eigen::NoChange_t, ii_ok);  //does not compile ... why ?
 }
 

src/scan_basics.h

@@ -33,6 +33,7 @@ namespace laserscanutils
      * 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.
      * A list of jumps indices is also returned by reference.
      * \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 
@@ -41,7 +42,7 @@ namespace laserscanutils
      * \param jumps_id_ list of indexes of jumps in range measurements. Indexes correpond to _points vector.
      * 
     **/
-    void ranges2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps);
+    void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps);
         
 }
 #endif