evaluateNewRefScene function modified

src/corner_falko_2d.cpp

@@ -91,14 +91,18 @@ int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scan
 
     int NewSceneAdded=0;
 
-    if (scanNumber % scanInterval == 0){
+    if (scanNumber % scanInterval == 0 || scanNumber == 1){
 
-        falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams);
+        findLoopClosure(scan, scanParams);
 
-        AddNewReferenceScene(scanFALKO);
+        if (matchingNumber < refSceneAddingTh) {
 
-        NewSceneAdded=1;
+            falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams);
 
+            AddNewReferenceScene(scanFALKO);
+
+            NewSceneAdded=1;
+        }
     }
 
     return NewSceneAdded;
@@ -106,5 +110,4 @@ int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scan
 }
 
 
-
 } // laserscanutils namespace

src/corner_falko_2d.h

@@ -108,6 +108,9 @@ public:
     int matchingPosition=-1;
 
     int keypointsNumberTh=2;
+
+    // Max number of matched keypoints between 2 scenes for the candidate scene be considered a good New reference scene
+    int refSceneAddingTh =6;
 };
 
 } /* namespace laserscanutils */

src/examples/corner_falko_demo.cpp

@@ -35,10 +35,14 @@ int main(int argc, char** argv)
 
     CornerFalko2d cornerMatching;
 
-    cornerMatching.evaluateNewReferenceScene(scan, laserParams, scanInterval);
+    int sceneAdded = cornerMatching.evaluateNewReferenceScene(scan, laserParams, scanInterval);
 
     std::cout << "num keypoints1 extracted: " << cornerMatching.lastKeypointSet.size() << std::endl;
 
+    if (sceneAdded==1){
+        std::cout << "NewRefSceneAdded!!! " << std::endl;
+    }
+
     cornerMatching.findLoopClosure(scan, laserParams);
 
     std::cout << "matching number : " << cornerMatching.matchingNumber << std::endl;