LaserScan and ScanSegment built successfully, and ready to be tested

src/laser_scan.cpp

@@ -24,7 +24,7 @@ void LaserScan::ranges2xy()
     ScalarT prev_range = 0;
     unsigned int ii = 0;
     unsigned int ii_ok = 0;
-    ScalarT kr = 1.5; //TODO: as a parameters somewhere. Times 
+    ScalarT kr = 1.5; //TODO: as a parameters somewhere. 
 
     //resize to all points case
     points_.resize(3,ranges_.size());
@@ -62,10 +62,41 @@ void LaserScan::ranges2xy()
             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);
 
     //resize the output matrix to the number of correct points, while keeping values
     points_.conservativeResize(3, ii_ok);
 }
 
+void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_list)
+{
+    std::list<unsigned int>::iterator jumps_it, next_jumps_it, jumps_last; 
+    unsigned int num_points; 
+    
+    //set jumps_last to the last valid element of jumps
+    jumps_last = std::prev(jumps_.end()); 
+    
+    //run over all jumps (except the last, which indicates the closing index) 
+    for (jumps_it = jumps_.begin(); jumps_it != jumps_last; jumps_it ++)
+    {
+        //new segment in the list
+        _segment_list.push_back(ScanSegment()); 
+        
+        //check how many points 
+        next_jumps_it = jumps_it;
+        next_jumps_it ++; 
+        num_points = (*next_jumps_it) - (*jumps_it);
+        
+        //fill points
+        _segment_list.back().points_.resize(3,num_points);
+        for ( unsigned int ii=0; ii < num_points; ii++)
+        {
+            _segment_list.back().points_.block<3,1>(0,ii) << this->points_.block<3,1>(0,(*jumps_it)+ii);
+        }
+    }
+}
+
 }//namespace
 

src/laser_scan.h

@@ -3,6 +3,7 @@
 
 //laserscanutils
 #include "laser_scan_utils.h"
+#include "scan_segment.h"
 
 //std
 #include <vector>
@@ -72,7 +73,7 @@ class LaserScan
          * Set scan params. 
          * 
          **/
-        void setScanParams(const ScanParams & _params);
+        void setScanParams(const laserscanutils::ScanParams & _params);
         
         /** \brief Transforms from ranges (polar) to euclidean (xy)
         *
@@ -85,12 +86,13 @@ class LaserScan
         **/
         void ranges2xy();        
 
-        /** \brief Detect jumps on range data
+        /** \brief Find segments based on jumps of consecutive scan points
         *
-        * Detect jumps on range data. Sets jumps_ vector
+        * Find segments based on jumps of consecutive scan points
+        * Do not compute segment parameters, just fill ScanSegment.points_
         *
         **/
-//         void detectJumps();        
+        void findSegments(std::list<laserscanutils::ScanSegment> & _segment_list);        
       
 };
 

src/scan_segment.cpp

