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

Simple version of corner detection. Poor results with real data

parent e9850926
No related branches found
No related tags found
No related merge requests found
Pipeline #
...@@ -17,9 +17,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan, ...@@ -17,9 +17,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
std::list<laserscanutils::CornerPoint> & _corner_list) std::list<laserscanutils::CornerPoint> & _corner_list)
{ {
//constants TODO: should be moved as a set algorithm params //constants TODO: should be moved as a set algorithm params
unsigned int HALF_WINDOW_SIZE = 5; //minimum size of line supproting corner unsigned int HALF_WINDOW_SIZE = 10; //minimum size of line supproting corner
ScalarT LINE_FIT_ERROR = 0.05; //maximum allowed mean point-line error to consider a line is straight enough ScalarT LINE_FIT_ERROR = 0.03; //maximum allowed mean point-line error to consider a line is straight enough
ScalarT MIN_ANGLE = 30*M_PI/180.; //minimum allowed absoulte angle between lines around the corner ScalarT MIN_ANGLE = 45.*M_PI/180.; //minimum allowed absoulte angle between lines around the corner
//variables //variables
ScalarT e1,e2, angle; ScalarT e1,e2, angle;
...@@ -44,16 +44,19 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan, ...@@ -44,16 +44,19 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
//check line error //check line error
// std::cout << "findCorners(): " << __LINE__ << std::endl; // std::cout << "findCorners(): " << __LINE__ << std::endl;
e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1); e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2. + 1.);
e2 = line2.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1); e2 = line2.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2. + 1.);
//std::cout << "findCorners(). e1: " << e1 << "; e2: " << e2 << std::endl;
if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) ) if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
{ {
//check angles between lines //check angles between lines
angle = line1.angleToLine(line2); //angle is in [0,2pi] angle = line1.angleToLine(line2); //angle is in [0,2pi]
if ( ( angle < M_PI-MIN_ANGLE ) || ( angle > M_PI+MIN_ANGLE ) ) if ( ( angle > MIN_ANGLE ) && ( angle < 2*M_PI-MIN_ANGLE ) )
{ {
//corner found !! //corner found !!
std::cout << "findCorners(): " << __LINE__ << std::endl; // std::cout << "findCorners(): " << __LINE__ << std::endl;
//std::cout << "findCorners(). angle " << angle << std::endl;
new_corner.point_ << _scan.points_all_.block<3,1>(0,ii);//TODO crossing point between lines new_corner.point_ << _scan.points_all_.block<3,1>(0,ii);//TODO crossing point between lines
new_corner.orientation_ = 0;//TODO new_corner.orientation_ = 0;//TODO
new_corner.aperture_ = angle; new_corner.aperture_ = angle;
...@@ -62,6 +65,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan, ...@@ -62,6 +65,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
} }
} }
} }
//return number of corners
return _corner_list.size();
} }
void CornerFinderRangeDiff::print() const void CornerFinderRangeDiff::print() const
......
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