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