From 6eef18fbfdd90f9bcd88628ef883c54dac7b77b0 Mon Sep 17 00:00:00 2001
From: andreucm <andreu@beta-robots.com>
Date: Mon, 22 Feb 2016 09:37:25 +0100
Subject: [PATCH] Set of intial classes deployed. Still a lot of methods TO
DO.
---
src/line_finder.cpp | 34 +++++++++
src/line_finder.h | 80 ++++++++++++++++++++
src/line_finder_hough.cpp | 137 +++++++++++++++++++++++++++++++++++
src/line_finder_hough.h | 91 +++++++++++++++++++++++
src/line_finder_jump_fit.cpp | 32 ++++++++
src/line_finder_jump_fit.h | 87 ++++++++++++++++++++++
src/line_segment.cpp | 41 +++++++++++
src/line_segment.h | 49 +++++++++++++
src/scan_segment.cpp | 22 ++++++
src/scan_segment.h | 42 +++++++++++
10 files changed, 615 insertions(+)
create mode 100644 src/line_finder.cpp
create mode 100644 src/line_finder.h
create mode 100644 src/line_finder_hough.cpp
create mode 100644 src/line_finder_hough.h
create mode 100644 src/line_finder_jump_fit.cpp
create mode 100644 src/line_finder_jump_fit.h
create mode 100644 src/line_segment.cpp
create mode 100644 src/line_segment.h
create mode 100644 src/scan_segment.cpp
create mode 100644 src/scan_segment.h
diff --git a/src/line_finder.cpp b/src/line_finder.cpp
new file mode 100644
index 0000000..43481ab
--- /dev/null
+++ b/src/line_finder.cpp
@@ -0,0 +1,34 @@
+#include "line_finder.h"
+
+laserscanutils::LineFinder::LineFinder()
+{
+
+}
+
+laserscanutils::LineFinder::~LineFinder()
+{
+
+}
+
+void laserscanutils::LineFinder::fitLine(const Eigen::MatrixXs & _points, LineSegment & _line)
+{
+ //build the system
+ Eigen::Matrix3s AA = _points * _points.transpose();
+ AA.row(2) << 0,0,1;
+
+ //solve for line
+ _line.abc_ = AA.inverse().col(2);
+
+ // normalize the line
+ _line.abc_ /= _line.abc_.head(2).norm();
+
+ // compute fitting error
+ _line.fit_error_ = (_points.transpose() * _line.abc_).squaredNorm() / _points.cols();
+ //_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / (_line.abc_.head(2).norm()*_points.cols());
+}
+
+// unsigned int laserscanutils::LineFinder::mergeLines(std::list<LineSegment> & _line_list)
+// {
+// //TODO
+// }
+
diff --git a/src/line_finder.h b/src/line_finder.h
new file mode 100644
index 0000000..e1c0913
--- /dev/null
+++ b/src/line_finder.h
@@ -0,0 +1,80 @@
+#ifndef LINE_FINDER_H_
+#define LINE_FINDER_H_
+
+//laserscanutils
+#include "laser_scan_utils.h"
+#include "line_segment.h"
+#include "scan_basics.h"
+
+namespace laserscanutils
+{
+/** \brief Base Class for methods to extract striaght segments from a scan
+*
+* Base Class for methods to extract striaght segments from a scan
+*
+*/
+class LineFinder
+{
+ protected:
+
+ public:
+ /** \brief Constructor
+ *
+ * Constructor
+ *
+ **/
+ LineFinder();
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ ~LineFinder();
+
+ /** \brief Find the best fittig line given a set of points
+ *
+ * Find the best fittig line given a set of points
+ *
+ * \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 _line: a laserscanutils::Line object of the best fitting line in the Least Squares sense
+ *
+ **/
+ void fitLine(const Eigen::MatrixXs & _points, LineSegment & _line);
+
+ /** \brief Merges lines of a list
+ *
+ * Updates the input list with the merged lines
+ * Lines are merged if all below conditions fulfill:
+ * - abc line homogeneous params close
+ * - Overlapping points
+ */
+ //unsigned int mergeLines(std::list<LineSegment> & _line_list);
+
+ /** \brief Find lines. Pure virtual. To be implemented by each inherited class
+ *
+ * Find lines from a set of scans.
+ * Returns Lines as a std::list<LineSegment>
+ *
+ * \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 _line_list set of lines extracted from _points
+ * \return Number of lines extracted.
+ *
+ */
+ virtual unsigned int findLines( const Eigen::MatrixXd & _points,
+ std::list<LineSegment> & _line_list) const = 0;
+
+ /** \brief Print things
+ *
+ * Print things about this class
+ *
+ **/
+ virtual void print() const = 0;
+};
+#endif
\ No newline at end of file
diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp
new file mode 100644
index 0000000..e5e774d
--- /dev/null
+++ b/src/line_finder_hough.cpp
@@ -0,0 +1,137 @@
+#include "line_finder_hough.h"
+
+laserscanutils::LineFinderHough::LineFinderHough()
+{
+
+}
+
+laserscanutils::LineFinderHough::~LineFinderHough()
+{
+
+}
+
+void laserscanutils::LineFinderHough::setHoughParams(const LineFinderHoughHoughParams & _params)
+{
+ //copy provided params
+ this->hough_params_ = _params;
+
+ //resize hough grid accordingly
+ hough_grid_rows = (unsigned int)ceil(M_PI/hough_params_.theta_step_);//[0,PI]
+ hough_grid_rows_half = (unsigned int)ceil(0.5*M_PI/hough_params_.theta_step_);//middle row index
+ hough_grid_cols = (unsigned int)ceil(2*hough_params_.range_max_/hough_params_.range_step_);//[-rmax,+rmax]
+ hough_grid_cols_half = (unsigned int)ceil(hough_params_.range_max_/hough_params_.range_step_);//middle col index
+ hough_grid_.resize(hough_grid_rows);
+ for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
+ {
+ hough_grid_[ii].resize(hough_grid_cols);
+ }
+}
+
+unsigned int laserscanutils::LineFinderHough::findLines( const Eigen::MatrixXd & _points,
+ std::list<laserscanutils::LineSegment> & _line_list)
+{
+ double theta, range;
+ int kr;
+ Line line;
+ double xmax, xmin, ymax, ymin;
+ Eigen::Matrix3s pt; //auxiliary point
+ Eigen::MatrixXs pts; //auxiliar array of points
+ std::list<Eigen::Vector3s>::iterator pts_it;
+
+ //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
+ {
+ for (unsigned int jth = 0; jth < hough_grid_rows; jth++) //loop over all theta values in the grid
+ {
+ //compute Real values of theta and range
+ theta = jth*hough_params_.theta_step_;
+ range = _points(0,ipt)*cos(theta) + _points(1,ipt)*sin(theta); //r=xcos(th)+ysin(th)
+
+ //discretize range
+ kr = (int)floor(range/hough_params_.range_step_) + (int)hough_grid_cols_half ;
+
+ //check validity of the discretized values
+ if( ( kr >= 0 ) && ( kr < hough_grid_cols ) )
+ {
+ //Add support to cell(jth,kr), by pushing back the point homogeneous coordinates
+ hough_grid_.at(jth).at(kr).push_back( Eigen::Vector3s(_points(0,ipt),_points(1,ipt)),1 );
+ }
+ }
+ }
+
+ ////STEP 2 .Find cells having a peak of at least min_supports_ points supporting them
+ for (unsigned int ii = 1; ii < hough_grid_rows-1; ii++)
+ {
+ for (unsigned int jj = 1; jj < hough_grid_cols-1; jj++)
+ {
+ //check min supports
+ unsigned int cell_supports = hough_grid_.at(ii).at(jj).size();
+ if( cell_supports >= hough_params_.min_supports_ )
+ {
+ //check if cell ii,jj is a peak ( 8 neighboring cells should be below in number of supports)
+ if ( ( cell_supports >= hough_grid_.at(ii-1).at(jj-1).size() ) &&
+ ( cell_supports >= hough_grid_.at(ii-1).at(jj).size() ) &&
+ ( cell_supports >= hough_grid_.at(ii-1).at(jj+1).size() ) &&
+ ( cell_supports >= hough_grid_.at(ii).at(jj-1).size() ) &&
+ ( cell_supports >= hough_grid_.at(ii).at(jj+1).size() ) &&
+ ( cell_supports >= hough_grid_.at(ii+1).at(jj-1).size() ) &&
+ ( cell_supports >= hough_grid_.at(ii+1).at(jj).size() ) &&
+ ( cell_supports >= hough_grid_.at(ii+1).at(jj+1).size() ) )
+ {
+ //find best fitting line with the supporting points
+ pts.resize(3,cell_supports); //each point in a column
+ for(unsigned int kk = 0, pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++, kk++)
+ {
+ pts.block<3,1>(0,kk) << *pts_it; //copy point
+ }
+ this->fitLine(pts, line);
+
+ //set the line hough params TODO: Compute them from fitLine result, not from the Grid !!
+ line.np_ = hough_grid_.at(ii).at(jj).size(); //supporters
+ line.theta_ = ii*hough_params_.theta_step_; //theta
+ line.range_ = jj*hough_params_.range_step_; //range
+
+ //set starting and ending points of the line
+ //TODO: take into account normal convention. abc_ should point to the free space
+ xmax=-100; xmin=100; ymax=-100; ymin=100;
+ for (pt_it = hough_grid_.at(ii).at(jj).begin(); pt_it != hough_grid_.at(ii).at(jj).end(); pt_it++)
+ {
+ //find xmax, xmin, ymax, ymin
+ if (pt_it->first > xmax) xmax = pt_it->first;
+ if (pt_it->second > ymax) ymax = pt_it->second;
+ if (pt_it->first < xmin) xmin = pt_it->first;
+ if (pt_it->second < ymin) ymin = pt_it->second;
+ }
+ if (ii < hough_grid_rows_half) //first and third quartile of r-theta plane
+ {
+ //TODO
+ //pt << xmin,ymax,1;
+ //line.point_first_ << projection of pt to line.abc_
+ //pt << xmax,ymin,1;
+ //line.point_last_ << projection of pt to line.abc_
+ line.point_first_ << xmin,ymax,1;
+ line.point_last_ << xmax,ymin,1;
+
+ }
+ else //second and fourth quartile of r-theta plane
+ {
+ //TODO same as above
+ line.point_first_ << xmin,ymin,1;
+ line.point_last_ << xmax,ymax,1;
+ }
+
+ //push back the line to the list
+ _line_list.push_back(line);
+ }
+ }
+ }
+ }
+
+ //return the number of lines detected
+ return _line_list.size();
+}
+
+void laserscanutils::LineFinderHough::print() const
+{
+ //TODO
+}
diff --git a/src/line_finder_hough.h b/src/line_finder_hough.h
new file mode 100644
index 0000000..087810d
--- /dev/null
+++ b/src/line_finder_hough.h
@@ -0,0 +1,91 @@
+#ifndef LINE_FINDER_HOUGH_H_
+#define LINE_FINDER_HOUGH_H_
+
+//laserscanutils
+#include "line_finder.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 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
+ unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line
+ void print() const; //just a print method
+};
+
+
+/** \brief Class implementing methods to extract striaght segments from a scan
+*
+* Class implementing methods to extract striaght segments from a scan
+*
+*/
+class LineFinderHough : public LineFinder
+{
+ protected:
+
+ //Tunning params for Hough approach
+ LineFinderHoughParams hough_params_;
+
+ //Hough grid in theta-range space. Each cell holds a list of the point coordinates supporting the cell
+ std::vector<std::vector<std::list<Eigen::Vector3s > > > hough_grid_;
+
+ //dereived Hough params
+ unsigned int hough_grid_rows;
+ unsigned int hough_grid_rows_half;
+ unsigned int hough_grid_cols;
+ unsigned int hough_grid_cols_half;
+
+ public:
+ /** \brief Constructor
+ *
+ * Constructor
+ *
+ **/
+ LineFinder();
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ ~LineFinder();
+
+ /** \brief Set Hough tunning params
+ *
+ * Set Hough tunning params. Resizes Hough grid accordingly.
+ *
+ **/
+ void setHoughParams(const LineFinderHoughParams & _params);
+
+ /** \brief Find lines using Hough transform. Result as a list of Line's
+ *
+ * Find lines from a set of points.
+ * Returns Lines as a std::list<laserscanutils::StraightSegment>
+ *
+ * \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 _line_list set of lines extracted from _points
+ * \return Number of lines extracted.
+ *
+ *
+ */
+ unsigned int findLines( const Eigen::MatrixXd & _points,
+ std::list<laserscanutils::LineSegment> & _line_list);
+
+ /** \brief Print things
+ *
+ * Print things about this class
+ *
+ **/
+ void print() const;
+};
+#endif
\ No newline at end of file
diff --git a/src/line_finder_jump_fit.cpp b/src/line_finder_jump_fit.cpp
new file mode 100644
index 0000000..b8a6302
--- /dev/null
+++ b/src/line_finder_jump_fit.cpp
@@ -0,0 +1,32 @@
+#include "line_finder_hough.h"
+
+laserscanutils::LineFinderJumpFit::LineFinderJumpFit()
+{
+
+}
+
+laserscanutils::LineFinderJumpFit::~LineFinderJumpFit()
+{
+
+}
+
+void laserscanutils::LineFinderJumpFit::setJumpFitParams(const LineFinderJumpFitParams & _params)
+{
+ this->jump_fit_params_ = _params;
+}
+
+unsigned int laserscanutils::LineFinderJumpFit::findLines( const Eigen::MatrixXd & _points,
+ std::list<laserscanutils::LineSegment> & _line_list)
+{
+ //TODO
+ //Import code from line_detector.h -> laserscanutils::extractLines(),
+ //but remove things considering two main assumptions of this method:
+ // - ordering
+ // - points belong to the same segment (no jumps in _points)
+
+}
+
+void laserscanutils::LineFinderJumpFit::print() const
+{
+ //TODO
+}
diff --git a/src/line_finder_jump_fit.h b/src/line_finder_jump_fit.h
new file mode 100644
index 0000000..baab129
--- /dev/null
+++ b/src/line_finder_jump_fit.h
@@ -0,0 +1,87 @@
+#ifndef LINE_FINDER_JUMP_FIT_H_
+#define LINE_FINDER_JUMP_FIT_H_
+
+//laserscanutils
+#include "line_finder.h"
+
+namespace laserscanutils
+{
+/** \brief Set of tunning parameters for the Jump&Fit approach
+*
+* Set of tunning parameters for line extraction using the Jump&Fit approach
+*
+*/
+struct LineFinderJumpFitParams
+{
+ //members
+ ScalarT jump_dist_ut_; //Upper threshold in consecutive ranges to consider a jump
+ ScalarT jump_angle_ut_; //Upper threshold in angle of two consecutive ranges to consider a jump
+ ScalarT window_length_; // length (m) of the window of points to fit lines in the first pass
+ unsigned int min_window_points_; // minimum number of points to fit lines in the first pass
+ ScalarT k_sigmas_ut_;//Uppet threshold of how many std_dev are tolerated to count that a point is supporting a line
+ unsigned int concatenate_ii_ut_;//Upper threshold for ray index difference between consecutive lines to consider concatenation
+ ScalarT concatenate_angle_ut_; //Upper threshold for angle between consecutive lines to consider concatenation
+ void print() const; //just a print method
+};
+
+
+/** \brief Class implementing methods to extract straight segments from a scan, following a "jump & fit approach"
+*
+* Class implementing methods to extract straight segments from a scan, following a "jump & fit approach"
+*
+*/
+class LineFinderJumpFit : public LineFinder
+{
+ protected:
+ //Tunning params for Jump & Fit approach
+ LineFinderJumpFitParams jump_fit_params_;
+
+ public:
+ /** \brief Constructor
+ *
+ * Constructor
+ *
+ **/
+ LineFinderJumpFit();
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ ~LineFinderJumpFit();
+
+ /** \brief Set Jump and Fit tunning params
+ *
+ * Set Jump and Fit tunning params
+ *
+ **/
+ void setJumpFitParams(const LineFinderJumpFitParams & _params);
+
+ /** \brief Find lines using Jump & Fit. Result as a list of Line's
+ *
+ * Find lines from a set of ordered points.
+ * Returns Lines as a std::list<laserscanutils::StraightSegment>
+ *
+ * \Requires:
+ * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1).
+ * Ordering is required.
+ * Points should belong to a single scan segment
+ *
+ * \Provides:
+ * \param _line_list set of lines extracted from _points
+ * \return Number of lines extracted.
+ *
+ *
+ */
+ unsigned int findLines( const Eigen::MatrixXd & _points,
+ std::list<laserscanutils::LineSegment> & _line_list);
+
+ /** \brief Print things
+ *
+ * Print things about this class
+ *
+ **/
+ void print() const;
+};
+#endif
\ No newline at end of file
diff --git a/src/line_segment.cpp b/src/line_segment.cpp
new file mode 100644
index 0000000..d7d0025
--- /dev/null
+++ b/src/line_segment.cpp
@@ -0,0 +1,41 @@
+
+#include "line_segment.h"
+
+laserscanutils::LineSegment::LineSegment() :
+ abc_(0,0,0),
+ idx_first_(0),
+ idx_last_(0),
+ point_first_(0,0,0),
+ point_last_(0,0,0),
+ error(-1.),
+ np_(0),
+ range_(0.),
+ theta_(0.),
+ length(0.)
+{
+
+}
+
+laserscanutils::LineSegment::~LineSegment()
+{
+
+}
+
+void laserscanutils::LineSegment::merge(const LineSegment & _segment)
+{
+ //TODO
+}
+
+void laserscanutils::LineSegment:ScalarT print() const
+{
+ std::cout << "Line Parameters : " << std::endl
+ << "\t(a,b,c): " << abc_.transpose() << std::endl
+ << "\idx_first_,idx_last_: " << idx_first_ << " , " << idx_last_ << std::endl
+ << "\tfirst point: " << point_first_.transpose() << std::endl
+ << "\tlast point: " << point_last_.transpose() << std::endl
+ << "\terror: " << error_ << std::endl
+ << "\tnumber of points: " << np_ << std::endl
+ << "\trange: " << range_ << std::endl
+ << "\ttheta: " << theta_ << std::endl;
+ << "\tlength: " << length_ << std::endl;
+}
diff --git a/src/line_segment.h b/src/line_segment.h
new file mode 100644
index 0000000..6945bb7
--- /dev/null
+++ b/src/line_segment.h
@@ -0,0 +1,49 @@
+#ifndef LINE_SEGMENT_H_
+#define LINE_SEGMENT_H_
+
+//laserscanutils
+#include "laser_scan_utils.h"
+
+//open namespace
+namespace laserscanutils
+{
+/** \brief Line segment
+*
+* Line segment
+*
+*/
+class LineSegment
+{
+ public:
+ /** \brief Homogeneous parameterization of the line to which the segment lies on.
+ *
+ * Homogeneous parameterization of the line to which the segment lies on: (a,b,c)^T -> ax+by+c=0.
+ * Vector (a,b,c)^T is the normal vector, pointing to a free surface
+ *
+ */
+ Eigen::Vector3s abc_;
+ unsigned int idx_first_; //corresponding index to the scan ranges of the first point used
+ unsigned int idx_last_; //corresponding index to the scan ranges of the last point used
+ Eigen::Vector3s point_first_; //homogeneous coordinates of the starting 2D point
+ Eigen::Vector3s point_last_; //homogeneous coordinates of the ending 2D point
+ ScalarT fit_error_; //sum of all distances from used points to line when fitting
+ 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.
+ ScalarT length_; //theta component in polar parameterization.
+
+ public:
+ //constructor
+ LineSegment();
+
+ //Destructor
+ ~LineSegment();
+
+ //merges this LineSegment with the argument.
+ void merge(const LineSegment & _segment);
+
+ //print
+ void print() const;
+};
+}
+#endif
\ No newline at end of file
diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp
new file mode 100644
index 0000000..f26b482
--- /dev/null
+++ b/src/scan_segment.cpp
@@ -0,0 +1,22 @@
+#include "scan_segment.h"
+
+laserscanutils::ScanSegment::ScanSegment()
+{
+
+}
+
+aserscanutils::ScanSegment::~ScanSegment(
+{
+
+}
+
+void laserscanutils::ScanSegment::merge(const ScanSegment & _segment)
+{
+ //TODO
+}
+
+void laserscanutils::ScanSegment::print() const
+{
+ //TODO
+}
+
diff --git a/src/scan_segment.h b/src/scan_segment.h
new file mode 100644
index 0000000..83e43e8
--- /dev/null
+++ b/src/scan_segment.h
@@ -0,0 +1,42 @@
+#ifndef SCAN_SEGMENT_H_
+#define SCAN_SEGMENT_H_
+
+//laserscanutils
+#include "laser_scan_utils.h"
+
+//open namespace
+namespace laserscanutils
+{
+/** \brief Scan segment
+*
+* A segment is a set of connected scan points.
+* "Connected" means that neigbors are close enough (below a threshold).
+* "Neigbors" implies scan ordering is required
+*
+*/
+class ScanSegment
+{
+ public:
+ /** \brief Set of points in homogeneous coordinates
+ *
+ * Set of points in homogeneous coordinates.
+ * Each column is a point (x,y,1)^T
+ *
+ */
+ Eigen::MatrixXs points_;
+
+ public:
+ //constructor
+ ScanSegment();
+
+ //Destructor
+ ~ScanSegment();
+
+ //merges this ScanSegment with the argument.
+ void merge(const ScanSegment & _segment);
+
+ //print
+ void print() const;
+};
+}
+#endif
\ No newline at end of file
--
GitLab