From 2992ef0fd2cf8786374853e0535717cd7dac25fb Mon Sep 17 00:00:00 2001
From: Sergi Pujol <sergi.pujol.badell@estudiantat.upc.edu>
Date: Tue, 16 Feb 2021 11:20:32 +0100
Subject: [PATCH] function matchScene finished and gtest added

---
 src/loop_closure_falko.cpp          |  4 ++--
 src/loop_closure_falko.h            | 15 ++++++++++++++-
 src/match_loop_closure.h            |  1 +
 test/gtest_loop_closure_base_2d.cpp | 12 ++++++------
 4 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp
index d0628a1..c0ab0fb 100644
--- a/src/loop_closure_falko.cpp
+++ b/src/loop_closure_falko.cpp
@@ -54,7 +54,7 @@ namespace laserscanutils{
         extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList);
         return newScene;
     }
-
+/*
     template <typename D,typename Extr, typename M>
     typename loopClosureFalko<D,Extr, M>::matchLoopClosurePtr matchScene(std::shared_ptr<sceneFalko<D>> scene1,std::shared_ptr<sceneFalko<D>> scene2){
         std::vector<std::pair<int, int> > assoNN;
@@ -72,7 +72,7 @@ namespace laserscanutils{
 
         return new_match;
     }
-
+*/
     //void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene newScene){}
 
     //Explicitly compile all the templates
diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index d8b21fa..bfbcb78 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -79,7 +79,20 @@ public:
 
     /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object
      **/
-    matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,std::shared_ptr<sceneFalko<D>> scene2);
+    matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,sceneFalkoBSCPtr scene2){
+        std::vector<std::pair<int, int> > assoNN;
+        int matching_number = matcher_.match(scene1->keypointsList,scene2->keypointsList, assoNN);
+        auto new_match=std::make_shared<matchLoopClosure<D>>();
+        new_match-> keypointsNumberMatch = matching_number;
+        int keypoints_number_th=5;
+        if (matching_number>keypoints_number_th){
+            new_match-> match = true;
+        } else {
+            new_match-> match = false;
+        }
+        new_match -> sceneTuple = std::make_tuple(scene1,scene2);
+        return new_match;
+    }
 
     
 };
diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h
index c4f0205..bf85aee 100644
--- a/src/match_loop_closure.h
+++ b/src/match_loop_closure.h
@@ -11,6 +11,7 @@
 #include <iostream>
 #include <fstream>
 #include <memory>
+#include <tuple>
 
 /**************************
  *      laser_scan_utils includes     *
diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp
index ed2fbdc..e2cab5c 100644
--- a/test/gtest_loop_closure_base_2d.cpp
+++ b/test/gtest_loop_closure_base_2d.cpp
@@ -23,24 +23,24 @@ TEST(loop_closure_falko, TestExtractSceneAndMatcher)
 
   loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko;
 
-  auto newScene=LCFalko.extractScene(scan, laserParams);
+  auto new_scene=LCFalko.extractScene(scan, laserParams);
 
-  int detectedKeypoints =((*newScene).keypointsList).size();
+  int detectedKeypoints =((*new_scene).keypointsList).size();
 
-  int detectedDescriptors =newScene->descriptorsList.size();
+  int detectedDescriptors =new_scene->descriptorsList.size();
 
   ASSERT_EQ(detectedKeypoints, 18);
 
   ASSERT_EQ(detectedDescriptors, 18);
-/*
+
   //Test matcher
 
-  auto new_match =LCFalko.matchScene(newScene,newScene);
+  auto new_match =LCFalko.matchScene(new_scene,new_scene);
 
   int keypoints_matched = new_match->keypointsNumberMatch;
 
   ASSERT_EQ(keypoints_matched, 18);
-*/
+
 //  PRINTF("All good at TestTest::DummyTestExample !\n");
 }
 
-- 
GitLab