Skip to content
Snippets Groups Projects
Commit f89de890 authored by Andreu Corominas-Murtra's avatar Andreu Corominas-Murtra
Browse files

Working Copy. corner finder development. Builds but crashes on run time with real data

parent d07a8943
No related branches found
No related tags found
No related merge requests found
Pipeline #
......@@ -26,7 +26,7 @@ SET(HDRS_BASE
SET(HDRS
corner_detector.h
corner_finder.h
corner_finder_range_diff.h
corner_finder_inscribed_angle.h
corner_point.h
entities.h
grid_2d.h
......@@ -48,7 +48,7 @@ SET(SRCS
corner_point.cpp
corner_detector.cpp
corner_finder.cpp
corner_finder_range_diff.cpp
corner_finder_inscribed_angle.cpp
corner_point.cpp
entities.cpp
grid_2d.cpp
......
......@@ -3,7 +3,7 @@
//laserscanutils
#include "laser_scan_utils.h"
#include "laser_scan.h"
//#include "laser_scan.h"
#include "corner_point.h"
//std
......@@ -48,10 +48,10 @@ class CornerFinder
* \return Number of corners extracted, or -1 if some error
*
*/
// virtual unsigned int findCorners( const Eigen::MatrixXs & _points,
virtual int findCorners( const Eigen::MatrixXs & _points,
std::list<laserscanutils::CornerPoint> & _corner_list) const = 0;
// virtual int findCorners(const laserscanutils::LaserScan & _scan,
// std::list<laserscanutils::CornerPoint> & _corner_list) = 0;
virtual int findCorners(const laserscanutils::LaserScan & _scan,
std::list<laserscanutils::CornerPoint> & _corner_list) = 0;
/** \brief Print things
*
......
#include "corner_finder_range_diff.h"
#include "corner_finder_inscribed_angle.h"
namespace laserscanutils
{
CornerFinderRangeDiff::CornerFinderRangeDiff() : CornerFinder()
CornerFinderInscribedAngle::CornerFinderInscribedAngle() : CornerFinder()
{
}
CornerFinderRangeDiff::~CornerFinderRangeDiff()
CornerFinderInscribedAngle::~CornerFinderInscribedAngle()
{
}
int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
std::list<laserscanutils::CornerPoint> & _corner_list)
int CornerFinderInscribedAngle::findCorners( const Eigen::MatrixXs & _points, std::list<laserscanutils::CornerPoint> & _corner_list) const
{
//params
unsigned int MIN_SIDE_POINTS = 5;
ScalarT LINE_FIT_ERROR = 0.03; //maximum allowed mean point-line error to consider a line is straight enough
ScalarT MIN_ANGLE = 45.*M_PI/180.; //minimum allowed absoulte angle between lines around the corner
//local variables
Eigen::VectorXs inscribed_angles;
Eigen::Vector3s line2o; //line from point i to _point(0)
Eigen::Vector3s line2e; //line from point i to _point(last)
ScalarT aux, min_angle, angle, e1, e2;
unsigned int ii_min;
LineSegment line1, line2;
CornerPoint new_corner;
//TODO: first check: if segment is not enough populated -> return -1
//TODO: second check: if segment has high eigen value ratio -> return -1
//resize inscribed_angles
inscribed_angles.resize(_points.cols()-2,1); //column vector
//compute inscribed angles for each point (excepting extremes)
for (unsigned int ipt = 1; ipt < _points.cols()-1; ipt++)
{
//find lines passing through point i and extremes of the set
line2o = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,0));
line2e = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,_points.cols()));
//normalize lines (http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html)
aux = sqrt(line2o(0)*line2o(0)+line2o(1)*line2o(1));
line2o << line2o(0)/aux,line2o(1)/aux,line2o(2)/aux;
aux = sqrt(line2e(0)*line2e(0)+line2e(1)*line2e(1));
line2e << line2e(0)/aux,line2e(1)/aux,line2e(2)/aux;
//compute the angle (http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html)
inscribed_angles(ipt-1) = acos( line2o(0)*line2e(0) + line2e(1)*line2e(1) ); //TODO: Review this angle computation
}
//find the minimum inscribed angle, and its index in the vector
min_angle = inscribed_angles.minCoeff(&ii_min);
//check enough points in left and right
if ( ( ii_min > MIN_SIDE_POINTS ) && ( ii_min < (inscribed_angles.rows() - MIN_SIDE_POINTS) ) )
{
//fit lines at left and right of the point
line_finder_.fitLine(_points.block(0,ii_min-MIN_SIDE_POINTS,3,MIN_SIDE_POINTS+1), line1);
line_finder_.fitLine(_points.block(0,ii_min,3,MIN_SIDE_POINTS+1), line2);
//check line error
// std::cout << "findCorners(): " << __LINE__ << std::endl;
e1 = line1.fit_error_ / ( (ScalarT)MIN_SIDE_POINTS * 2. + 1.);
e2 = line2.fit_error_ / ( (ScalarT)MIN_SIDE_POINTS * 2. + 1.);
// std::cout << "findCorners(). e1: " << e1 << "; e2: " << e2 << std::endl;
if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
{
//check angles between lines
angle = line1.angleToLine(line2); //angle is in [0,2pi]
if ( ( angle > MIN_ANGLE ) && ( angle < 2*M_PI-MIN_ANGLE ) )
{
//corner found !!
// std::cout << "findCorners(): " << __LINE__ << std::endl;
// std::cout << "findCorners(). angle " << angle << std::endl;
new_corner.point_ << _points.block<3,1>(0,ii_min+1);//TODO crossing point between lines
new_corner.orientation_ = 0;//TODO
new_corner.aperture_ = angle;
_corner_list.push_back(new_corner);
}
}
}
}
/*
int CornerFinderInscribedAngle::findCorners(const laserscanutils::LaserScan & _scan, std::list<laserscanutils::CornerPoint> & _corner_list)
{
//constants TODO: should be moved as a set algorithm params
unsigned int HALF_WINDOW_SIZE = 10; //minimum size of line supproting corner
......@@ -69,8 +143,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
//return number of corners
return _corner_list.size();
}
*/
void CornerFinderRangeDiff::print() const
void CornerFinderInscribedAngle::print() const
{
}
......
#ifndef CORNER_FINDER_RANGE_DIFF_H_
#define CORNER_FINDER_RANGE_DIFF_H_
#ifndef CORNER_FINDER_INSCRIBED_ANGLE_H_
#define CORNER_FINDER_INSCRIBED_ANGLE_H_
//laserscanutils
#include "corner_finder.h"
#include "line_finder.h"
//std
#include <algorithm> //std::min, max
namespace laserscanutils
{
......@@ -13,13 +16,14 @@ namespace laserscanutils
* set of tunning parameters for the Hough transform line detection
*
*/
struct CornerFinderRangeDiffParams
struct CornerFinderInscribedAngleParams
{
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
//TODO !!
// 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
//void print() const; //just a print method
};
/** \brief Base Class for methods to extract corners from a scan
......@@ -27,10 +31,10 @@ struct CornerFinderRangeDiffParams
* Base Class for methods to extract corners from a scan
*
*/
class CornerFinderRangeDiff : public CornerFinder
class CornerFinderInscribedAngle : public CornerFinder
{
protected:
CornerFinderRangeDiffParams params_;
CornerFinderInscribedAngleParams params_;
LineFinder line_finder_; //required just fitLine method
public:
......@@ -39,21 +43,21 @@ class CornerFinderRangeDiff : public CornerFinder
* Constructor
*
**/
CornerFinderRangeDiff();
CornerFinderInscribedAngle();
/** \brief Destructor
*
* Destructor
*
**/
~CornerFinderRangeDiff();
~CornerFinderInscribedAngle();
/** \brief Set tunning params
*
* Set tunning params.
*
**/
void setParams(const CornerFinderRangeDiffParams & _params);
void setParams(const CornerFinderInscribedAngleParams & _params);
/** \brief Find corners.
*
......@@ -61,15 +65,15 @@ class CornerFinderRangeDiff : public CornerFinder
* Returns corners as a std::list<CornerPoint>
*
* \Requires:
* \param _scan: A LaserScan object, already raw processed. If not raw processed, this method returns -1
* \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 _scan
* \return Number of corners extracted.
*
*/
int findCorners(const laserscanutils::LaserScan & _scan,
std::list<laserscanutils::CornerPoint> & _corner_list);
int findCorners( const Eigen::MatrixXs & _points,
std::list<laserscanutils::CornerPoint> & _corner_list) const;
/** \brief Print things
*
......
......@@ -17,7 +17,7 @@ struct LineFinderHoughParams
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
//void print() const; //just a print method
};
......
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