Added CornerFalko2d::findLoopClosure function

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

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_ */

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);
+
 }