Just a working copy of corner detection. Doesn't compilr, Not tested yet

src/corner_finder_range_diff.cpp

@@ -17,57 +17,48 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
                                        std::list<laserscanutils::CornerPoint> & _corner_list)
 {
     //constants TODO: should be moved as a set algorithm params
-    ScalarT ZERO_RANGE_DIFF = 0.02; 
-    unsigned int CORNER_WINDOW_SIZE = 5; //minimum size of line supproting corner
+    unsigned int HALF_WINDOW_SIZE = 5; //minimum size of line supproting corner
+    ScalarT LINE_FIT_ERROR = 0.05; //maximum allowed mean point-line error to consider a line is straight enough
+    ScalarT MIN_ANGLE = 30*M_PI/180.; //minimum allowed absoulte angle between lines around the corner
     
     //variables
-    ScalarT range_diff; 
+    ScalarT e1,e2, angle; 
     LineSegment line1, line2;
+    CornerPoint new_corner; 
     
     //check if _scan is already raw processed
     if ( !_scan.isRawProcessed() ) return -1; 
     
     //run over all scan data
-    for (unsigned int ii = CORNER_WINDOW_SIZE; ii<_scan.ranges_.size()-CORNER_WINDOW_SIZE; ii++)
+    for (unsigned int ii = HALF_WINDOW_SIZE; ii<_scan.ranges_.size()-HALF_WINDOW_SIZE; ii++)
     {
-        //check if ranges ii and ii-1 are both correct and there is no jump between them
-        if (  ( _scan.ranges_[ii-1] > 0 ) && ( _scan.ranges_[ii] > 0 ) && ( _scan.jumps_mask_[ii] == false ) )
+        //check correctness and jumps
+        if ( ( _scan.checkScanCorrectness(ii, HALF_WINDOW_SIZE) == true ) 
+             &&
+             ( _scan.checkScanJumps(ii, HALF_WINDOW_SIZE) == false ) )
         {
-            //compute range difference
-            range_diff = _scan.ranges_[ii] - _scan.ranges_[ii-1];
+            //fit lines at left and right of the point
+            line_finder_.fitLine(_scan.points_.block(0,ii-HALF_WINDOW_SIZE,3,HALF_WINDOW_SIZE+1), line1);
+            line_finder_.fitLine(_scan.points_.block(0,ii,3,HALF_WINDOW_SIZE+1), line2);    
             
-            //corners have range diff close to zero TODO: this condition is to be discussed
-            if(range_diff < ZERO_RANGE_DIFF) 
+            //check line error 
+            e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
+            e2 = line2.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
+            if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
             {
-                //fit lines at eft and right of the point
-                line_finder_.fitLine(_scan.points_.block(0,ii-CORNER_WINDOW_SIZE,3,CORNER_WINDOW_SIZE*2), line1);
-                line_finder_.fitLine(_scan.points_.block(0,ii-CORNER_WINDOW_SIZE,3,CORNER_WINDOW_SIZE*2), line2);    
-                
-                //check angle between lines
-                //check fit errors
-                
-//                 if ( fit errors < FIT_ERROR) && (angle in (0,135)U(225,360) ) //CORNER_MIN_ANGLE
-//                 {
-//                     //corner found
-//                     //set corner
-//                     // _corner_list.push_back(); 
-//                 }                
+                //check angles between lines 
+                angle = line1.angleToLine(line2); //angle is in [0,2pi]
+                if ( ( angle < M_PI-MIN_ANGLE ) || ( angle > M_PI+MIN_ANGLE ) )
+                {
+//                     //corner found !!
+//                     new_corner.point_ << _scan.points_all_.block<1,3>(0,);
+//                     //new_corner.orientation_ = ;
+//                     new_corner.aperture_ = angle; 
+//                     _corner_list.push_back(new_corner); 
+                }
             }
         }
     }
-    
-/*
-    range_diff_i = range_i - range_(i-1)
-    
-    if(range_diff_i < ZERO_RANGE_DIFF) 
-    {
-        v1,b1 = check {r_{i-CORNER_WINDOW_SIZE}, r_i} is line (with FIT_ERROR)
-        v2,b2 = check {r_i, r_{i-CORNER_WINDOW_SIZE}} is line (with FIT_ERROR)
-        
-        angle = getAngle(b1,b2);
-        
-    }
-*/
 }
 
 void CornerFinderRangeDiff::print() const

src/corner_finder_range_diff.h

@@ -61,10 +61,10 @@ class CornerFinderRangeDiff : public CornerFinder
         * Returns corners as a std::list<CornerPoint>
         * 
         * \Requires: 
-        *    \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
+        *    \param _scan: A LaserScan object, already raw processed. If not raw processed, this method returns -1
         * 
         * \Provides: 
-        *    \param _corner_list set of corners extracted from _points
+        *    \param _corner_list set of corners extracted from _scan
         *    \return Number of corners extracted.
         *
         */

src/laser_scan.cpp

@@ -100,6 +100,56 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
     is_raw_processed_ = true; 
 }
 
