diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp
index 661e18f080421327a7841032d2d800592a15d383..09bf36f9d91f9dfaabf8b8369bd99bfade0b71f7 100644
--- a/src/corner_falko_2d.cpp
+++ b/src/corner_falko_2d.cpp
@@ -61,13 +61,28 @@ void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){
     compute(scan, keypointSet2, descriptorSet2);
 
     //Matching
-    int rows =  sizeof(keypointSets)/sizeof(keypointSets[0]);
-    int cols = sizeof(keypointSets)/(sizeof(int)*rows);
+    int rows = keypointSets.size();
 
+    matchingNumber=0;
+    matchingPosition=-1;
     for (int i=0; i<rows;i++){
         std::vector<std::pair<int, int> > assoNN;
-        matchingNumber = match(keypointSets[i], keypointSet2, assoNN);
-    }
+
+        int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN);
+
+        if (NewMatchingNumber > matchingNumber ) {
+            matchingNumber=NewMatchingNumber;
+            matchingPosition=i;
+        }
+
+/*        for (auto& match : assoNN) {
+            if (match.second >= 0) {
+                int i1 = match.first;
+                int i2 = match.second;
+                std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl;
+            }
+       }
+ */    }
 
 }
 
diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h
index 5ce667ebd98eeb23ae84b9481a6f753625356153..8bc7639acb16c499cae6db3e463f6172f0c0846a 100644
--- a/src/corner_falko_2d.h
+++ b/src/corner_falko_2d.h
@@ -82,6 +82,8 @@ public:
     int scanNumber=0;
 
     int scansExtracted=0;
+
+    int matchingPosition=-1;
 };
 
 } /* namespace laserscanutils */