added files for clustering functions and function range2xyh

src/clustering.cpp

+#include "clustering.h"
+
+
+
+//****************************************************
+//**********     xyExtractSegments
+//****************************************************
+std::vector< std::pair<int,int> >  laserscanutils::xyCalculateSegments(const Eigen::MatrixXs _points,
+                                                                       float  dist_threshold,
+                                                                       int num_min_points)
+{
+
+  std::pair<float,float> pointA;
+  std::pair<float,float> pointB;
+
+  std::vector< std::pair<int,int> > segments_index;
+  std::pair<int,int> seg;      // saves the first and last point for each segment
+
+  float points_distance = 0.0;
+  int count_num_segment = 0;
+
+  // Initial Segment
+  count_num_segment = 1;
+  seg.first = 0;
+  float sq_threshold = pow(dist_threshold,2.0);
+
+  for (unsigned int pos = 1; pos < xy_scan.size(); ++pos)
+  {
+    pointA = xy_scan[pos];
+    pointB = xy_scan[pos-1];
+    points_distance = ( pow( (pointA.first)-(pointB.first),2.0 ) + pow( (pointA.second)-(pointB.second),2.0) );
+
+
+    if ( points_distance <=  sq_threshold)  // same segment points
+    {
+      // Same segment -> TODO counter?
+    }
+    else if ( pos != xy_scan.size() )      // new segment
+    {
+      //std::cout << "P " << pos << " - A - (" << pointA.x << "-" << pointA.y << ") - B - (" << pointB.x << "-" << pointB.y << ") -> distance: " << points_distance << " More Than th" << std:endl;
+
+      seg.second = pos-1;                     // end of the previous segment
+      segments_index.push_back(seg);
+
+      count_num_segment++;    // count next segment
+
+      seg.first = pos;
+    }
+  }
+  // last point close last segment
+  seg.second = xy_scan.size()-1;
+  segments_index.push_back(seg);
+
+
+
+
+//   //SHOWING SEGMENTS
+//  std::vector< std::pair<int,int> >::iterator it_a; // declare an iterator to a vector of pairs
+//  for( it_a=segments_index.begin() ; it_a!=segments_index.end(); ++it_a)
+//  {
+//    std::cout << "Segment Number: " << it_a - segments_index.begin() << " - from " << (*it_a).first << " to " << (*it_a).second << std::endl;
+//  }
+
+
+  // REMOVING Segments with less than XX num points  minPoints => ITERATOR!!!
+  std::vector< std::pair<int,int> >::iterator it; // declare an iterator to a vector of strings
+  for( it=segments_index.begin() ; it!=segments_index.end();)
+  {
+    std::pair<int,int> tempPair = *it;
+    if ( (tempPair.second - tempPair.first) <= num_min_points)
+    {
+      segments_index.erase(it);
+//      std::cout << "[ExtractSegments] - Deleting small segment - " << it - segments_index.begin() << std::endl;
+    }
+    else
+      ++it;
+  }
+
+
+
+//  // SHOWING FINAL SEGMENTS
+//  std::cout << "FINAL Total Segments (size): " << segments_index.size() << std::endl;
+//  std::vector< std::pair<int,int> >::iterator it_c; // declare an iterator to a vector of pairs
+//  for( it_c=segments_index.begin() ; it_c!=segments_index.end(); ++it_c)
+//  {
+//    std::cout << "Segment Number: " << it_c - segments_index.begin() << " - from " << (*it_c).first << " to " << (*it_c).second << std::endl;
+//  }
+
+
+  return segments_index;
+}

src/clustering.h

+#ifndef CLUSTERING_H
+#define CLUSTERING_H
+
+//laserscanutils
+#include "types_laser_scan_utils.h"
+#include "entities.h"
+
+namespace laserscanutils
+{
+    /** \brief Cluster the 2D points according to closeness in XY euclidean distance
+     *
+     * Receives a matrix of 2D valid homogeneous points and cluster them
+     *
+     **/
+
+    std::vector< std::pair<int,int> >  laserscanutils::xyCalculateSegments(const Eigen::MatrixXs _points,
+                                                                           float  dist_threshold,
+                                                                           int num_min_points);
+
+
+
+}
+#endif// CLUSTERING_H

