line detector and corner detector improvements

src/corner_detector.cpp

@@ -80,7 +80,7 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
         
         line_it1 ++;
     }
-//     std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
+    // std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
 
     // Erase duplicated corners
     corner_it1 = _corner_list.begin();
@@ -102,7 +102,7 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
         }
         corner_it1 ++;
     }
-//     std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
+    // std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
 
     return _corner_list.size();
 }

src/examples/test_extract_corners.cpp

@@ -95,7 +95,7 @@ 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_ = 0.5;
+    alg_params.line_params_.window_length_ = 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; 

src/line_detector.cpp

@@ -5,7 +5,7 @@ void laserscanutils::ExtractLineParams::print() const
 {
     std::cout << "Extract Line Algorithm Parameters : " << std::endl 
                 << "   jump_dist_ut_: " << jump_dist_ut_ << std::endl
-                << "   window_sz: " << window_sz_ << std::endl
+                << "   window_length: " << window_length_ << std::endl
                 << "   k_sigmas_ut_: " << k_sigmas_ut_ << std::endl
                 << "   concatenate_ii_ut_: " << concatenate_ii_ut_ << std::endl
                 << "   concatenate_angle_ut_: " << concatenate_angle_ut_ << std::endl;
@@ -20,8 +20,13 @@ void laserscanutils::fitLine(const Eigen::MatrixXs & _points, Line & _line)
     //solve for line
     _line.vector_ = AA.inverse().col(2);
 
+    // normalize the line
+    //std::cout << "line squaredNorm (before) " << _line.vector_.head(2).squaredNorm() << std::endl;
+    _line.vector_ /= _line.vector_.head(2).norm();
+    //std::cout << "line squaredNorm " << _line.vector_.head(2).squaredNorm() << std::endl;
+
     // compute fitting error
-    _line.error_ = (_points.transpose() * _line.vector_).array().abs().sum() / (_line.vector_.head(2).norm()*_points.cols());
+    _line.error_ = (_points.transpose() * _line.vector_).squaredNorm() / _points.cols(); //_line.error_ = (_points.transpose() * _line.vector_).array().abs().sum() / (_line.vector_.head(2).norm()*_points.cols());
 }
 
 unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _params, 
@@ -33,9 +38,9 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
     Eigen::MatrixXs points;
     ScalarT cos_theta, theta;
     Line line;
-    std::list<Line>::iterator line_it1, line_it2;
+    std::list<Line>::iterator line_it1, line_it2, last_it;
     std::list<unsigned int> jumps;
-    std::vector<float> ranges = _ranges;
+    std::vector<float> ranges(_ranges);
     std::list<unsigned int>::iterator jj_jump;
     bool update_iterators;
     unsigned int ii, window_points;
@@ -48,7 +53,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
     // STEP 2: find line segments running over the scan
     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])));
+    window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_params.angle_step_ * ranges[ii])));
     jj_jump = jumps.begin();
 
 
@@ -66,7 +71,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
             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_ )
+            if ( line.error_ < _params.range_std_dev_*_params.range_std_dev_*_alg_params.k_sigmas_ut_*_alg_params.k_sigmas_ut_ )
             {
                 line.first_ = ii;
                 line.last_ = ii + window_points;
@@ -79,25 +84,27 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
             //increment window iterators
             ii++;
         }
-        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;
+        window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_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
     line_it1 = _line_list.begin();
     line_it2 = line_it1;
-    line_it2 ++; 
-    while (line_it2 != _line_list.end())
+    line_it2++;
+    last_it = _line_list.end();
+    last_it--;
+    while (line_it1 != last_it && line_it1 != _line_list.end())
     {
-        // check index threshold
+        // check number of beams between lines
         if ( ( (int)line_it2->first_ - (int)line_it1->last_ ) <=  (int)_alg_params.concatenate_ii_ut_ )
         {
             //compute angle between lines 1 and 2
@@ -119,40 +126,85 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
                     new_line.point_first_ = line_it1->point_first_;
                     new_line.point_last_ = line_it2->point_last_;
                     new_line.np_ = new_line.last_ - new_line.first_ + 1;
-                    
-                    //assign new values to l1. Removes l2
+
+                    //assign new values to line 1. Remove line 2
                     *line_it1 = new_line;
                     line_it2 = _line_list.erase(line_it2);//iterator line_it2 points to the next line of the list
+                    //Update iterator to last element
+                    last_it = _line_list.end();
+                    last_it--;
+                    //std::cout << "lines concatenated " << _line_list.size() << std::endl;
                 }
                 else
-                {
-                    line_it1++; 
                     line_it2++;
-                }
             }
             else