@@ -2,8 +2,12 @@
 
 namespace laserscanutils
 {
+
+//init static segment counter
+unsigned int ScanSegment::segment_id_count_ = 0;
     
-ScanSegment::ScanSegment()
+ScanSegment::ScanSegment():
+    segment_id_(++segment_id_count_)
 {
 
 }
@@ -20,17 +24,97 @@ void ScanSegment::merge(const ScanSegment & _segment)
 
 void ScanSegment::computeCentroid()
 {
-    //TODO
+    ScalarT mx=0, my=0; 
+    for (unsigned int ii=0; ii<points_.cols(); ii++)
+    {
+        mx += points_(0,ii); 
+        my += points_(1,ii); 
+    }
+
+    centroid_ << mx/points_.cols(), my/points_.cols(), 1; 
 }
 
 void ScanSegment::computeBoundingBox(const double & _clearance)
 {
-    //TODO
+    double cxx, cyy, cxy; //variance and covariance terms
+    Eigen::MatrixXd points_o, points_c; //points wrt origin, points wrt cov eigen vectors
+    Eigen::Matrix2d c_mat; //covariance matrix of cluster points
+    Eigen::EigenSolver<Eigen::Matrix2d>::EigenvectorsType e_mat; //matrix of eigenvectors (could be complex or real)
+    Eigen::EigenSolver<Eigen::Matrix2d>::EigenvalueType evals; //matrix of eigenvectors (could be complex or real)
+    Eigen::Matrix2d r_mat; //real velued rotation matrix
+    Eigen::Matrix<double, 2,4> corners_c; //corners wrt cov eigen vectors
+    Eigen::Matrix<double, 2,4> corners_o; //Final box corners. wrt global axis and translated to cluster centroid
+    
+    //copy cluster point x,y coordinates to an Eigen Matrix, subtracting centroid coordinates
+    points_o.resize(2,points_.cols());
+    for (unsigned int ii=0; ii<points_.cols(); ii++) 
+    {
+        points_o.block<2,1>(0,ii) << (points_(0,ii)-centroid_(0)) , (points_(1,ii)-centroid_(1)); 
+    }
+
+    //compute covariance matrix (c_mat)
+    cxx = points_o.row(0).dot(points_o.row(0))/points_.cols();
+    cyy = points_o.row(1).dot(points_o.row(1))/points_.cols(); 
+    cxy = points_o.row(0).dot(points_o.row(1))/points_.cols(); 
+    c_mat << cxx,cxy,cxy,cyy; 
+    
+    //get eigen vectors of c_mat
+    Eigen::EigenSolver<Eigen::Matrix2d> e_solver(c_mat);
+    e_mat = e_solver.eigenvectors();
+    evals = e_solver.eigenvalues();
+    
+    //keep eigen values. eval_1_ is the largest
+    if ( evals(0).real() > evals(1).real() )
+    {
+        eval_1_ = evals(0).real();
+        eval_2_ = evals(1).real();
+    }
+    else
+    {
+        eval_1_ = evals(1).real();
+        eval_2_ = evals(0).real();
+    }
+    
+    //mount a Real rotation matrix. e_mat is real since c_mat is positive symetric
+    r_mat << e_mat(0,0).real(), e_mat(0,1).real(), e_mat(1,0).real(), e_mat(1,1).real();
+    
+    //rotate all points_o to points_c
+    points_c.resize(2,points_.cols());
+    points_c = r_mat.inverse()*points_o; 
+    
+    //get min and max values
+    double min_x = points_c.row(0).minCoeff();
+    double max_x = points_c.row(0).maxCoeff();
+    double min_y = points_c.row(1).minCoeff();
+    double max_y = points_c.row(1).maxCoeff();
+    
+    //set 4 corners of the box wrt c. One corner per column. Order is: top-left, top-right, bottom-left, bottom-right
+    corners_c.row(0) << min_x-_clearance,max_x+_clearance,max_x+_clearance,min_x-_clearance; //x coordinates
+    corners_c.row(1) << max_y+_clearance,max_y+_clearance,min_y-_clearance,min_y-_clearance; //y coordinates
+
+    //rotate corners to origin frame
+    corners_o = r_mat*corners_c; 
+    
+    //set class member bbox_corners_, adding the translation (centroids) substracted at the beggining of this function
+    for (unsigned int ii=0; ii<4; ii++)
+    {
+        bbox_corners_[ii] << corners_o(0,ii)+centroid_(0), corners_o(1,ii)+centroid_(1), 1;
+    }
+}
+
+void ScanSegment::computeAll()
+{
+    computeCentroid();  
+    computeBoundingBox();
 }
       
 void ScanSegment::print() const
 {
-    //TODO
+    //print segment data
+    std::cout 
+            << "\tsegment_id_: " << segment_id_ << std::endl 
+            << "\tcentroid_x_: " << centroid_(0) << std::endl 
+            << "\tcentroid_y_: " << centroid_(1)<< std::endl;
 }
 
 }//namespace

src/scan_segment.h

@@ -4,6 +4,9 @@
 //laserscanutils
 #include "laser_scan_utils.h"
 
+//std
+#include <iostream>
+
 //open namespace
 namespace laserscanutils
 {
@@ -17,14 +20,16 @@ namespace laserscanutils
 class ScanSegment
 {
     protected:
+        unsigned int segment_id_; //Segment id. 
+        static unsigned int segment_id_count_; //Segment counter (acts as simple ID factory)        
         Eigen::Vector3s centroid_; //homogeneous coordinates of the segment centroid 
         Eigen::Vector3s bbox_corners_[4]; //4 corners of the bounding box [m]
-        ScalarT eval_1_, eval_2_; //eigenvalues. eval_1_ isthe biggest one
+        ScalarT eval_1_, eval_2_; //eigenvalues. eval_1_ is the biggest one
     
     public:
-        /** \brief Set of points in homogeneous coordinates
+        /** \brief Set of points belonging to this segment
          * 
-         * Set of points in homogeneous coordinates. 
+         * Set of points belonging to this segment, in homogeneous coordinates. 
          * Each column is a point (x,y,1)^T
          * 
          */
@@ -53,7 +58,16 @@ class ScanSegment
          * Clearance is an extra distance added to the bb limits. Typically half od the grid cell size
          * 
          **/
-        void computeBoundingBox(const double & _clearance);
+        void computeBoundingBox(const ScalarT & _clearance = 0.01);
+        
+        /** \brief Compute both centroids and bounding box
+         * 
+         * Compute both centroids and bounding box
+         * 
+         **/
+        void computeAll(); 
+        
+        
               
         //print
         void print() const;