diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 824400ad2533c7727ee1e227a1022b1d74447486..c798fa221225eeef39fa2734b7c685faf44488da 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -30,11 +30,11 @@ namespace laserscanutils{ matcher_.setDistanceThreshold(0.1); } - +/* // DESTRUCTOR template <typename D,typename Extr, typename M> loopClosureFalko<D,Extr,M>::~loopClosureFalko(){} - +*/ template <typename D,typename Extr, typename M> std::shared_ptr<falkolib::LaserScan> loopClosureFalko<D,Extr, M>::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ auto scanFALKO=std::make_shared<falkolib::LaserScan>(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); @@ -76,6 +76,8 @@ namespace laserscanutils{ */ //void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene newScene){} + + //Explicitly compile all the templates template class loopClosureFalko <bsc, bscExtractor, NNMatcher>; } diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 95c534e573a2678cfb3acfd0261e96a22e5a950a..f92b6dd18ab7e7d3b490024efc1a73b94a0c3a95 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -11,6 +11,8 @@ #include <iostream> #include <fstream> #include <memory> +#include <iterator> +#include <list> /************************** * laser_scan_utils includes * @@ -63,11 +65,7 @@ public: /** \brief Destructor **/ - ~loopClosureFalko(); - - /** \brief compare new scans against the trained set in order to find loop closures - **/ - //void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene newScene){} + ~loopClosureFalko(){} /** \brief Create and update the scene struct with keypoints and descriptors **/ @@ -101,7 +99,28 @@ public: new_match -> sceneTuple = std::make_tuple(scene1,scene2); return new_match; } - + + /** \brief compare new scans against the trained set in order to find loop closures + **/ + std::list<matchLoopClosurePtr> findLoopClosure(std::list<sceneFalkoBSCPtr> &l_scenes, const sceneFalkoBSCPtr &new_scene){ + int number_ref_sc = l_scenes.size(); + + std::list<matchLoopClosurePtr> matchings; + + for (int i=0; i<number_ref_sc; i++ ){ + + auto l_front = l_scenes.begin(); + + std::advance(l_front, i); + //std::cout << "list_scenes[i]" << *l_front << std::endl; + + //matchScene() + + } + + return matchings; + + } }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index d1777d541680f56718273e9b221232ab9cae6b08..29ab213bb197fbe7c7850450c81c78ab373ee8fb 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -41,6 +41,16 @@ TEST(loop_closure_falko, TestExtractSceneAndMatcher) ASSERT_EQ(keypoints_matched, 18); + //TEST findLoopClosure + + std::list<std::shared_ptr<sceneFalko<bsc>>> ref_scenes; + + ref_scenes.push_back(new_scene); + + ref_scenes.push_back(new_scene); + + auto matchings = LCFalko.findLoopClosure(ref_scenes,new_scene); + // PRINTF("All good at TestTest::DummyTestExample !\n"); }