diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f5ca0efeeb7cd46f688475d55138da4585340050..2f726f02431b209b5d6eee55512175a4c8b40aff 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/point_set.cpp b/src/point_set.cpp new file mode 100644 index 0000000000000000000000000000000000000000..70ad4a488b890098e59d68513b21ae5cb9956c44 --- /dev/null +++ b/src/point_set.cpp @@ -0,0 +1,141 @@ +#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 diff --git a/src/point_set.h b/src/point_set.h new file mode 100644 index 0000000000000000000000000000000000000000..f270d9b0a9c49492154ee11375fb263c3267e724 --- /dev/null +++ b/src/point_set.h @@ -0,0 +1,98 @@ +#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 diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp index 4d60dc190f77a4930de7426ddac6651aee9d1944..8b65bb6fa024b1c53080367fbf764eda4083f068 100644 --- a/src/scan_segment.cpp +++ b/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 diff --git a/src/scan_segment.h b/src/scan_segment.h index 6dff963bd81e5837f20a1787f97b5bfd84fc5dfe..6b1f2b8b9ad12b02503ca1d5485e11a49e4f18ec 100644 --- a/src/scan_segment.h +++ b/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