diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 63b08cdc7a135d4bcebe4a359972cb763b6c713d..f95a3926851dc2335997e94cf9c46e5b499fe4ff 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -25,6 +25,9 @@ SET(HDRS_BASE SET(HDRS corner_detector.h + corner_finder.h + corner_finder_range_diff.h + corner_point.h entities.h grid_2d.h grid_cluster.h @@ -42,7 +45,11 @@ SET(HDRS #sources SET(SRCS + corner_point.cpp corner_detector.cpp + corner_finder.cpp + corner_finder_range_diff.cpp + corner_point.cpp entities.cpp grid_2d.cpp grid_cluster.cpp diff --git a/src/corner_finder.cpp b/src/corner_finder.cpp new file mode 100644 index 0000000000000000000000000000000000000000..73ac303629b3bfab70733add8836abdbb1d1e2ac --- /dev/null +++ b/src/corner_finder.cpp @@ -0,0 +1,17 @@ +#include "corner_finder.h" + +namespace laserscanutils +{ + +CornerFinder::CornerFinder() +{ + +} + +CornerFinder::~CornerFinder() +{ + +} + +}//namespace + diff --git a/src/corner_finder.h b/src/corner_finder.h new file mode 100644 index 0000000000000000000000000000000000000000..c9fd969746807776cfea29322f0ac43d825f7218 --- /dev/null +++ b/src/corner_finder.h @@ -0,0 +1,61 @@ +#ifndef CORNER_FINDER_H_ +#define CORNER_FINDER_H_ + +//laserscanutils +#include "laser_scan_utils.h" +#include "corner_point.h" + +//std +#include <list> + +namespace laserscanutils +{ +/** \brief Base Class for methods to extract corners from a scan +* +* Base Class for methods to extract corners from a scan +* +*/ +class CornerFinder +{ + protected: + + public: + /** \brief Constructor + * + * Constructor + * + **/ + CornerFinder(); + + /** \brief Destructor + * + * Destructor + * + **/ + ~CornerFinder(); + + /** \brief Find corners. Pure virtual. To be implemented by each inherited class + * + * Find corners from a set of scans. + * Returns corners as a std::list<CornerPoint> + * + * \Requires: + * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required. + * + * \Provides: + * \param _corner_list set of corners extracted from _points + * \return Number of corners extracted. + * + */ + virtual unsigned int findCorners( const Eigen::MatrixXs & _points, + std::list<laserscanutils::CornerPoint> & _corner_list) = 0; + + /** \brief Print things + * + * Print things about this class + * + **/ + virtual void print() const = 0; +}; +}//namespace +#endif diff --git a/src/corner_finder_range_diff.cpp b/src/corner_finder_range_diff.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7b996896f168a61afb89ef77cc088a7328f90048 --- /dev/null +++ b/src/corner_finder_range_diff.cpp @@ -0,0 +1,27 @@ +#include "corner_finder_range_diff.h" + +namespace laserscanutils +{ + +CornerFinderRangeDiff::CornerFinderRangeDiff() : CornerFinder() +{ + +} + +CornerFinderRangeDiff::~CornerFinderRangeDiff() +{ + +} + +unsigned int CornerFinderRangeDiff::findCorners( const Eigen::MatrixXs & _points, + std::list<laserscanutils::CornerPoint> & _corner_list) +{ + +} + +void CornerFinderRangeDiff::print() const +{ + +} + +}//namespace diff --git a/src/corner_finder_range_diff.h b/src/corner_finder_range_diff.h new file mode 100644 index 0000000000000000000000000000000000000000..182f1cc339d4f2babc20c161a7c3ab261d67e24e --- /dev/null +++ b/src/corner_finder_range_diff.h @@ -0,0 +1,82 @@ +#ifndef CORNER_FINDER_RANGE_DIFF_H_ +#define CORNER_FINDER_RANGE_DIFF_H_ + +//laserscanutils +#include "corner_finder.h" +#include "line_finder_hough.h" + +namespace laserscanutils +{ + +/** \brief set of tunning parameters for the Hough transform line detection +* +* set of tunning parameters for the Hough transform line detection +* +*/ +struct CornerFinderRangeDiffParams +{ + ScalarT zero_range_diff_; //threshold to consider range diff between consecutive scan hits is zero + ScalarT corner_window_size_; //number of scan points, separately at left and right of the corner, to consider to fit a line + ScalarT line_fit_error_; //Maximum allowed error to consider thar both left and right subset fits in a line + ScalarT max_aperture_; //Maximum aperture angle to consider a candidate point as a corner. See corner.h for a precise definition of corner aperture + void print() const; //just a print method +}; + +/** \brief Base Class for methods to extract corners from a scan +* +* Base Class for methods to extract corners from a scan +* +*/ +class CornerFinderRangeDiff : public CornerFinder +{ + protected: + CornerFinderRangeDiffParams params_; + LineFinderHough line_finder_; //required just fitLine method, but cannot use LineFinder class becaus it is virtual. TODO: May devirtualize LineFinder + + public: + /** \brief Constructor + * + * Constructor + * + **/ + CornerFinderRangeDiff(); + + /** \brief Destructor + * + * Destructor + * + **/ + ~CornerFinderRangeDiff(); + + /** \brief Set tunning params + * + * Set tunning params. + * + **/ + void setParams(const CornerFinderRangeDiffParams & _params); + + /** \brief Find corners. + * + * Find corners from a set of scans. + * Returns corners as a std::list<CornerPoint> + * + * \Requires: + * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required. + * + * \Provides: + * \param _corner_list set of corners extracted from _points + * \return Number of corners extracted. + * + */ + unsigned int findCorners( const Eigen::MatrixXs & _points, + std::list<laserscanutils::CornerPoint> & _corner_list); + + /** \brief Print things + * + * Print things about this class + * + **/ + void print() const; +}; +}//namespace +#endif diff --git a/src/corner_point.cpp b/src/corner_point.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9cdf404a76073e5002ca19a72ff46a7234b3765c --- /dev/null +++ b/src/corner_point.cpp @@ -0,0 +1,26 @@ +#include "corner_point.h" + +//open namespace +namespace laserscanutils +{ + +CornerPoint::CornerPoint() +{ + +} + +CornerPoint::~CornerPoint() +{ + +} + +void CornerPoint::print() const +{ + std::cout << "CornerPoint Parameters : " << std::endl + << "\tpoint: " << point_.transpose() << std::endl + << "\torientation: " << orientation_ << std::endl + << "\taperture: " << aperture_ << std::endl; +} + +}//namespace + diff --git a/src/corner_point.h b/src/corner_point.h new file mode 100644 index 0000000000000000000000000000000000000000..98a0011db27e44cc369bf785cb48ba4a3db64cdd --- /dev/null +++ b/src/corner_point.h @@ -0,0 +1,39 @@ +#ifndef CORNER_POINT_H_ +#define CORNER_POINT_H_ + +//laserscanutils +#include "laser_scan_utils.h" + +//std +#include <list> +#include <iostream> + +//open namespace +namespace laserscanutils +{ +/** \brief Corner point +* +* Corner point +* +*/ +class CornerPoint +{ + public: + Eigen::Vector3s point_; //2D corner coordinates in homogeneous form + ScalarT orientation_; //orientation angle from the line1 to x axis, in [-pi,pi] + ScalarT aperture_; //angle aperture of the corner in [0,2pi]. Aperture angle defined passing thorugh the shadow area of the scan (not crossing rays) +// Line line_1_; // line of the corner +// Line line_2_; // line of the corner + + public: + //constructor + CornerPoint(); + + //Destructor + ~CornerPoint(); + + //print + void print() const; +}; +}//namespace +#endif diff --git a/src/line_finder_hough.h b/src/line_finder_hough.h index 2f43810c672334129bdf790a21bd13a2322afb48..a627990085f6872d0d40bc0e569f77f27a9d5951 100644 --- a/src/line_finder_hough.h +++ b/src/line_finder_hough.h @@ -13,9 +13,9 @@ namespace laserscanutils */ struct LineFinderHoughParams { - double range_max_; //maximum allowed range for lines - double range_step_; //range step in the voting grid - double theta_step_; //theta step in the voting grid + ScalarT range_max_; //maximum allowed range for lines + ScalarT range_step_; //range step in the voting grid + ScalarT theta_step_; //theta step in the voting grid unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line void print() const; //just a print method };