diff --git a/src/corner_detector.cpp b/src/corner_detector.cpp
index 3267ffdb5a1681813640f2fad7e4362dcb1e99d4..6fddddac37819478c167a6f345918e151ed6acf8 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 0e1be80c39b951111e5bfd0d72b778b1c9ee9af3..aed6946fc32cc5c9a9cf59f396a58ddc481155fb 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 37c765a761ce8928daf365654d33c1616bf71aac..42f8b955df671354c6d0967b1fbdf87fbb0d0194 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