From c19c19a9fe241f57c20efc3551699fbd2f77032c Mon Sep 17 00:00:00 2001
From: Sergi Pujol <sergi.pujol.badell@estudiantat.upc.edu>
Date: Wed, 17 Feb 2021 11:50:32 +0100
Subject: [PATCH] added gtest for findLoopClosure function
---
src/loop_closure_falko.cpp | 6 ++++--
src/loop_closure_falko.h | 31 +++++++++++++++++++++++------
test/gtest_loop_closure_base_2d.cpp | 10 ++++++++++
3 files changed, 39 insertions(+), 8 deletions(-)
diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp
index 824400a..c798fa2 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 95c534e..f92b6dd 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 d1777d5..29ab213 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");
}
--
GitLab