fitLineRansac pseudocode added

README.txt

-A Toolbox with utilities for 2D laser scan processing, made at IRI (www.iri.upc.edu)
+A C++ Toolbox with utilities for 2D laser scan processing, made at IRI (www.iri.upc.edu)

src/line_finder.cpp

@@ -30,6 +30,45 @@ void LineFinder::fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) c
     //_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / (_line.abc_.head(2).norm()*_points.cols());
 }
 
+void LineFinder::fitLineRansac(const Eigen::MatrixXs & _points, LineSegment & _line, ScalarT _error) const
+{
+    std::vector<bool> used_points(_points.cols(),false); //mask, true means used
+    std::vector<unsigned int> available_points(_points.cols());
+    unsigned int support_counter, best_support_counter; 
+    LineSegment line; 
+        
+    //init best_support_counter
+    best_support_counter = 0; 
+    
+    //  while (attempts < max_attempts)
+    //  {
+            //init available_points vector with all indexes
+            for ( unsigned int ii=0; ii<available_points.size(); ii++ )
+            {
+                available_points[ii] = ii; 
+            }
+    //      pick randomly two indexes from available_points
+    //      Set the selecte points to MatrixXs P
+    //      fitLine(P,line)
+    //      support_counter = 0; 
+    //      while (available_points.size() > 0 )
+    //      {
+    //          choose a point q randomly from the index vector available_points
+    //          remove that index from available_points
+    //          Compute dist between q and line
+    //          if (dist < max_error) support_counter ++;
+    //      }
+    //      
+    //      if ( support_counter > best_support_counter )
+    //      {
+    //          _line = line;
+    //          best_support_counter = support_counter; 
+    //      }
+    //      attempts ++;
+    //  }
+    
+}
+
 void LineFinder::pointProjectionToLine(const Eigen::Vector3s & _line, const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const
 {
     ScalarT a,b,c,qx,qy,dd;

src/line_finder.h

@@ -47,6 +47,22 @@ class LineFinder
         **/
         void fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) const;    
         
+        /** \brief Find the best fittig line given a set of points, applying RANSAC
+        * 
+        * Find the best fittig line given a set of points, applying RANSAC
+        * 
+        * \Requires:
+        * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
+        * \param _error: maximum fitting error to accept a point in a line
+        * 
+        * \Provides:
+        * \param _line: a laserscanutils::Line object of the best fitting line in the Least Squares sense
+        * 
+        * RANSAC is applied as outlier rejection method. 
+        * 
+        **/
+        void fitLineRansac(const Eigen::MatrixXs & _points, LineSegment & _line, ScalarT _error = 0.1) const;    
+
         /** \brief Computes projection of point to line
          * 
          * Computes the projection of _in_pt to _line. Result at _out_pt