Implemented Iterative Line Fitting. Tested successfully with real data

src/CMakeLists.txt

@@ -35,6 +35,7 @@ SET(HDRS
     line_detector.h
     line_finder.h
     line_finder_hough.h
+    line_finder_iterative.h
     line_finder_jump_fit.h
     line_segment.h
     point_set.h
@@ -57,6 +58,7 @@ SET(SRCS
     line_detector.cpp
     line_finder.cpp
     line_finder_hough.cpp
+    line_finder_iterative.cpp
     line_finder_jump_fit.cpp
     line_segment.cpp
     point_set.cpp

src/corner_finder_inscribed_angle.cpp

@@ -17,30 +17,34 @@ int CornerFinderInscribedAngle::findCorners( const Eigen::MatrixXs & _points, st
 {
     //params
     unsigned int MIN_SIDE_POINTS = 5; 
-    ScalarT LINE_FIT_ERROR = 0.03; //maximum allowed mean point-line error to consider a line is straight enough
+    ScalarT LINE_FIT_ERROR = 0.1; //maximum allowed mean point-line error to consider a line is straight enough
     ScalarT MIN_ANGLE = 45.*M_PI/180.; //minimum allowed absoulte angle between lines around the corner
     
     //local variables
     Eigen::VectorXs inscribed_angles; 
     Eigen::Vector3s line2o; //line from point i to _point(0)
     Eigen::Vector3s line2e; //line from point i to _point(last)
-    ScalarT aux, min_angle, angle, e1, e2; 
+    ScalarT aux, min_angle, angle; 
     unsigned int ii_min; 
     LineSegment line1, line2;
     CornerPoint new_corner;     
     
-    //TODO: first check: if segment is not enough populated -> return -1
-    //TODO: second check: if segment has high eigen value ratio -> return -1
+    //first check: if segment is not enough populated -> return -1
+    if (_points.cols() < MIN_SIDE_POINTS*2 ) return -1; 
+    
+    //second check: if segment has high eigen value ratio -> return -1
+    //TODO: getEvalRatio()
     
     //resize inscribed_angles
-    inscribed_angles.resize(_points.cols()-2,1); //column vector    
+//     std::cout << "findCorners(): " << __LINE__ << std::endl; 
+    inscribed_angles.resize(_points.cols()-2); //column vector    
     
-    //compute inscribed angles for each point (excepting extremes)
+    //compute inscribed angles for each point (exceptuating extremes)
     for (unsigned int ipt = 1; ipt < _points.cols()-1; ipt++) 
     {
         //find lines passing through point i and extremes of the set
         line2o = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,0));
-        line2e = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,_points.cols()));
+        line2e = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,_points.cols()-1));
         
         //normalize lines (http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html)
         aux = sqrt(line2o(0)*line2o(0)+line2o(1)*line2o(1));
@@ -53,30 +57,28 @@ int CornerFinderInscribedAngle::findCorners( const Eigen::MatrixXs & _points, st
     }
 
     //find the minimum inscribed angle, and its index in the vector
