diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index d0628a1c46fff951a75a54ed03830c5f90a950ee..c0ab0fbc41367d5d41a57f7b1b88a123cf4e14d9 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 d8b21fa99566262561901efd241aa4c68125507e..bfbcb78c44de8943328b5664298e6af4d95985bb 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 c4f020549ccca7cbe2cdbfd7cfaa7a574a663a6c..bf85aeee59dfe3adc5a8aa35cee9cee0450573c1 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 ed2fbdcafc959e7c956134719c2a78d5b053a024..e2cab5c6f65ed3485036714d1a82b87592a809e9 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"); }