From d07a89430f75a806a953066591dd0fe7d0071e28 Mon Sep 17 00:00:00 2001
From: andreucm <acoromin@iri.upc.edu>
Date: Fri, 1 Apr 2016 19:00:19 +0200
Subject: [PATCH] Simple version of corner detection. Poor results with real
data
---
src/corner_finder_range_diff.cpp | 20 +++++++++++++-------
1 file changed, 13 insertions(+), 7 deletions(-)
diff --git a/src/corner_finder_range_diff.cpp b/src/corner_finder_range_diff.cpp
index 62f5632..821c0a7 100644
--- a/src/corner_finder_range_diff.cpp
+++ b/src/corner_finder_range_diff.cpp
@@ -17,9 +17,9 @@ int CornerFinderRangeDiff::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 = 5; //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 MIN_ANGLE = 30*M_PI/180.; //minimum allowed absoulte angle between lines around the corner
+ unsigned int HALF_WINDOW_SIZE = 10; //minimum size of line supproting corner
+ 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
//variables
ScalarT e1,e2, angle;
@@ -44,16 +44,19 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
//check line error
// std::cout << "findCorners(): " << __LINE__ << std::endl;
- e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
- e2 = line2.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.);
+ //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 < M_PI-MIN_ANGLE ) || ( angle > M_PI+MIN_ANGLE ) )
+ if ( ( angle > MIN_ANGLE ) && ( angle < 2*M_PI-MIN_ANGLE ) )
{
//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.orientation_ = 0;//TODO
new_corner.aperture_ = angle;
@@ -62,6 +65,9 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
}
}
}
+
+ //return number of corners
+ return _corner_list.size();
}
void CornerFinderRangeDiff::print() const
--
GitLab