From 7f76b8a860be1c00bd64554066c21712737984aa Mon Sep 17 00:00:00 2001 From: andreucm <andreu@beta-robots.com> Date: Tue, 23 Feb 2016 20:54:19 +0100 Subject: [PATCH] Tested version of LaserScan, ScanSegment and LineSegment. Still missing merging at ScanSegment level to join close segments. Not convinced on jumping criteria applieda at LaserScan::ranges2xy() --- src/laser_scan.cpp | 35 ++++++++++++++++++++++++----------- src/laser_scan.h | 12 ++++++++---- src/line_finder.h | 4 ++-- src/line_finder_hough.cpp | 11 ++++++++++- src/line_finder_hough.h | 2 +- src/line_finder_jump_fit.cpp | 2 +- src/line_finder_jump_fit.h | 2 +- src/scan_segment.cpp | 26 ++++++++++++++++++++++++++ src/scan_segment.h | 30 +++++++++++++++++++++++++++++- 9 files changed, 102 insertions(+), 22 deletions(-) diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp index 634b323..3f88f9f 100644 --- a/src/laser_scan.cpp +++ b/src/laser_scan.cpp @@ -13,18 +13,19 @@ LaserScan::~LaserScan() } -void LaserScan::setScanParams(const ScanParams & _params) +void LaserScan::setLaserScanParams(const LaserScanParams & _params) { params_ = _params; } -void LaserScan::ranges2xy() +void LaserScan::ranges2xy(Eigen::Matrix4s _device_T) { ScalarT azimuth = params_.angle_min_; ScalarT prev_range = 0; unsigned int ii = 0; unsigned int ii_ok = 0; - ScalarT kr = 1.5; //TODO: as a parameters somewhere. + ScalarT kr = 2; //TODO: as a parameters somewhere. + Eigen::Vector4s point_laser, point_ref; //resize to all points case points_.resize(3,ranges_.size()); @@ -41,13 +42,23 @@ void LaserScan::ranges2xy() ( !std::isnan(ranges_[ii]) ) && ( !std::isinf(ranges_[ii]) ) ) { - //transform from polar to euclidean - points_(0,ii_ok) = ranges_[ii] * cos(azimuth); - points_(1,ii_ok) = ranges_[ii] * sin(azimuth); - points_(2,ii_ok) = 1; + //transform the laser hit from polar to 3D euclidean homogeneous + point_laser << ranges_[ii]*cos(azimuth), ranges_[ii]*sin(azimuth),0,1; + + //apply device mounting point calibration (p_r = T * p_l) + point_ref = _device_T*point_laser; + + //set to points_ as a 2D homogeneous + points_.block<3,1>(0,ii_ok) << point_ref(0),point_ref(1),1; + +// //transform from polar to euclidean +// points_(0,ii_ok) = ranges_[ii] * cos(azimuth); +// points_(1,ii_ok) = ranges_[ii] * sin(azimuth); +// points_(2,ii_ok) = 1; //check jump. Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times if ( fabs(ranges_[ii]-prev_range) > kr*ranges_[ii]*params_.angle_step_) //jump condition (kr*r*sin(a) ~ kr*r*a) + //if ( fabs(ranges_[ii]-prev_range) > 0.5) //jump condition >0.5m { jumps_.push_back(ii_ok); } @@ -56,11 +67,13 @@ void LaserScan::ranges2xy() ii_ok ++; //keep current range as previous for the next iteration - prev_range = ranges_[ii]; - - //increment azimuth with angle step - azimuth += params_.angle_step_; + prev_range = ranges_[ii]; } + else + prev_range = 0; + + //increment azimuth with angle step + azimuth += params_.angle_step_; } //push back the last index to jumps_, to properly close the jumps_ vector. This will be used by findSegments() diff --git a/src/laser_scan.h b/src/laser_scan.h index 7f36d90..e0a0f17 100644 --- a/src/laser_scan.h +++ b/src/laser_scan.h @@ -18,7 +18,7 @@ namespace laserscanutils * Laser scan parameters * **/ -struct ScanParams +struct LaserScanParams { //members double angle_min_; //radians @@ -41,7 +41,7 @@ struct ScanParams class LaserScan { protected: - ScanParams params_; + LaserScanParams params_; public: //Ordered raw range data @@ -73,7 +73,7 @@ class LaserScan * Set scan params. * **/ - void setScanParams(const laserscanutils::ScanParams & _params); + void setLaserScanParams(const laserscanutils::LaserScanParams & _params); /** \brief Transforms from ranges (polar) to euclidean (xy) * @@ -81,10 +81,14 @@ class LaserScan * Invalid values (inf's and nan's) and out of range measurements are not transformed. * Valid measurements are set to points_ in homogeneous coordinates. * + * _device_T is the homogeneous transformation, moving points from laser device frame to the reference one: + * p_ref = _device_T*p_laser; + * If _device_T is provided, points are transformed to a new reference frame. + * * Set also the jumps_ vector, which after the call holds the indexes to points_ where a scan segment starts * **/ - void ranges2xy(); + void ranges2xy(Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity()); /** \brief Find segments based on jumps of consecutive scan points * diff --git a/src/line_finder.h b/src/line_finder.h index 1310f9f..6d888d6 100644 --- a/src/line_finder.h +++ b/src/line_finder.h @@ -69,8 +69,8 @@ class LineFinder * \return Number of lines extracted. * */ - virtual unsigned int findLines( const Eigen::MatrixXd & _points, - std::list<LineSegment> & _line_list) const = 0; + virtual unsigned int findLines( const Eigen::MatrixXs & _points, + std::list<laserscanutils::LineSegment> & _line_list) = 0; /** \brief Print things * diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp index 10a6e0a..de178a4 100644 --- a/src/line_finder_hough.cpp +++ b/src/line_finder_hough.cpp @@ -30,7 +30,7 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params) } } -unsigned int LineFinderHough::findLines( const Eigen::MatrixXd & _points, +unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points, std::list<laserscanutils::LineSegment> & _line_list) { double theta, range; @@ -41,6 +41,15 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXd & _points, Eigen::MatrixXs pts; //auxiliar array of points std::list<Eigen::Vector3s>::iterator pts_it; + //clear the Houhg Grid + for (unsigned int ii = 0; ii < hough_grid_.size(); ii++) + { + for (unsigned int jj = 0; jj < hough_grid_.at(ii).size(); jj++) + { + hough_grid_.at(ii).at(jj).clear(); + } + } + //STEP 1. For each scan point, accumulate hits in the Hough Grid for (unsigned int ipt = 0; ipt < _points.cols(); ipt++) //loop over all points { diff --git a/src/line_finder_hough.h b/src/line_finder_hough.h index 2e99334..e2a2c4c 100644 --- a/src/line_finder_hough.h +++ b/src/line_finder_hough.h @@ -78,7 +78,7 @@ class LineFinderHough : public LineFinder * * */ - unsigned int findLines( const Eigen::MatrixXd & _points, + unsigned int findLines( const Eigen::MatrixXs & _points, std::list<laserscanutils::LineSegment> & _line_list); /** \brief Print things diff --git a/src/line_finder_jump_fit.cpp b/src/line_finder_jump_fit.cpp index 28f5e58..393129d 100644 --- a/src/line_finder_jump_fit.cpp +++ b/src/line_finder_jump_fit.cpp @@ -18,7 +18,7 @@ void LineFinderJumpFit::setJumpFitParams(const LineFinderJumpFitParams & _params this->jump_fit_params_ = _params; } -unsigned int LineFinderJumpFit::findLines( const Eigen::MatrixXd & _points, +unsigned int LineFinderJumpFit::findLines( const Eigen::MatrixXs & _points, std::list<laserscanutils::LineSegment> & _line_list) { //TODO diff --git a/src/line_finder_jump_fit.h b/src/line_finder_jump_fit.h index 8f6b2fb..e6040cb 100644 --- a/src/line_finder_jump_fit.h +++ b/src/line_finder_jump_fit.h @@ -74,7 +74,7 @@ class LineFinderJumpFit : public LineFinder * * */ - unsigned int findLines( const Eigen::MatrixXd & _points, + unsigned int findLines( const Eigen::MatrixXs & _points, std::list<laserscanutils::LineSegment> & _line_list); /** \brief Print things diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp index 4953347..4d60dc1 100644 --- a/src/scan_segment.cpp +++ b/src/scan_segment.cpp @@ -22,6 +22,32 @@ 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; diff --git a/src/scan_segment.h b/src/scan_segment.h index fe6acdd..6dff963 100644 --- a/src/scan_segment.h +++ b/src/scan_segment.h @@ -45,6 +45,34 @@ 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. @@ -58,7 +86,7 @@ class ScanSegment * Clearance is an extra distance added to the bb limits. Typically half od the grid cell size * **/ - void computeBoundingBox(const ScalarT & _clearance = 0.01); + void computeBoundingBox(const ScalarT & _clearance = 0.1); /** \brief Compute both centroids and bounding box * -- GitLab