Added new members to LaserScan class: ranges_raw_, jumps_mask_ and is_raw_processed_

src/corner_finder_range_diff.cpp

@@ -13,20 +13,20 @@ CornerFinderRangeDiff::~CornerFinderRangeDiff()
 
 }
 
-// unsigned int CornerFinderRangeDiff::findCorners( const Eigen::MatrixXs & _points, 
-//                                                  std::list<laserscanutils::CornerPoint> & _corner_list)
-unsigned int CornerFinderRangeDiff::findCorners( const laserscanutils::LaserScan & _scan, 
+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]
-        }
-    }
+//     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)
@@ -45,6 +45,7 @@ unsigned int CornerFinderRangeDiff::findCorners( const laserscanutils::LaserScan
             // _corner_list.push_back(); 
         }
     }
+*/
 }
 
 void CornerFinderRangeDiff::print() const

src/laser_scan.cpp

@@ -3,7 +3,8 @@
 namespace laserscanutils
 {
     
-LaserScan::LaserScan()
+LaserScan::LaserScan() :
+    is_raw_processed_(false)
 {
 
 }
@@ -18,6 +19,11 @@ void LaserScan::setLaserScanParams(const LaserScanParams & _params)
     params_ = _params; 
 }
 
+bool LaserScan::isRawProcessed() const
+{
+    return is_raw_processed_; 
+}
+
 void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
 {
     ScalarT azimuth = params_.angle_min_;
@@ -27,21 +33,25 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
     ScalarT kr = 10; //TODO: as a parameters somewhere. 
     Eigen::Vector4s point_laser, point_ref;
 
-    //resize to all points case
-    points_.resize(3,ranges_.size());
+    //resize onvolved vectors for all points case
+    ranges_.resize(ranges_raw_.size());
+    points_.resize(3,ranges_raw_.size());
+    //jumps_indexes_.resize(ranges_raw_.size());
+    jumps_mask_.resize(ranges_raw_.size());
 
     //clear jumps_ vector
-    jumps_.clear();
+    jumps_indexes_.clear();
 
     //for each range, check correctness of value and translate from polar to xy coordinates
-    for ( ii = 0; ii < ranges_.size(); ii++ )
+    for ( ii = 0; ii < ranges_raw_.size(); ii++ )
     {
         //check raw range integrity
-        if ( ( ranges_[ii] > params_.range_min_ ) && 
-             ( ranges_[ii] < params_.range_max_ ) && 
-             ( !std::isnan(ranges_[ii]) ) && 
-             ( !std::isinf(ranges_[ii]) ) )
+        if ( ( ranges_raw_[ii] > params_.range_min_ ) && ( ranges_raw_[ii] < params_.range_max_ ) && 
+             ( !std::isnan(ranges_raw_[ii]) ) && ( !std::isinf(ranges_raw_[ii]) ) )
         {
+            //set as valid range
+            ranges_[ii] = ranges_raw_[ii];
+            
             //transform the laser hit from polar to 3D euclidean homogeneous
             point_laser << ranges_[ii]*cos(azimuth), ranges_[ii]*sin(azimuth),0,1;
             
@@ -51,36 +61,43 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
             //set to points_ as a 2D homogeneous
             points_.block<3,1>(0,ii_ok) << point_ref(0),point_ref(1),1;
                         
-//             //transform from polar to euclidean
-//             points_(0,ii_ok) = ranges_[ii] * cos(azimuth);
-//             points_(1,ii_ok) = ranges_[ii] * sin(azimuth);
-//             points_(2,ii_ok) = 1;
-            
-            //check jump. Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times
+            //check jump. 
+            //Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times
             if ( fabs(ranges_[ii]-prev_range) > fabs(kr*ranges_[ii]*params_.angle_step_)) //jump condition (kr*r*sin(a) ~ kr*r*a)
-            //if ( fabs(ranges_[ii]-prev_range) > 0.5) //jump condition >0.5m
             {
-                jumps_.push_back(ii_ok);
+                jumps_indexes_.push_back(ii_ok); //indexes over points_
+                jumps_mask_[ii] = true; //masks over ranges_
+            }
+            else
+            {
+                jumps_mask_[ii] = false; 
             }
            
             //increment ok counter
             ii_ok ++; 
             
             //keep current range as previous for the next iteration
-            prev_range = ranges_[ii];            
+            prev_range = ranges_raw_[ii];            
         }
-        else
+        else //invalid raw value
+        {
+            ranges_[ii] = -1.; 
+            jumps_mask_[ii] = 1; 
             prev_range = 0; 
+        }
         
         //increment azimuth with angle step
         azimuth += params_.angle_step_;
     }
     
-    //push back the last index to jumps_, to properly close the jumps_ vector. This will be used by findSegments()
-    jumps_.push_back(ii_ok);
+    //push back the last index to jumps_indexes_, to properly close the list. This will be used by findSegments()
+    jumps_indexes_.push_back(ii_ok);
 
     //resize the output matrix to the number of correct points, while keeping values
     points_.conservativeResize(3, ii_ok);
+    
+    //raise the flag
+    is_raw_processed_ = true; 
 }
 
 void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_list)
@@ -89,10 +106,10 @@ void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_l
     unsigned int num_points; 
     
     //set jumps_last to the last valid element of jumps
-    jumps_last = std::prev(jumps_.end()); 
+    jumps_last = std::prev(jumps_indexes_.end()); 
     
     //run over all jumps (except the last, which indicates the closing index) 
-    for (jumps_it = jumps_.begin(); jumps_it != jumps_last; jumps_it ++)
+    for (jumps_it = jumps_indexes_.begin(); jumps_it != jumps_last; jumps_it ++)
     {
         //new segment in the list
         _segment_list.push_back(ScanSegment()); 

src/laser_scan.h

@@ -41,23 +41,28 @@ struct LaserScanParams
 class LaserScan
 {
     protected:
+        //laser scan parameters
         LaserScanParams params_;
         
+        //flag indicating wheter this scan has been raw-processed or not
+        bool is_raw_processed_; 
+        
     public: 
         //Ordered raw range data
         std::vector<float> ranges_raw_; 
         
-        //Ordered range data, without NaN's, Inf's
+        //Ordered range data, without NaN's, Inf's. Bad values ar indicated with -1
         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 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_; 
+        
+        //list of indexes over points_ 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_; 
+
             
     public: 
         /** \brief Constructor
@@ -81,7 +86,14 @@ class LaserScan
          **/
         void setLaserScanParams(const laserscanutils::LaserScanParams & _params);
         
-        /** \brief Transforms from ranges (polar) to euclidean (xy)
+        /** \brief Returns the value of is_raw_processed_ flag
+         * 
+         * Returns the value of is_raw_processed_ flag
+         * 
+         **/
+        bool isRawProcessed() const; 
+        
+        /** \brief Sets points_ by transforming from ranges (polar) to euclidean (xy), and sets jumps_x_ vectors
         *
         * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data.
         * Invalid values (inf's and nan's) and out of range measurements are not transformed.