src/scan_basics.cpp

@@ -18,10 +18,9 @@ void laserscanutils::polar2xy(const ScanParams & _params, Eigen::VectorXs & _ran
 }
 
 
+// Just a test!
 Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, std::vector<float> & _ranges)
 {
-  //assert( (_ranges.size() == _points.cols() ) &&(_points.rows() == 3) &&("Error: [laserPolar2xy] points Matrix size is different than number of ranges. Wrong Matrix Size."));
-
   std::vector< double > xy_scan_points;
   double thetaPoint = 0.0;
 
@@ -43,18 +42,16 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, std::v
 
     }//END for
 
-    // TODO REMAP from std::vector to EIGEN::MATRIX
-
     Eigen::Map<Eigen::MatrixXs> matrixReturn(xy_scan_points.data(),3, xy_scan_points.size()/3);
-//    Eigen::MatrixXs matrixReturn(xy_scan_points.data(),3, xy_scan_points.size()/3);
 
-    std::cout << "EIGEN Matrix return: " << matrixReturn.col(3).transpose() << std::endl;
+//    std::cout << "EIGEN Matrix return: " << matrixReturn.col(3).transpose() << std::endl;
     return matrixReturn; // Remap the vector to the Matrix
 
-
 }
 
 
+
+
 Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_theshold, std::queue<unsigned int> & jumps_id_)
 {
   while (!jumps_id_.empty())
@@ -83,3 +80,35 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, const
 
   return Eigen::Map<Eigen::MatrixXs>(points.data(),3, points.size()/3);
 }
+
+
+
+// Victor - Converts ranges from a laserScan to homogeneous coordinates. Returned matrix of same size than ranges size, returning index till valid values.
+unsigned int laserscanutils::ranges2xyh(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points)
+{
+  assert( (_ranges.size() == _points.cols() ) &&(_points.rows() == 3) &&("Error: [laserPolar2xy] points Matrix size is different than number of ranges. Wrong Matrix Size."));
+
+  double thetaPos = 0.0;
+  unsigned int matPosition = 0;
+
+  for(unsigned int i=0; i<_ranges.size(); ++i)
+  {
+    if ( (_ranges[i] >=_params.range_min_) && (_ranges[i] <= _params.range_max_) && !(isnan(_ranges[i])) && !(isinf(_ranges[i]))  )
+    {
+      thetaPos = _params.angle_min_+ _params.angle_step_*(i);
+
+      _points.col(matPosition) << _ranges[i]*cos(thetaPos), _ranges[i]*sin(thetaPos), 1; //row0-> x coordinate, row1->y coordinate, row2->1;
+
+      matPosition++;
+    }
+    else
+    {
+      //std::cout << "Invalid Point: " << _ranges[i] << " - index: " << i << std::endl;
+    }
+
+  }//END for
+
+  return matPosition-1; // Remap the vector to the Matrix - Accounting for 0
+
+
+}

src/scan_basics.h

@@ -53,5 +53,19 @@ namespace laserscanutils
         \return _points is the returned Eigen Matrix of homogeneous points.
     **/
     Eigen::MatrixXs laserPolar2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_theshold, std::queue<unsigned int> & jumps_id_);
+
+
+    /** \brief Receives the ranges from a laser Scanner and returns the valid Homogenian coordinates (X,Y,1)
+
+        This means that invalid scanner measures are filtered above and beyond the range limits and the valid
+        ones are returned in Homogenian Coordinates inside a MatrixXs laser_scan_util's type.
+
+        \param _params is the laser_scan_utils structure of the laser parameters.
+        \param _ranges is the laser range measures
+        \param _points is the returned Eigen Matrix of homogeneous points.
+        \return unsigned int that represents the number of valid points in the scan.
+    **/
+    unsigned int ranges2xyh(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points);
+
 }
 #endif