+//     std::cout << "findCorners(): " << __LINE__ << std::endl; 
     min_angle = inscribed_angles.minCoeff(&ii_min); 
     
     //check enough points in left and right
     if ( ( ii_min > MIN_SIDE_POINTS ) && ( ii_min < (inscribed_angles.rows() - MIN_SIDE_POINTS) ) )
     {
         //fit lines at left and right of the point
+//         std::cout << "findCorners(): " << __LINE__ << std::endl; 
         line_finder_.fitLine(_points.block(0,ii_min-MIN_SIDE_POINTS,3,MIN_SIDE_POINTS+1), line1);
         line_finder_.fitLine(_points.block(0,ii_min,3,MIN_SIDE_POINTS+1), line2);
             
-        //check line error 
-//         std::cout << "findCorners(): " << __LINE__ << std::endl; 
-        e1 = line1.fit_error_ / ( (ScalarT)MIN_SIDE_POINTS * 2. + 1.);
-        e2 = line2.fit_error_ / ( (ScalarT)MIN_SIDE_POINTS * 2. + 1.);
-//         std::cout << "findCorners(). e1: " << e1 << "; e2: " << e2 << std::endl; 
-            
-        if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
+        //check line error             
+        if ( ( line1.fit_error_ < LINE_FIT_ERROR ) && ( line2.fit_error_ < LINE_FIT_ERROR ) )
         {
             //check angles between lines 
-            angle = line1.angleToLine(line2); //angle is in [0,2pi]
+            angle = fabs(line1.angleToLine(line2)); //angle is in [0,2pi]->TODO:  Debug that !!
+//             std::cout << "findCorners(). " << __LINE__ << ".  angle: " << angle << std::endl;
+
             if ( ( angle > MIN_ANGLE ) && ( angle < 2*M_PI-MIN_ANGLE ) )
             {
                 //corner found !!
-//                 std::cout << "findCorners(): " << __LINE__ << std::endl; 
-//                 std::cout << "findCorners(). angle " << angle << std::endl;
+                std::cout << "findCorners(). " << __LINE__ << ".  angle: " << angle << std::endl;
                 new_corner.point_ << _points.block<3,1>(0,ii_min+1);//TODO crossing point between lines
                 new_corner.orientation_ = 0;//TODO
                 new_corner.aperture_ = angle; 

src/line_finder.cpp

@@ -22,12 +22,13 @@ void LineFinder::fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) c
     //solve for line
     _line.abc_ = AA.inverse().col(2);
 
-    // normalize the line
+    // normalize the line (according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html)
     _line.abc_ /= _line.abc_.head(2).norm();
 
     // compute fitting error
     _line.fit_error_ = (_points.transpose() * _line.abc_).squaredNorm() / _points.cols(); 
     //_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / (_line.abc_.head(2).norm()*_points.cols());
+    //_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / _points.cols();
 }
 
 void LineFinder::fitLineRansac(const Eigen::MatrixXs & _points, LineSegment & _line, ScalarT _error) const

src/line_finder_iterative.cpp

+#include "line_finder_iterative.h"
+
+namespace laserscanutils
+{
+
+LineFinderIterative::LineFinderIterative()
+{
+
+}
+
+LineFinderIterative::~LineFinderIterative()
+{
+
+}
+
+void LineFinderIterative::setIlfParams(const LineFinderIterativeParams & _params)
+{
+    ilf_params_.max_fit_error_ = _params.max_fit_error_;
+    ilf_params_.min_supports_ = _params.min_supports_;
+}
+
+unsigned int LineFinderIterative::findLines( const Eigen::MatrixXs & _points, std::list<laserscanutils::LineSegment> & _line_list)
+{
+    LineSegment line;//, line1, line2; 
+    ScalarT max_dist, di; 
+    unsigned int max_ii; 
+    
+    //fits a line between first and last points
+    line.abc_ = _points.block<3,1>(0,0).cross(_points.block<3,1>(0,_points.cols()-1));
+    
+    //normalize the line
+    line.abc_ /= line.abc_.head(2).norm();
+
+    //for each point, check distance to line, and get the farthest point to line
+    max_dist = 0; 
+    for (unsigned int ii=0; ii< _points.cols()-1; ii++)
+    {
+        di = fabs( line.abc_.dot(_points.block<3,1>(0,ii)) );
+        if ( di > max_dist )
+        {
+            max_dist = di; 
+            max_ii = ii; 
+        }
+    }
+    
+    //check if farthest point at distance lower than threshold
+    if ( max_dist < ilf_params_.max_fit_error_ ) //straight line case
+    {
+        //Fit line, fill start and end points and push_back it
+        fitLine(_points, line);
+        line.point_first_ << _points.block<3,1>(0,0);
+        line.point_last_ << _points.block<3,1>(0,_points.cols()-1); 
+        _line_list.push_back(line);
+    }
+    else //non straight line case -> carry on recursivity
+    {
+        //split _points into two subsets and call findLines again for each one, if enough points in the subset
+        if ( max_ii >= ilf_params_.min_supports_ ) 
+        {
+            this->findLines(_points.block(0,0,3,max_ii), _line_list);
+        }
+        if ( _points.cols()-max_ii >= ilf_params_.min_supports_ ) 
+        {
+            this->findLines(_points.block(0,max_ii,3,_points.cols()-max_ii), _line_list); 
+        }
+    }
+    
+    //return number of lines detected
+    return _line_list.size(); 
+}
+
+void LineFinderIterative::print() const
+{
+    
+}
+
+}//end namespace

src/line_finder_iterative.h

+#ifndef LINE_FINDER_ITERATIVE_H_
+#define LINE_FINDER_ITERATIVE_H_
+
+//laserscanutils
+#include "line_finder.h"
+
+namespace laserscanutils
+{
+/** \brief set of tunning parameters for the iterative line fit
+* 
+* set of tunning parameters for the iterative line fit
+* 
+*/
+struct LineFinderIterativeParams
+{
+    ScalarT max_fit_error_; //maximum allowed fit error to consider a straight line
+    unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line
+};
+
+    
+/** \brief Iterative line fit class
+* 
+* Iterative line fit class
+* Implements iterative line fit
+* 
+*/
+class LineFinderIterative : public LineFinder
+{
+    protected:
+        
+        //Tunning params for iterative line fit approach
+        LineFinderIterativeParams ilf_params_; 
+            
+    public: 
+        /** \brief Constructor
+         * 
+         * Constructor
+         * 
+         **/
+        LineFinderIterative(); 
+        
+        /** \brief Destructor
+         * 
+         * Destructor
+         * 
+         **/
+        ~LineFinderIterative(); 
+                
+        /** \brief Set Iterative Line Fit tunning params
+         * 
+         * Set Iterative Line Fit tunning params.
+         * 
+         **/
+        void setIlfParams(const LineFinderIterativeParams & _params);
+                
+        /** \brief Find lines using Iterative line Fit. Result as a list of Line's
+        *
+        * Find lines from a set of points.
+        * Returns Lines as a std::list<laserscanutils::StraightSegment>
+        * 
+        * \Requires: 
+        *    \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is assumed
+        * 
+        * \Provides: 
+        *    \param _line_list set of lines extracted from _points
+        *    \return Number of lines extracted.
+        * 
+        *
+        */
+        unsigned int findLines( const Eigen::MatrixXs & _points, 
+                                std::list<laserscanutils::LineSegment> & _line_list);
+                
+        /** \brief Print things
+         * 
+         * Print things about this class
+         * 
+         **/
+        void print() const;
+};
+}//namespace
+#endif
\ No newline at end of file

src/line_segment.cpp

@@ -60,16 +60,26 @@ void LineSegment::pointProjectionToLine(const Eigen::Vector3s & _in_pt, Eigen::V
 
 double LineSegment::angleToLine(const laserscanutils::LineSegment & _line) const
 {
+    //See http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
+    //Assuming normalized lines
+    //ScalarT cos_a = abc_(0)*_line.abc_(0)+abc_(1)*_line.abc_(1);
+    //ScalarT angle = acos(cos_a);
+    
+    //return value in [0,pi]  
+    //return angle;     
+    
     //compute each angle at 4-quadrant in [-pi,pi]
-    ScalarT angle_this = atan2(abc_(1)/abc_(2), abc_(0)/abc_(2)); 
-    ScalarT angle_line = atan2(_line.abc_(1)/_line.abc_(2), _line.abc_(0)/_line.abc_(2)); 
+    ScalarT angle_this = atan2(abc_(1), abc_(0)); 
+    ScalarT angle_line = atan2(_line.abc_(1), _line.abc_(0)); 
     
     //Fit angles to [0,2pi] domain
     if ( angle_this < 0 ) angle_this = 2*M_PI + angle_this;
     if ( angle_line < 0 ) angle_line = 2*M_PI + angle_line; 
     
-    //return angle from this to _line, in [0,2pi]
-    return (angle_line-angle_this); 
+    //return angle from this to _line, in [-2pi,2pi]
+    ScalarT angle_diff = angle_line-angle_this;
+    if ( angle_diff < 0 ) return ( 2*M_PI + angle_diff );
+    else return angle_diff; 
 }
 
 void LineSegment::merge(const LineSegment & _segment)

src/line_segment.h

@@ -22,14 +22,15 @@ class LineSegment
          * 
          * Homogeneous parameterization of the line to which the segment lies on: (a,b,c)^T -> ax+by+c=0. 
          * Vector (a/c,b/c)^T is the normal vector, pointing to a free surface
+         * Normalized after operations according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
          * 
          */
-        Eigen::Vector3s abc_; 
+        Eigen::Vector3s abc_;
         unsigned int idx_first_; //corresponding index to the scan ranges of the first point used
         unsigned int idx_last_; //corresponding index to the scan ranges of the last point used
         Eigen::Vector3s point_first_; //homogeneous coordinates of the starting 2D point
         Eigen::Vector3s point_last_; //homogeneous coordinates of the ending 2D point
-        ScalarT fit_error_; //sum of all distances from used points to line when fitting
+        ScalarT fit_error_; //mean error from points to line when fitting
         unsigned int np_; // number of scan points supporting this line (through fitting, Hough voting, ...)
         ScalarT range_; //range component in polar parameterization
         ScalarT theta_; //theta component in polar parameterization.

src/scilab/homogeneous_lines.sce

+
+//set points defining line 1 and 2
+p1o = [2;2;1];
+p1e = [-1;1;1];
+p2o = [-1;2;1];
+p2e = [2;-2;1];
+
+//compute lines
+l1 = cross(p1o,p1e);
+l2 = cross(p2o,p2e);
+
+//normalize (A) lines according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
+aux = sqrt( l1(1)*l1(1) + l1(2)*l1(2) );
+l1nA = l1./aux;
+aux = sqrt( l2(1)*l2(1) + l2(2)*l2(2) );
+l2nA = l2./aux;
+
+//normalize (B) lines according standard point normalization
+l1nB = l1./l1(3);
+l2nB = l2./l2(3);
+
+//plot lines
+fh=figure();
+fh2.background = color("white");
+
+ah = gca(); 
+ah.isoview = "on";
+ah.auto_margins = "off";
+ah.data_bounds = [-4,4,-4,4];
+
+plot([p1o(1) p1e(1)], [p1o(2) p1e(2)], '+b');
+xpoly([p1o(1) p1e(1)], [p1o(2) p1e(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",9);
+set(e,"thickness",3);
+
+plot([p2o(1) p2e(1)], [p2o(2) p2e(2)], '+r');
+xpoly([p2o(1) p2e(1)], [p2o(2) p2e(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",5);
+set(e,"thickness",3);
+
+//plot normal vectors
+p1c = (p1o+p1e)./2; //center point of l1
+xpoly([p1c(1) p1c(1)+l1nB(1)], [p1c(2) p1c(2)+l1nB(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",9);
+set(e,"thickness",3);
+
+p2c = (p2o+p2e)./2; //center point of l2
+xpoly([p2c(1) p2c(1)+l2nB(1)], [p2c(2) p2c(2)+l2nB(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",5);
+set(e,"thickness",3);
+
+
+