diff --git a/src/entities.cpp b/src/entities.cpp index da6d3d37fb6b9c6e47fb6aaf1073617ed886e95f..a4a1fafdb375640b9e53260e60ba19b26b99ed78 100644 --- a/src/entities.cpp +++ b/src/entities.cpp @@ -12,9 +12,9 @@ void laserscanutils::Line::print() const // << " first,last: " << first_ << " , " << last_ << std::endl // << " first point: " << point_first_.transpose() << std::endl // << " last point: " << point_last_.transpose() << std::endl - << " number of points: " << np_ << std::endl - << " range: " << range_ << std::endl - << " theta: " << theta_ << std::endl; + << "\tnumber of points: " << np_ << std::endl + << "\trange: " << range_ << std::endl + << "\ttheta: " << theta_ << std::endl; } float laserscanutils::Line::length() diff --git a/src/entities.h b/src/entities.h index 1917aa26a777a687f6be098bd281e340560d3f64..fc72d8107e47eaa4f5d0e4d9f29ef4acbfb857b0 100644 --- a/src/entities.h +++ b/src/entities.h @@ -5,24 +5,30 @@ //laserscanutils #include "laser_scan_utils.h" +//std #include <iostream> #include <list> -//TODO: .cpp with print() functions per each entity namespace laserscanutils { + /** \brief Line entity represents straight segments on the plane + * + * Line entity represents straight segments on the plane + * Several parameterizations can coexist, but no coherency check between them is performed. + * Not all parameterizations are required. + * + **/ struct Line { - Eigen::Vector3s vector_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0 - ScalarT error_; //sum of all distances from used points to line - unsigned int first_; //index of the range vector of the first point used - unsigned int last_; //index of the range vector of the last point used + Eigen::Vector3s vector_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0 TODO: rename to abc_ + unsigned int first_; //corresponding index to the scan ranges of the first point used TODO: rename to idx_first_ + unsigned int last_; //corresponding index to the scan ranges of the last point used TODO: rename to idx_last_ + ScalarT error_; //sum of all distances from used points to line when fitting Eigen::Vector3s point_first_; //homogeneous coordinates of the starting 2D point Eigen::Vector3s point_last_; //homogeneous coordinates of the ending 2D point - unsigned int np_; // number of points of the line - double range_; //range component in polar coordinates - double theta_; //theta component in polar coordinates - //TODO: add an Eigen::Map to the supporting points ... but who ensures memory allocation of such points ??? + unsigned int np_; // number of scan points supporting this line (through fitting, Hough voting, ...) + ScalarT range_; //range component in polar parameterization. + ScalarT theta_; //theta component in polar parameterization. //just a print method void print() const; diff --git a/src/line_detector.cpp b/src/line_detector.cpp index 650930c624626a913368cbf1e8d08918c1a528c3..a371203ad31787e55e6d59c646f4717e8df2875a 100644 --- a/src/line_detector.cpp +++ b/src/line_detector.cpp @@ -204,8 +204,8 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa } -unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixXd> & _laser_cloud, - const ExtractLineParamsHough & _alg_params, +unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixXd> & _points, + const ExtractLinesHoughParams & _alg_params, std::list<laserscanutils::Line> & _line_list ) { double theta, range; diff --git a/src/line_detector.h b/src/line_detector.h index 6fb88ab4a3b30820ae7fdcef943ac8a5be450b0f..c2af976b265038eb8f0ca50dd3daada9815dcd17 100644 --- a/src/line_detector.h +++ b/src/line_detector.h @@ -14,7 +14,7 @@ namespace laserscanutils * Set of tunning parameters for line extraction * */ - struct ExtractLineParams + struct ExtractLineParams //TODO rename to ExtractLinesParams { //members ScalarT jump_dist_ut_; //Upper threshold in consecutive ranges to consider a jump @@ -34,7 +34,7 @@ namespace laserscanutils * set of tunning parameters for the Hough transform line detection * **/ - struct ExtractLineParamsHough + struct ExtractLinesHoughParams { double range_max_; //maximum allowed range for lines double range_step_; //range step in the voting grid @@ -45,7 +45,11 @@ namespace laserscanutils /** \brief Find the best fittig line given a set of points * * Find the best fittig line given a set of points - * Input points at each column of _points matrix + * \requires + * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). + * + * \provides + * \param _line: a laserscanutils::Line object of the best fitting line in the Least Squares sense * **/ void fitLine(const Eigen::MatrixXs & _points, Line & _line); @@ -79,11 +83,8 @@ namespace laserscanutils * * */ -// unsigned int extractLinesHough(const std::list<std::pair<laserscanutils::ScanParams,const std::vector<float> > > _laser_data, -// const ExtractLineParamsHough & _alg_params, -// std::list<laserscanutils::Line> & _line_list); - unsigned int extractLinesHough( const std::vector<Eigen::MatrixXd> & _laser_cloud, - const ExtractLineParamsHough & _alg_params, + unsigned int extractLinesHough( const std::vector<Eigen::MatrixXd> & _points, + const ExtractLinesHoughParams & _alg_params, std::list<laserscanutils::Line> & _line_list);