From f8afb2842c40ed4b13ac2de1c2a3af9b32c26a6f Mon Sep 17 00:00:00 2001 From: jvallve <jvallve@224674b8-e365-4e73-a4a8-558dbbfec58c> Date: Thu, 22 Oct 2015 13:59:21 +0000 Subject: [PATCH] --- src/corner_detector.cpp | 6 ++++-- src/scan_basics.cpp | 5 ++++- src/scan_basics.h | 6 ++++++ 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/corner_detector.cpp b/src/corner_detector.cpp index 3267ffd..6fdddda 100644 --- a/src/corner_detector.cpp +++ b/src/corner_detector.cpp @@ -90,8 +90,10 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _ corner_it2 ++; while (corner_it2 != _corner_list.end()) { - // Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) ! - if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq ) + // Check if two corners are close enough with similar orientation and aperture + if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq && + abs( pi2pi( corner_it1->aperture_ - corner_it2->aperture_) ) < _alg_params.theta_min_ && + abs( pi2pi( corner_it1->orientation_ - corner_it2->orientation_) ) < _alg_params.theta_min_) { // Keep the one with bigger product of number of points of each line (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2); diff --git a/src/scan_basics.cpp b/src/scan_basics.cpp index 0e1be80..aed6946 100644 --- a/src/scan_basics.cpp +++ b/src/scan_basics.cpp @@ -92,4 +92,7 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & //_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ? } - +laserscanutils::ScalarT laserscanutils::pi2pi(const ScalarT& angle) +{ + return (angle > 0 ? fmod(angle + M_PI, 2 * M_PI) - M_PI : fmod(angle - M_PI, 2 * M_PI) + M_PI); +} diff --git a/src/scan_basics.h b/src/scan_basics.h index 37c765a..42f8b95 100644 --- a/src/scan_basics.h +++ b/src/scan_basics.h @@ -60,5 +60,11 @@ namespace laserscanutils * **/ void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, const ScalarT& _jump_angle_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps); + + /** \brief Fits angle in (-pi, pi] + * + * Fits angle in (-pi, pi] + **/ + ScalarT pi2pi(const ScalarT& angle); } #endif -- GitLab