From 65e79c90ffdfb657704f7d2176b34e5a197cc0f8 Mon Sep 17 00:00:00 2001
From: Sergi Pujol <sergi.pujol.badell@estudiantat.upc.edu>
Date: Thu, 28 Jan 2021 20:44:29 +0100
Subject: [PATCH] Added CornerFalko2d::findLoopClosure function
---
src/corner_falko_2d.cpp | 42 +++++++++++++++++++++++++-----
src/corner_falko_2d.h | 16 ++++--------
src/examples/corner_falko_demo.cpp | 5 +++-
3 files changed, 44 insertions(+), 19 deletions(-)
diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp
index 4b298a5..8f1e6b7 100644
--- a/src/corner_falko_2d.cpp
+++ b/src/corner_falko_2d.cpp
@@ -26,18 +26,46 @@ CornerFalko2d::~CornerFalko2d()
}
-void CornerFalko2d::train (falkolib::LaserScan scan){
+void CornerFalko2d::storeCorners (falkolib::LaserScan scan, int scanInterval){
- // Extract keypoints
- lastKeypointSet.clear();
- extract(scan, lastKeypointSet);
+ scanNumber=scanNumber+1;
+
+ if (scanNumber % scanInterval == 0){
+ // Extract keypoints
+ lastKeypointSet.clear();
+ extract(scan, lastKeypointSet);
+
+ //Compute descriptors
+ lastDescriptorSet.clear();
+ compute(scan, lastKeypointSet, lastDescriptorSet);
+
+ keypointSets.push_back(lastKeypointSet);
+ descriptorSets.push_back(lastDescriptorSet);
+
+ }
+
+}
+
+void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){
+ //Compute descriptors
+ std::vector<falkolib::FALKO> keypointSet2;
+ extract(scan, keypointSet2);
//Compute descriptors
- lastDescriptorSet.clear();
- compute(scan, lastKeypointSet, lastDescriptorSet);
+ std::vector <falkolib::BSC> descriptorSet2;
+ compute(scan, keypointSet2, descriptorSet2);
+
+ //Matching
+
+ for (int i=0; i<keypointSets.size();i++){
+ std::vector<std::pair<int, int> > assoNN;
+ int matchingNumber = match(keypointSets[i], keypointSet2, assoNN);
+ }
+
+
}
-} // laserscanutils namespace
+} // laserscanutils namespace
diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h
index cb1b675..079db34 100644
--- a/src/corner_falko_2d.h
+++ b/src/corner_falko_2d.h
@@ -41,7 +41,7 @@ namespace laserscanutils
*
*/
-class CornerFalko2d: public falkolib::FALKOExtractor, falkolib::BSCExtractor<falkolib::FALKO>
+class CornerFalko2d: public falkolib::FALKOExtractor, falkolib::BSCExtractor<falkolib::FALKO>, falkolib::NNMatcher<falkolib::FALKO>
{
public:
/**
@@ -62,7 +62,7 @@ public:
/** \brief Gets a set of landmarks/scenes to use as trained set.
**/
- void train(falkolib::LaserScan scan);
+ void storeCorners(falkolib::LaserScan scan, int scanInterval);
/** \brief Extract landmark/scene (list of corners) from a given 2D scan
**/
@@ -70,22 +70,16 @@ public:
/** \brief compare new scans against the training set in order to find loop closures
**/
- void findLoopClosure();
+ void findLoopClosure(falkolib::LaserScan scan);
std::vector<std::vector<falkolib::FALKO>> keypointSets;
std::vector<falkolib::FALKO> lastKeypointSet;
std::vector<std::vector<falkolib::BSC>> descriptorSets;
std::vector<falkolib::BSC>lastDescriptorSet;
- //int _circularSectorNumber;
- //int _radialRingNumber;
- // Keypoints extractor
- //falkolib::FALKOExtractor fe;
- // BSC descriptor extractor
- //falkolib::BSCExtractor<falkolib::FALKO> bsc(_circularSectorNumber,_radialRingNumber);
-
+ int scanNumber=0;
};
-} /* namespace wolf */
+} /* namespace laserscanutils */
#endif /* LANDMARK_POLYLINE_2d_H_ */
diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp
index acfdfe4..cda359c 100644
--- a/src/examples/corner_falko_demo.cpp
+++ b/src/examples/corner_falko_demo.cpp
@@ -21,6 +21,7 @@ int main(int argc, char** argv)
{
int scanSize = 1440;
+ int scanInterval =1;
falkolib::LaserScan scan1(0, 2.0 * M_PI, scanSize);
@@ -28,8 +29,10 @@ int main(int argc, char** argv)
CornerFalko2d CornerMatching;
- CornerMatching.extract(scan1, CornerMatching.lastKeypointSet);
+ CornerMatching.storeCorners(scan1, scanInterval);
std::cout << "num keypoints1 extracted: " << CornerMatching.lastKeypointSet.size() << std::endl;
+ CornerMatching.findLoopClosure(scan1);
+
}
--
GitLab