Skip to content
Snippets Groups Projects
Commit 6eef18fb authored by andreucm's avatar andreucm
Browse files

Set of intial classes deployed. Still a lot of methods TO DO.

parent d6684c06
No related branches found
No related tags found
No related merge requests found
#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
// }
#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
#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
}
#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
#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
}
#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
#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;
}
#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
#include "scan_segment.h"
laserscanutils::ScanSegment::ScanSegment()
{
}
aserscanutils::ScanSegment::~ScanSegment(
{
}
void laserscanutils::ScanSegment::merge(const ScanSegment & _segment)
{
//TODO
}
void laserscanutils::ScanSegment::print() const
{
//TODO
}
#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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment