Working commit, adding extra features to LaserScan class to be used in corner detection

src/corner_finder_range_diff.cpp

@@ -13,10 +13,38 @@ CornerFinderRangeDiff::~CornerFinderRangeDiff()
 
 }
 
-unsigned int CornerFinderRangeDiff::findCorners( const Eigen::MatrixXs & _points, 
+// unsigned int CornerFinderRangeDiff::findCorners( const Eigen::MatrixXs & _points, 
+//                                                  std::list<laserscanutils::CornerPoint> & _corner_list)
+unsigned int CornerFinderRangeDiff::findCorners( const laserscanutils::LaserScan & _scan, 
                                                  std::list<laserscanutils::CornerPoint> & _corner_list)
 {
+    ScalarT range_diff; 
     
+    for (unsigned int ii = 1; ii<_scan.ranges.size(); ii++)
+    {
+        if (no jump)
+        {
+            range_diff = _scan.ranges[ii] - _scan.ranges[ii-1]
+        }
+    }
+    
+/*
+    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);
+        
+        if (b1&&b2) && (angle in (0,135)U(225,360) ) //CORNER_MIN_ANGLE
+        {
+            //corner found
+            //set corner
+            // _corner_list.push_back(); 
+        }
+    }
 }
 
 void CornerFinderRangeDiff::print() const

src/laser_scan.h

@@ -45,13 +45,19 @@ class LaserScan
         
     public: 
         //Ordered raw range data
+        std::vector<float> ranges_raw_; 
+        
+        //Ordered range data, without NaN's, Inf's
         std::vector<float> ranges_; 
         
         //ordered 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. NaN and inf's are filtered out. 
         Eigen::MatrixXs points_;
         
-        //list of indexes where a scan jump is found. Indexes incicate start of a scan segment
-        std::list<unsigned int> jumps_; 
+        //list of indexes where a scan jump is found. Indexes indicate the second point of the jump (start of a scan segment)
+        std::list<unsigned int> jumps_indexes_; 
+        
+        //For each element in ranges_, r_i, indicates if there is a jump (true) between that element and the previouos.
+        std::vector<bool> jumps_mask_; 
             
     public: 
         /** \brief Constructor
@@ -88,7 +94,8 @@ class LaserScan
         * Set also the jumps_ vector, which after the call holds the indexes to points_ where a scan segment starts
         *
         **/
-        void ranges2xy(Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity());        
+        void ranges2xy(Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity());   
+        //TODO void processRaw()-> from ranges_raw_ fills: ranges_, points_, jumps_indexes and jumps_mask
 
         /** \brief Find segments based on jumps of consecutive scan points
         *