Added class PointSet, as a base for ScanSegment and GridCluster

src/CMakeLists.txt

@@ -33,6 +33,7 @@ SET(HDRS
     line_finder_hough.h
     line_finder_jump_fit.h
     line_segment.h
+    point_set.h
     scan_basics.h
     scan_segment.h
     clustering.h
@@ -49,6 +50,7 @@ SET(SRCS
     line_finder_hough.cpp
     line_finder_jump_fit.cpp
     line_segment.cpp
+    point_set.cpp
     scan_basics.cpp
     scan_segment.cpp
     clustering.cpp

src/point_set.cpp

+#include "scan_segment.h"
+
+namespace laserscanutils
+{
+    
+PointSet::PointSet()
+{
+    //
+}
+        
+PointSet::~PointSet()
+{
+    //
+}
+        
+void PointSet::merge(const PointSet & _segment)
+{
+    //TODO
+}
+
+unsigned int PointSet::numPoints() const
+{
+    return points_.cols(); 
+}
+
+const Eigen::Vector3s & PointSet::getCentroid() const
+{
+    return centroid_; 
+}
+
+const Eigen::Vector3s & PointSet::getBBox(unsigned int _corner_idx) const
+{
+    if (_corner_idx < 4)
+        return bbox_corners_[_corner_idx]; 
+    else
+        return bbox_corners_[0]; 
+}
+
+ScalarT PointSet::getEvalRatio() const
+{
+    if ( eval_1_ != 0 )
+        return eval_2_/eval_1_; 
+    else
+        return 1e10;
+}
+
+void PointSet::computeCentroid()
+{
+    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 PointSet::computeBoundingBox(const double & _clearance)
+{
+    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 PointSet::computeAll()
+{
+    computeCentroid();  
+    computeBoundingBox();
+}
+      
+void PointSet::print() const
+{
+    //print segment data
+    std::cout 
+            << "\tcentroid_x_: " << centroid_(0) << std::endl 
+            << "\tcentroid_y_: " << centroid_(1)<< std::endl;
+}
+
+}//namespace

src/point_set.h

+#ifndef POINT_SET_H_
+#define POINT_SET_H_
+
+//laserscanutils
+#include "laser_scan_utils.h"
+
+//std
+#include <iostream>
+
+//open namespace
+namespace laserscanutils
+{
+/** \brief Scan segment
+* 
+* A set of points, and methods to compute some parameters
+* 
+*/
+class PointSet
+{
+    protected:
+        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_ is the biggest one
+    
+    public:
+        /** \brief Set of points belonging to this segment
+         * 
+         * Set of points belonging to this segment, in homogeneous coordinates. 
+         * Each column is a point (x,y,1)^T
+         * 
+         */
+        Eigen::MatrixXs points_; 
+
+    public:
+        //constructor
+        PointSet();
+        
+        //Destructor
+        ~PointSet();
+        
+        //merges this PointSet with the argument. 
+        void merge(const PointSet & _segment); 
+        
+        /** \brief Return the number of points supporting this segment
+         * 
+         * Return the number of points supporting this segment
+         * 
+         **/
+        unsigned int numPoints() const;  
+        
+        /** \brief Return const ref to centroid
+         * 
+         * Return const ref to centroid
+         * 
+         **/
+        const Eigen::Vector3s & getCentroid() const;  
+        
+        /** \brief Return const ref to one corner of the bounding box
+         * 
+         * Return const ref to one corner of the bounding box
+         * 
+         **/
+        const Eigen::Vector3s & getBBox(unsigned int _corner_idx) const;  
+        
+        /** \brief Return the ratio eval_2_/eval_1_
+         * 
+         * Return the ratio eval_2_/eval_1_
+         * 
+         **/        
+        ScalarT getEvalRatio() const; 
+
+        /** \brief Compute centroid coordinates
+         *
+         * Compute centroid coordinate.
+         *      
+         **/        
+        void computeCentroid(); 
+        
+        /** \brief Compute the oriented bounding box
+         * 
+         * Compute the oriented bounding box
+         * Clearance is an extra distance added to the bb limits. Typically half od the grid cell size
+         * 
+         **/
+        void computeBoundingBox(const ScalarT & _clearance = 0.1);
+        
+        /** \brief Compute both centroids and bounding box
+         * 
+         * Compute both centroids and bounding box
+         * 
+         **/
+        void computeAll(); 
+                   
+        //print
+        virtual void print() const;    
+};
+}//namespace
+#endif
\ No newline at end of file

src/scan_segment.cpp

@@ -7,14 +7,15 @@ namespace laserscanutils
 unsigned int ScanSegment::segment_id_count_ = 0;
     
 ScanSegment::ScanSegment():
+    PointSet(),
     segment_id_(++segment_id_count_)
 {
-
+    //
 }
         
 ScanSegment::~ScanSegment()
 {
-
+    //
 }
         
 void ScanSegment::merge(const ScanSegment & _segment)
@@ -22,118 +23,6 @@ void ScanSegment::merge(const ScanSegment & _segment)
     //TODO
 }
 
-unsigned int ScanSegment::numPoints() const
-{
-    return points_.cols(); 
-}
-
-const Eigen::Vector3s & ScanSegment::getCentroid() const
-{
-    return centroid_; 
-}
-
-const Eigen::Vector3s & ScanSegment::getBBox(unsigned int _corner_idx) const
-{
-    if (_corner_idx < 4)
-        return bbox_corners_[_corner_idx]; 
-    else
-        return bbox_corners_[0]; 
-}
-
-ScalarT ScanSegment::getEvalRatio() const
-{
-    if ( eval_1_ != 0 )
-        return eval_2_/eval_1_; 
-    else
-        return 1e10;
-}
-
-void ScanSegment::computeCentroid()
-{
-    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)
-{
-    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
 {
     //print segment data

src/scan_segment.h

@@ -3,6 +3,7 @@
 
 //laserscanutils
 #include "laser_scan_utils.h"
+#include "point_set.h"
 
 //std
 #include <iostream>
@@ -17,24 +18,12 @@ namespace laserscanutils
 * "Neigbors" implies scan ordering is required 
 * 
 */
-class ScanSegment
+class ScanSegment : public PointSet
 {
     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_ is the biggest one
     
-    public:
-        /** \brief Set of points belonging to this segment
-         * 
-         * Set of points belonging to this segment, in homogeneous coordinates. 
-         * Each column is a point (x,y,1)^T
-         * 
-         */
-        Eigen::MatrixXs points_; 
-
     public:
         //constructor
         ScanSegment();
@@ -44,61 +33,9 @@ class ScanSegment
         
         //merges this ScanSegment with the argument. 
         void merge(const ScanSegment & _segment); 
-        
-        /** \brief Return the number of points supporting this segment
-         * 
-         * Return the number of points supporting this segment
-         * 
-         **/
-        unsigned int numPoints() const;  
-        
-        /** \brief Return const ref to centroid
-         * 
-         * Return const ref to centroid
-         * 
-         **/
-        const Eigen::Vector3s & getCentroid() const;  
-        
-        /** \brief Return const ref to one corner of the bounding box
-         * 
-         * Return const ref to one corner of the bounding box
-         * 
-         **/
-        const Eigen::Vector3s & getBBox(unsigned int _corner_idx) const;  
-        
-        /** \brief Return the ratio eval_2_/eval_1_
-         * 
-         * Return the ratio eval_2_/eval_1_
-         * 
-         **/        
-        ScalarT getEvalRatio() const; 
-
-        /** \brief Compute centroid coordinates
-         *
-         * Compute centroid coordinate.
-         *      
-         **/        
-        void computeCentroid(); 
-        
-        /** \brief Compute the oriented bounding box
-         * 
-         * Compute the oriented bounding box
-         * Clearance is an extra distance added to the bb limits. Typically half od the grid cell size
-         * 
-         **/
-        void computeBoundingBox(const ScalarT & _clearance = 0.1);
-        
-        /** \brief Compute both centroids and bounding box
-         * 
-         * Compute both centroids and bounding box
-         * 
-         **/
-        void computeAll(); 
-        
-        
-              
+                           
         //print
-        void print() const;    
+        virtual void print() const;    
 };
 }//namespace
 #endif
\ No newline at end of file