diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp index a146372816dd22864c824446ac69f2295562f97c..634b323b1b76447de0f0f67e276c722c66882078 100644 --- a/src/laser_scan.cpp +++ b/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 diff --git a/src/laser_scan.h b/src/laser_scan.h index 618a1a74e923c9897669c008c921a682ce079f9f..7f36d90655c4456ef803f51bd601bb6b42281283 100644 --- a/src/laser_scan.h +++ b/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); }; diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp index 508f28e990daa06c3a4d709a84832b820d260062..49533474f3dd72b4225408c80d9775deca418f97 100644 --- a/src/scan_segment.cpp +++ b/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 diff --git a/src/scan_segment.h b/src/scan_segment.h index 8612e8c4e4bcb3468519a562002d97713f288aa0..fe6acdd7e0fbe5c17041363a1460b0bbbd051a40 100644 --- a/src/scan_segment.h +++ b/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;