+bool LaserScan::checkScanCorrectness(unsigned int _idx, unsigned int _idx_range) const
+{
+    bool correct = true; 
+    
+    //first of all check if scan has been raw processed
+    if ( ! is_raw_processed_ ) return false;
+
+    //set loop bounds
+    unsigned int ii_init = std::max( 0 , (_idx-_idx_range) );
+    unsigned int ii_end = std::min( ranges_.size()-1 , (_idx+_idx_range) );
+    
+    //proceed
+    for (unsigned int ii=ii_init; ii<=ii_end; ii++ )
+    {
+        if (ranges_[ii] < 0)
+        {
+            correct = false; 
+            break; 
+        }
+    }
+    
+    //return
+    return correct; 
+}
+
+bool LaserScan::checkScanJumps(unsigned int _idx, unsigned int _idx_range) const
+{
+    bool jump = false; 
+    
+    //first of all check if scan has been raw processed
+    if ( ! is_raw_processed_ ) return true;
+    
+    //set loop bounds
+    unsigned int ii_init = std::max( 0 , (_idx-_idx_range) );
+    unsigned int ii_end = std::min( ranges_.size()-1 , (_idx+_idx_range) );
+    
+    //proceed
+    for (unsigned int ii=ii_init; ii<=ii_end; ii++ )
+    {
+        if ( jumps_mask_[ii] )
+        {
+            jump = true; 
+            break; 
+        }
+    }
+    
+    //return
+    return jump; 
+}
+
 void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_list)
 {
     std::list<unsigned int>::iterator jumps_it, next_jumps_it, jumps_last; 

src/laser_scan.h

@@ -9,6 +9,7 @@
 #include <vector>
 #include <list>
 #include <iostream>
+#include <algorithm> //std::min, max
 
 namespace laserscanutils
 {
@@ -51,16 +52,27 @@ class LaserScan
         //Ordered raw range data
         std::vector<float> ranges_raw_; 
         
-        //Ordered and marked range data. Bad values (NaN's, Inf's, out of range) are indicated with -1
+        //Ordered and marked range data. Bad values (NaN's, Inf's, out of range) are indicated with -1. 
+        //Same size as ranges_raw_
         std::vector<float> ranges_; 
+
+        //Ordered, Marked 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. 
+        //NaN's, inf's and out of range are marked as points at infty->last component set to 0.
+        //Same size as ranges_raw_
+        //Eigen::MatrixXs points_all_;
         
-        //ordered 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. NaN and inf's are filtered out. 
+        //Ordered, Correct 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. 
+        //NaN's, inf's and out of range are filtered out, implying:
+        //          - not necessarily regular angular increments between consecutive points
+        //          - size equal or smaller than ranges_raw_
         Eigen::MatrixXs points_;
         
-        //For each element in ranges_, r_i, indicates if there is a jump (true) between that element and the previouos.
+        //For each element in ranges_, r_i, indicates if there is a jump (true) between that element and the previouos. 
+        //Same size as ranges_raw_
         std::vector<bool> jumps_mask_; 
         
         //list of indexes over points_ where a scan jump is found. Indexes indicate the second point of the jump (start of a scan segment)
+        //size smaller than ranges_raw_
         std::list<unsigned int> jumps_indexes_; 
 
             
@@ -108,8 +120,26 @@ class LaserScan
         **/
         void ranges2xy(Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity());   
         //TODO void processRaw()-> from ranges_raw_ fills: ranges_, points_, jumps_indexes and jumps_mask
+        
+        /** \brief Check for scan correctness 
+         * 
+         * Check for scan correctness
+         * Returns True if all scan points in [ _idx-_idx_range , _idx+_idx_range ] are ok
+         * Otherwise returns false. 
+         * 
+         **/
+        bool checkScanCorrectness(unsigned int _idx, unsigned int _idx_range) const; 
 
-        /** \brief Find segments based on jumps of consecutive scan points
+        /** \brief Check for scan jumps
+         * 
+         * Check for scan jumps
+         * Returns True if there is at least one jump in some point in [ _idx-_idx_range , _idx+_idx_range ]
+         * Otherwise returns false. 
+         * 
+         **/
+        bool checkScanJumps(unsigned int _idx, unsigned int _idx_range) const; 
+        
+        /** \brief Find segments based on jumps of consecutive scan points TODO. MOve away this method to anoter class SegmentFinder
         *
         * Find segments based on jumps of consecutive scan points
         * Do not compute segment parameters, just fill ScanSegment.points_

src/line_segment.cpp

@@ -58,6 +58,19 @@ void LineSegment::pointProjectionToLine(const Eigen::Vector3s & _in_pt, Eigen::V
     _out_pt(0) = -( c + b*_out_pt(1) ) / a; 
 }
 
+double angleToLine(const laserscanutils::LineSegment & _line) const
+{
+    //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)); 
+    
+    //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); 
+}
 
 void LineSegment::merge(const LineSegment & _segment)
 {

src/line_segment.h

@@ -50,6 +50,14 @@ class LineSegment
         **/
         void pointProjectionToLine(const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const;
         
+        /** \brief Angle from this to line, in [0,2pi]
+         * 
+         * Returns the angle from this vector abc_ to the argument line's vector abc_
+         * Return value in [0,2pi]
+         * 
+         **/
+        double angleToLine(const laserscanutils::LineSegment & _line) const; 
+        
         //merges this LineSegment with the argument. 
         void merge(const LineSegment & _segment);