+                line_it2++;
+
+            if (line_it2 == _line_list.end()) //no more lines to be checked with line 1, next line 1 (and 2 is the next one)
             {
-                line_it1++; 
+                //std::cout << "no more lines to be checked with line 1, next line 1 (and 2 is the next one) " << _line_list.size() << std::endl;
+                line_it1++;
+                line_it2 = line_it1;
                 line_it2++;
             }
         }
-        else
+        else // lines not close enought, next line 1 (and 2 is the next one)
         {
-            line_it1++; 
-            line_it2++;            
+            //std::cout << "lines not close enought, next line 1 (and 2 is the next one) " << _line_list.size() << std::endl;
+            line_it1++;
+            line_it2 = line_it1;
+            line_it2++;
         }
     }
-    std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
+    //std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
+
+    //STEP 4: removing outliers
+    for (line_it1 = _line_list.begin(); line_it1 != _line_list.end(); line_it1++)
+    {
+        // Remove front and back points with high error
+        while (line_it1->last_ - line_it1->first_ > 1 && pow(points.col(line_it1->first_).transpose() * line_it1->vector_,2) > 2 * line_it1->error_)
+            line_it1->first_++;
+
+        while (line_it1->last_ - line_it1->first_ > 1 && pow(points.col(line_it1->last_).transpose() * line_it1->vector_,2) > 2 * line_it1->error_)
+            line_it1->last_--;
+
+        if (line_it1->last_ - line_it1->first_ +1 < line_it1->np_) // Only if any point removed
+        {
+            if (line_it1->last_ - line_it1->first_ < _alg_params.min_window_points_)
+            {
+                //std::cout << "line too short after removing outliers!" << std::endl;
+                line_it1 = _line_list.erase(line_it1);//iterator line_it points to the next line of the list
+                line_it1--;
+            }
+            else
+            {
+                //std::cout << "updating line after removing " << line_it1->np_ - (line_it1->last_ - line_it1->first_ +1) << " outliers!" << std::endl;
+                //update line
+                Line new_line;
+                new_line.first_ = line_it1->first_;
+                new_line.last_ = line_it1->last_;
+                new_line.point_first_ = points.col(line_it1->first_);
+                new_line.point_last_ = points.col(line_it1->last_);
+                new_line.np_ = new_line.last_ - new_line.first_ + 1;
+                fitLine(points.block(0,new_line.first_, 3, new_line.np_), new_line);
+                *line_it1 = new_line;
+            }
+        }
+    }
+
 
     return _line_list.size();
 }
 
 laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2)
 {
-    ScalarT theta;
-    
+    // lines should be normalized
+    assert(abs(line1.vector_.head(2).squaredNorm() - 1) < 1e-9 && "line 1 is not normalized!");
+    assert(abs(line2.vector_.head(2).squaredNorm() - 1) < 1e-9 && "line 2 is not normalized!");
+
     //compute angle, result in [0,PI]
-    theta = acos( (line1.vector_.head(2)).dot(line2.vector_.head(2)) / ( line1.vector_.head(2).norm()*line2.vector_.head(2).norm() ) );
+    ScalarT theta = acos( (line1.vector_.head(2)).dot(line2.vector_.head(2)) );
 
     //returns the angle lower PI/2 of the crossing lines
     if (theta > M_PI/2) theta = M_PI - theta;

src/line_detector.h

@@ -18,7 +18,7 @@ namespace laserscanutils
     {
         //members
         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
+        ScalarT window_length_; // length (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

src/scan_basics.cpp

@@ -14,7 +14,7 @@ void laserscanutils::ScanParams::print() const
 
 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 azimuth = _params.angle_min_;
     ScalarT prev_range;
     unsigned int ii = 0;
     
@@ -22,7 +22,7 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
     _points.resize(3,_ranges.size());
     
     //jumps_id_.clear(); //std::queue has no member clear(). why it is a queue ?
-    _jumps.clear(); 
+    _jumps.clear();
     
     //for each range, check correctness of value and translate from polar to xy coordinates 
     while (ii <_ranges.size())
@@ -30,7 +30,6 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
         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) = _ranges[ii] * cos(azimuth);
             _points(1,ii) = _ranges[ii] * sin(azimuth);
             _points(2,ii) = 1;
@@ -41,10 +40,12 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
 
             //keep current range and update counter
             prev_range = _ranges[ii];
-            ii ++;
+            ii++;
         }
         else
             _ranges.erase(_ranges.begin()+ii);
+
+        azimuth += _params.angle_step_;
     }
     
     //resize the output matrix to the number of correct points, while keeping values