improved function cornerFalko2d->findLoopClosure

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;
+            }
+       }
+ */    }
 
 }
 

src/corner_falko_2d.h

@@ -82,6 +82,8 @@ public:
     int scanNumber=0;
 
     int scansExtracted=0;
+
+    int matchingPosition=-1;
 };
 
 } /* namespace laserscanutils */