Simple version of corner detection. Poor results with real data

src/corner_finder_range_diff.cpp

@@ -17,9 +17,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
                                        std::list<laserscanutils::CornerPoint> & _corner_list)
 {
     //constants TODO: should be moved as a set algorithm params
-    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
+    unsigned int HALF_WINDOW_SIZE = 10; //minimum size of line supproting corner
+    ScalarT LINE_FIT_ERROR = 0.03; //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
     
     //variables
     ScalarT e1,e2, angle; 
@@ -44,16 +44,19 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
             
             //check line error 
 //             std::cout << "findCorners(): " << __LINE__ << std::endl; 
-            e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
-            e2 = line2.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
+            e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2. + 1.);
+            e2 = line2.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2. + 1.);
+            //std::cout << "findCorners(). e1: " << e1 << "; e2: " << e2 << std::endl; 
+            
             if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
             {
                 //check angles between lines 
                 angle = line1.angleToLine(line2); //angle is in [0,2pi]
-                if ( ( angle < M_PI-MIN_ANGLE ) || ( angle > M_PI+MIN_ANGLE ) )
+                if ( ( angle > MIN_ANGLE ) && ( angle < 2*M_PI-MIN_ANGLE ) )
                 {
                     //corner found !!
-                    std::cout << "findCorners(): " << __LINE__ << std::endl; 
+//                     std::cout << "findCorners(): " << __LINE__ << std::endl; 
+                    //std::cout << "findCorners(). angle " << angle << std::endl;
                     new_corner.point_ << _scan.points_all_.block<3,1>(0,ii);//TODO crossing point between lines
                     new_corner.orientation_ = 0;//TODO
                     new_corner.aperture_ = angle; 
@@ -62,6 +65,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
             }
         }
     }
+    
+    //return number of corners
+    return _corner_list.size(); 
 }
 
 void CornerFinderRangeDiff::print() const