From f4f75253f1bf813e09cef749d2c817d6d17f0db1 Mon Sep 17 00:00:00 2001 From: Sergi Pujol <sergi.pujol.badell@estudiantat.upc.edu> Date: Mon, 22 Feb 2021 07:22:38 +0100 Subject: [PATCH] format modification with clang file --- src/corner_falko_2d.cpp | 153 +++++++++++++++--------------- src/corner_falko_2d.h | 140 ++++++++++++++------------- src/loop_closure_base.cpp | 17 ++-- src/loop_closure_base.h | 46 ++++----- src/loop_closure_falko.cpp | 85 +++++++++-------- src/loop_closure_falko.h | 145 ++++++++++++++-------------- src/match_loop_closure.h | 25 +++-- src/scene_base.h | 13 +-- src/scene_falko.h | 22 ++--- test/gtest_loop_closure_falko.cpp | 70 +++++++------- 10 files changed, 361 insertions(+), 355 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 6f105e8..edce4b9 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -7,114 +7,115 @@ #include "corner_falko_2d.h" -namespace laserscanutils{ - -CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, bool _useKeypointRadius, double _radius) :BSCExtractor( _circularSectorNumber, _radialRingNumber, _useKeypointRadius ,_radius ) { - // FALKO Extractor Parameters - setMinExtractionRange(0.1); - setMaxExtractionRange(25); - enableSubbeam(true); - setNMSRadius(0.1); - setNeighB(0.01); - setBRatio(4); - setGridSectors(16); - - // Matcher Extractor Parameters - setDistanceThreshold(0.1); - -} - -CornerFalko2d::~CornerFalko2d() -{ - +namespace laserscanutils { + +CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, + bool _useKeypointRadius, double _radius) + : BSCExtractor(_circularSectorNumber, _radialRingNumber, _useKeypointRadius, + _radius) { + // FALKO Extractor Parameters + setMinExtractionRange(0.1); + setMaxExtractionRange(25); + enableSubbeam(true); + setNMSRadius(0.1); + setNeighB(0.01); + setBRatio(4); + setGridSectors(16); + + // Matcher Extractor Parameters + setDistanceThreshold(0.1); } -void CornerFalko2d::AddNewReferenceScene (falkolib::LaserScan scanFALKO){ +CornerFalko2d::~CornerFalko2d() {} - // Extract keypoints - lastKeypointSet.clear(); - extract(scanFALKO, lastKeypointSet); +void CornerFalko2d::AddNewReferenceScene(falkolib::LaserScan scanFALKO) { - //Compute descriptors - lastDescriptorSet.clear(); - compute(scanFALKO, lastKeypointSet, lastDescriptorSet); + // Extract keypoints + lastKeypointSet.clear(); + extract(scanFALKO, lastKeypointSet); - keypointSets.push_back(lastKeypointSet); - descriptorSets.push_back(lastDescriptorSet); + // Compute descriptors + lastDescriptorSet.clear(); + compute(scanFALKO, lastKeypointSet, lastDescriptorSet); - scansExtracted=scansExtracted+1; + keypointSets.push_back(lastKeypointSet); + descriptorSets.push_back(lastDescriptorSet); + scansExtracted = scansExtracted + 1; } -void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ - - falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); +void CornerFalko2d::findLoopClosure(LaserScan scan, + LaserScanParams scanParams) { - //Compute descriptors - std::vector<falkolib::FALKO> keypointSet2; - extract(scanFALKO, keypointSet2); + falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams); - //Compute descriptors - std::vector <falkolib::BSC> descriptorSet2; - compute(scanFALKO, keypointSet2, descriptorSet2); + // Compute descriptors + std::vector<falkolib::FALKO> keypointSet2; + extract(scanFALKO, keypointSet2); - //Matching - int rows = keypointSets.size(); + // Compute descriptors + std::vector<falkolib::BSC> descriptorSet2; + compute(scanFALKO, keypointSet2, descriptorSet2); - numberKeypointsMatch=0; - numberSceneMatch=0; - matchingPosition=-1; - for (int i=0; i<rows;i++){ - std::vector<std::pair<int, int> > assoNN; + // Matching + int rows = keypointSets.size(); - int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN); + numberKeypointsMatch = 0; + numberSceneMatch = 0; + matchingPosition = -1; + for (int i = 0; i < rows; i++) { + std::vector<std::pair<int, int>> assoNN; - if (NewMatchingNumber > numberKeypointsMatch && NewMatchingNumber>keypointsNumberTh) { - numberKeypointsMatch=NewMatchingNumber; - matchingPosition=i; - numberSceneMatch=numberSceneMatch+1; + int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN); - } + if (NewMatchingNumber > numberKeypointsMatch && + NewMatchingNumber > keypointsNumberTh) { + numberKeypointsMatch = NewMatchingNumber; + matchingPosition = i; + numberSceneMatch = numberSceneMatch + 1; } - if (numberSceneMatch>1){ - numberSceneMatch=0; - matchingPosition=-1; - } - + } + if (numberSceneMatch > 1) { + numberSceneMatch = 0; + matchingPosition = -1; + } } -falkolib::LaserScan CornerFalko2d::convert2LaserScanFALKO(LaserScan scan,LaserScanParams scanParams){ - - falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - std::vector<double> doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); - scanFALKO.fromRanges(doubleRanges); - return scanFALKO; +falkolib::LaserScan +CornerFalko2d::convert2LaserScanFALKO(LaserScan scan, + LaserScanParams scanParams) { + falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, + scan.ranges_raw_.size()); + std::vector<double> doubleRanges(scan.ranges_raw_.begin(), + scan.ranges_raw_.end()); + scanFALKO.fromRanges(doubleRanges); + return scanFALKO; } -int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scanParams, int scanInterval){ +int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan, + LaserScanParams scanParams, + int scanInterval) { - scanNumber=scanNumber+1; + scanNumber = scanNumber + 1; - int NewSceneAdded=0; + int NewSceneAdded = 0; - if (scanNumber % scanInterval == 0 || scanNumber == 1){ + if (scanNumber % scanInterval == 0 || scanNumber == 1) { - findLoopClosure(scan, scanParams); + findLoopClosure(scan, scanParams); - if (numberKeypointsMatch < refSceneAddingTh) { + if (numberKeypointsMatch < refSceneAddingTh) { - falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); + falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams); - AddNewReferenceScene(scanFALKO); + AddNewReferenceScene(scanFALKO); - NewSceneAdded=1; - } + NewSceneAdded = 1; } + } - return NewSceneAdded; - + return NewSceneAdded; } - } // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 746e4e1..89513e6 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -8,8 +8,8 @@ #ifndef CORNER_FALKO_2D_H_ #define CORNER_FALKO_2D_H_ -#include <iostream> #include <fstream> +#include <iostream> /************************** * laser_scan_utils includes * @@ -24,86 +24,94 @@ /************************** * Falko includes * **************************/ -#include <falkolib/Feature/FALKO.h> -#include <falkolib/Feature/CGH.h> #include <falkolib/Feature/BSC.h> +#include <falkolib/Feature/CGH.h> +#include <falkolib/Feature/FALKO.h> #include <falkolib/Feature/FALKOExtractor.h> #include <falkolib/Feature/BSCExtractor.h> #include <falkolib/Feature/CGHExtractor.h> -#include <falkolib/Matching/NNMatcher.h> #include <falkolib/Matching/AHTMatcher.h> +#include <falkolib/Matching/NNMatcher.h> - -namespace laserscanutils -{ +namespace laserscanutils { /** \brief A 2D corner extractor and loop closure computing class **/ -class CornerFalko2d: public falkolib::FALKOExtractor, public falkolib::BSCExtractor<falkolib::FALKO>, public falkolib::NNMatcher<falkolib::FALKO> -{ +class CornerFalko2d : public falkolib::FALKOExtractor, + public falkolib::BSCExtractor<falkolib::FALKO>, + public falkolib::NNMatcher<falkolib::FALKO> { private: - /** \brief Get and stores a scene to use as trained/Reference set of keypoints. - **/ - void AddNewReferenceScene (falkolib::LaserScan scanFALKO); + /** \brief Get and stores a scene to use as trained/Reference set of + *keypoints. + **/ + void AddNewReferenceScene(falkolib::LaserScan scanFALKO); public: - /** - * @brief Constructor - * @param _circularSectorNumber number of grid circular sector (BSCExtractor) - * @param _radialRingNumber number of grid radial number (BSCExtractor) - * @param _useKeypointRadius if true, the selected neighborhood points search radius is keypoint one (BSCExtractor) - * @param _radius neighborhood points search radius (BSCExtractor) - */ - CornerFalko2d(int _circularSectorNumber=16, int _radialRingNumber=8, bool _useKeypointRadius = true, double _radius = 0.1) ; - - /** \brief Destructor - **/ - ~CornerFalko2d(); - - /** \brief compare new scans against the training set in order to find loop closures - **/ - void findLoopClosure(LaserScan scan,LaserScanParams scanParams); - - /** @brief set a threshold for the minimum number of matched keypoints to consider a matching correct*/ - void setKeypointsNumberTh(int _th) { - keypointsNumberTh = _th; - } - - /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object - **/ - falkolib::LaserScan convert2LaserScanFALKO(LaserScan scan,LaserScanParams scanParams); - - /** \brief Evaluates if New scene is a good candidate to be a Reference Scene. If it is detected as good, - * the scene is added and the functions returns 1. If it is not a good candidate, the function returns 0 - * and the scene is not added. - **/ - int evaluateNewReferenceScene(LaserScan scan,LaserScanParams scanParams, int scanInterval); - - - std::vector<std::vector<falkolib::FALKO>> keypointSets; - std::vector<falkolib::FALKO> lastKeypointSet; - std::vector<std::vector<falkolib::BSC>> descriptorSets; - std::vector<falkolib::BSC>lastDescriptorSet; - - // Number of Scene matched when performing the loop closure - int numberKeypointsMatch=0; - - //Number of keypoints that are matched between 2 scenes when performing the loop closure - int numberSceneMatch=0; - - int scanNumber=0; - - int scansExtracted=0; - - int matchingPosition=-1; - - int keypointsNumberTh=2; - - // Max number of matched keypoints between 2 scenes for the candidate scene be considered a good New reference scene - int refSceneAddingTh =4; + /** + * @brief Constructor + * @param _circularSectorNumber number of grid circular sector (BSCExtractor) + * @param _radialRingNumber number of grid radial number (BSCExtractor) + * @param _useKeypointRadius if true, the selected neighborhood points search + * radius is keypoint one (BSCExtractor) + * @param _radius neighborhood points search radius (BSCExtractor) + */ + CornerFalko2d(int _circularSectorNumber = 16, int _radialRingNumber = 8, + bool _useKeypointRadius = true, double _radius = 0.1); + + /** \brief Destructor + **/ + ~CornerFalko2d(); + + /** \brief compare new scans against the training set in order to find loop + *closures + **/ + void findLoopClosure(LaserScan scan, LaserScanParams scanParams); + + /** @brief set a threshold for the minimum number of matched keypoints to + * consider a matching correct*/ + void setKeypointsNumberTh(int _th) { keypointsNumberTh = _th; } + + /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan + *object + **/ + falkolib::LaserScan convert2LaserScanFALKO(LaserScan scan, + LaserScanParams scanParams); + + /** \brief Evaluates if New scene is a good candidate to be a Reference Scene. + *If it is detected as good, + * the scene is added and the functions returns 1. If it is not a good + *candidate, the function returns 0 + * and the scene is not added. + **/ + int evaluateNewReferenceScene(LaserScan scan, LaserScanParams scanParams, + int scanInterval); + + std::vector<std::vector<falkolib::FALKO>> keypointSets; + std::vector<falkolib::FALKO> lastKeypointSet; + std::vector<std::vector<falkolib::BSC>> descriptorSets; + std::vector<falkolib::BSC> lastDescriptorSet; + + // Number of Scene matched when performing the loop closure + int numberKeypointsMatch = 0; + + // Number of keypoints that are matched between 2 scenes when performing the + // loop closure + int numberSceneMatch = 0; + + int scanNumber = 0; + + int scansExtracted = 0; + + int matchingPosition = -1; + + int keypointsNumberTh = 2; + + // Max number of matched keypoints between 2 scenes for the candidate scene be + // considered a good New reference scene + int refSceneAddingTh = 4; }; } /* namespace laserscanutils */ diff --git a/src/loop_closure_base.cpp b/src/loop_closure_base.cpp index adf300d..687cefd 100644 --- a/src/loop_closure_base.cpp +++ b/src/loop_closure_base.cpp @@ -5,14 +5,15 @@ * \author: spujol */ -# include "loop_closure_base.h" +#include "loop_closure_base.h" -namespace laserscanutils{ +namespace laserscanutils { - loopClosureBase2d::loopClosureBase2d(){} - loopClosureBase2d::~loopClosureBase2d(){} +loopClosureBase2d::loopClosureBase2d() {} +loopClosureBase2d::~loopClosureBase2d() {} /* - std::shared_ptr<cornerScene> loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + std::shared_ptr<cornerScene> loopClosureBase2d::extractScene(LaserScan + &scan,LaserScanParams &scanParams){ auto NewScene=std::make_shared<cornerScene>(); @@ -20,8 +21,6 @@ namespace laserscanutils{ } */ - - //void findLoopClosure(std::list<cornerScene>& scenes, const cornerScene newScene){} - - +// void findLoopClosure(std::list<cornerScene>& scenes, const cornerScene +// newScene){} } diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index cc50265..81c538c 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -8,8 +8,8 @@ #ifndef LOOP_CLOSURE_BASE_2D_H_ #define LOOP_CLOSURE_BASE_2D_H_ -#include <iostream> #include <fstream> +#include <iostream> #include <memory> /************************** @@ -22,43 +22,43 @@ /************************** * Falko includes * **************************/ -#include <falkolib/Feature/FALKO.h> -#include <falkolib/Feature/CGH.h> #include <falkolib/Feature/BSC.h> +#include <falkolib/Feature/CGH.h> +#include <falkolib/Feature/FALKO.h> #include <falkolib/Feature/FALKOExtractor.h> #include <falkolib/Feature/BSCExtractor.h> #include <falkolib/Feature/CGHExtractor.h> -#include <falkolib/Matching/NNMatcher.h> #include <falkolib/Matching/AHTMatcher.h> +#include <falkolib/Matching/NNMatcher.h> -namespace laserscanutils -{ +namespace laserscanutils { - /** \brief A 2base class for loop closure using falko library +/** \brief A 2base class for loop closure using falko library */ -class loopClosureBase2d{ +class loopClosureBase2d { private: - public: - /** \brief Constructor - **/ - loopClosureBase2d(); - - /** \brief Destructor - **/ - ~loopClosureBase2d(); + /** \brief Constructor + **/ + loopClosureBase2d(); - /** \brief compare new scans against the trained set in order to find loop closures - **/ - //virtual void findLoopClosure(std::list<cornerScene>& scenes, const cornerScene newScene){} - //virtual void findLoopClosure(){} + /** \brief Destructor + **/ + ~loopClosureBase2d(); - /** \brief update the scene struct with keypoints and descriptors - **/ - virtual std::shared_ptr<sceneFalko<falkolib::BSC>> extractScene(LaserScan &scan,LaserScanParams &scanParams){} + /** \brief compare new scans against the trained set in order to find loop + *closures + **/ + // virtual void findLoopClosure(std::list<cornerScene>& scenes, const + // cornerScene newScene){} + // virtual void findLoopClosure(){} + /** \brief update the scene struct with keypoints and descriptors + **/ + virtual std::shared_ptr<sceneFalko<falkolib::BSC>> + extractScene(LaserScan &scan, LaserScanParams &scanParams) {} }; } /* namespace laserscanutils */ diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index c798fa2..c07eb63 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -5,61 +5,69 @@ * \author: spujol */ -# include "loop_closure_falko.h" +#include "loop_closure_falko.h" -namespace laserscanutils{ +namespace laserscanutils { - //CONSTRUCTOR - template <typename D,typename Extr, typename M> - loopClosureFalko<D,Extr, M>::loopClosureFalko(int _circularSectorNumber, int _radialRingNumber) : - loopClosureBase2d(), - falkolib::FALKOExtractor(), - extractor_(_circularSectorNumber, _radialRingNumber), - matcher_() - { - // FALKO Extractor Parameters - setMinExtractionRange(0.1); - setMaxExtractionRange(25); - enableSubbeam(true); - setNMSRadius(0.1); - setNeighB(0.01); - setBRatio(4); - setGridSectors(16); - - // Matcher Extractor Parameters - matcher_.setDistanceThreshold(0.1); +// CONSTRUCTOR +template <typename D, typename Extr, typename M> +loopClosureFalko<D, Extr, M>::loopClosureFalko(int _circularSectorNumber, + int _radialRingNumber) + : loopClosureBase2d(), falkolib::FALKOExtractor(), + extractor_(_circularSectorNumber, _radialRingNumber), matcher_() { + // FALKO Extractor Parameters + setMinExtractionRange(0.1); + setMaxExtractionRange(25); + enableSubbeam(true); + setNMSRadius(0.1); + setNeighB(0.01); + setBRatio(4); + setGridSectors(16); - } + // Matcher Extractor Parameters + 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()); - std::vector<double> doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); - ((*scanFALKO).fromRanges)(doubleRanges); - return scanFALKO; - } +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()); + std::vector<double> doubleRanges(scan.ranges_raw_.begin(), + scan.ranges_raw_.end()); + ((*scanFALKO).fromRanges)(doubleRanges); + return scanFALKO; +} /* template <typename D,typename Extr, typename M> - typename loopClosureFalko<D,Extr, M>::sceneFalkoBSCPtr loopClosureFalko<D,Extr, M>::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + typename loopClosureFalko<D,Extr, M>::sceneFalkoBSCPtr + loopClosureFalko<D,Extr, M>::extractScene(LaserScan &scan,LaserScanParams + &scanParams){ auto newScene=std::make_shared<sceneFalko<D>>(); - auto scanFALKO =loopClosureFalko<D,Extr,M>::convert2LaserScanFALKO(scan, scanParams); + auto scanFALKO =loopClosureFalko<D,Extr,M>::convert2LaserScanFALKO(scan, + scanParams); // Extract keypoints extract((*scanFALKO), (newScene->keypointsList)); // Compute descriptors - extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList); + 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){ + 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; - int matching_number = match(scene1->keypointsList,scene2->keypointsList, assoNN); + int matching_number = match(scene1->keypointsList,scene2->keypointsList, + assoNN); auto new_match=std::make_shared<matchLoopClosure<D>>(); new_match-> keypointsNumberMatch = matching_number; int keypoints_number_th=5; @@ -74,10 +82,9 @@ namespace laserscanutils{ return new_match; } */ - //void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene newScene){} - - +// void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene +// newScene){} - //Explicitly compile all the templates - template class loopClosureFalko <bsc, bscExtractor, NNMatcher>; +// 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 c4ee28a..0e28aee 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -8,111 +8,118 @@ #ifndef LOOP_CLOSURE_FALKO_H_ #define LOOP_CLOSURE_FALKO_H_ -#include <iostream> #include <fstream> -#include <memory> +#include <iostream> #include <iterator> #include <list> +#include <memory> /************************** * laser_scan_utils includes * **************************/ #include "laser_scan.h" -#include "scene_falko.h" #include "loop_closure_base.h" #include "match_loop_closure.h" +#include "scene_falko.h" /************************** * Falko includes * **************************/ -#include <falkolib/Feature/FALKO.h> -#include <falkolib/Feature/CGH.h> #include <falkolib/Feature/BSC.h> +#include <falkolib/Feature/CGH.h> +#include <falkolib/Feature/FALKO.h> #include <falkolib/Feature/FALKOExtractor.h> #include <falkolib/Feature/BSCExtractor.h> #include <falkolib/Feature/CGHExtractor.h> -#include <falkolib/Matching/NNMatcher.h> #include <falkolib/Matching/AHTMatcher.h> +#include <falkolib/Matching/NNMatcher.h> -namespace laserscanutils -{ +namespace laserscanutils { typedef falkolib::BSC bsc; typedef falkolib::BSCExtractor<falkolib::FALKO> bscExtractor; typedef falkolib::NNMatcher<falkolib::FALKO> NNMatcher; - - /** \brief A base class for loop closure using falko library - **/ +/** \brief A base class for loop closure using falko library +**/ template <typename D, typename Extr, typename M> -class loopClosureFalko : public loopClosureBase2d, public falkolib::FALKOExtractor{ +class loopClosureFalko : public loopClosureBase2d, + public falkolib::FALKOExtractor { private: - public: + typedef std::shared_ptr<sceneFalko<D>> sceneFalkoBSCPtr; + typedef std::shared_ptr<matchLoopClosure<D>> matchLoopClosurePtr; - typedef std::shared_ptr<sceneFalko<D>> sceneFalkoBSCPtr; - typedef std::shared_ptr<matchLoopClosure<D>> matchLoopClosurePtr; - - Extr extractor_; - M matcher_; - - /** \brief Constructor - **/ - loopClosureFalko(int _circularSectorNumber=16, int _radialRingNumber=8); - - /** \brief Destructor - **/ - ~loopClosureFalko(){} - - /** \brief Create and update the scene struct with keypoints and descriptors - **/ - sceneFalkoBSCPtr extractScene(LaserScan &scan,LaserScanParams &scanParams) { - auto newScene=std::make_shared<sceneFalko<D>>(); - auto scanFALKO =loopClosureFalko<D,Extr,M>::convert2LaserScanFALKO(scan, scanParams); - // Extract keypoints - extract((*scanFALKO), (newScene->keypointsList)); - // Compute descriptors - extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList); - return newScene; - } + Extr extractor_; + M matcher_; + + /** \brief Constructor + **/ + loopClosureFalko(int _circularSectorNumber = 16, int _radialRingNumber = 8); + + /** \brief Destructor + **/ + ~loopClosureFalko() {} - /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object - **/ - std::shared_ptr<falkolib::LaserScan> convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams); - - /** \brief Create and update a matchLoopClosure struct with the info that is produced when matching two given scenes - **/ - 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; + /** \brief Create and update the scene struct with keypoints and descriptors + **/ + sceneFalkoBSCPtr extractScene(LaserScan &scan, LaserScanParams &scanParams) { + auto newScene = std::make_shared<sceneFalko<D>>(); + auto scanFALKO = + loopClosureFalko<D, Extr, M>::convert2LaserScanFALKO(scan, scanParams); + // Extract keypoints + extract((*scanFALKO), (newScene->keypointsList)); + // Compute descriptors + extractor_.compute(*scanFALKO, newScene->keypointsList, + newScene->descriptorsList); + return newScene; + } + + /** \brief Convert scans from laserscanutils::LaserScan to + *falkolib::LaserScan object + **/ + std::shared_ptr<falkolib::LaserScan> + convert2LaserScanFALKO(LaserScan &scan, LaserScanParams &scanParams); + + /** \brief Create and update a matchLoopClosure struct with the info that is + *produced when matching two given scenes + **/ + 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; + } - /** \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); - auto new_match = matchScene(*l_front,new_scene); - matchings.push_back(new_match); - } - return matchings; + /** \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); + auto new_match = matchScene(*l_front, new_scene); + matchings.push_back(new_match); } + return matchings; + } }; } /* namespace laserscanutils */ diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index bf85aee..2753a4b 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -8,8 +8,8 @@ #ifndef MATCH_LOOP_CLOSURE_H_ #define MATCH_LOOP_CLOSURE_H_ -#include <iostream> #include <fstream> +#include <iostream> #include <memory> #include <tuple> @@ -18,23 +18,20 @@ **************************/ #include "scene_falko.h" -namespace laserscanutils -{ +namespace laserscanutils { -template <typename D> -struct matchLoopClosure{ - //std::shared_ptr<sceneFalko<D>> scene1; - //std::shared_ptr<sceneFalko<D>> scene2; - - // tuple that stores the pointers of two matched scenes - std::tuple<std::shared_ptr<sceneFalko<D>>, std::shared_ptr<sceneFalko<D>>> sceneTuple; - bool match; - int keypointsNumberMatch; - double score; +template <typename D> struct matchLoopClosure { + // std::shared_ptr<sceneFalko<D>> scene1; + // std::shared_ptr<sceneFalko<D>> scene2; + // tuple that stores the pointers of two matched scenes + std::tuple<std::shared_ptr<sceneFalko<D>>, std::shared_ptr<sceneFalko<D>>> + sceneTuple; + bool match; + int keypointsNumberMatch; + double score; }; - } /* namespace laserscanutils */ #endif /* MATCH_LOOP_CLOSURE_H_ */ diff --git a/src/scene_base.h b/src/scene_base.h index 4be0f75..3511177 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -8,19 +8,16 @@ #ifndef SCENE_BASE_H_ #define SCENE_BASE_H_ -#include <iostream> #include <fstream> +#include <iostream> +namespace laserscanutils { -namespace laserscanutils -{ - -struct sceneBase{ - //std::vector<falkolib::FALKO> keypointsList; - //std::vector<D> descriptorsList; +struct sceneBase { + // std::vector<falkolib::FALKO> keypointsList; + // std::vector<D> descriptorsList; }; - } /* namespace laserscanutils */ #endif /* SCENE_BASE_H_ */ diff --git a/src/scene_falko.h b/src/scene_falko.h index 9d041f8..ef096ae 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -8,41 +8,35 @@ #ifndef SCENE_FALKO_H_ #define SCENE_FALKO_H_ -#include <iostream> #include <fstream> +#include <iostream> /************************** * LaserScanUtils includes * **************************/ #include "scene_base.h" - /************************** * Falko includes * **************************/ -#include <falkolib/Feature/FALKO.h> -#include <falkolib/Feature/CGH.h> #include <falkolib/Feature/BSC.h> +#include <falkolib/Feature/CGH.h> +#include <falkolib/Feature/FALKO.h> #include <falkolib/Feature/FALKOExtractor.h> #include <falkolib/Feature/BSCExtractor.h> #include <falkolib/Feature/CGHExtractor.h> -#include <falkolib/Matching/NNMatcher.h> #include <falkolib/Matching/AHTMatcher.h> +#include <falkolib/Matching/NNMatcher.h> -namespace laserscanutils -{ +namespace laserscanutils { -template <typename D> -struct sceneFalko : public sceneBase{ - std::vector<falkolib::FALKO> keypointsList; - std::vector<D> descriptorsList; +template <typename D> struct sceneFalko : public sceneBase { + std::vector<falkolib::FALKO> keypointsList; + std::vector<D> descriptorsList; }; - - - } /* namespace laserscanutils */ #endif /* SCENE_FALKO_H_ */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index e004f63..caa15c5 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -1,46 +1,43 @@ -#include "gtest/utils_gtest.h" -#include "testData2.h" #include "../src/loop_closure_base.h" #include "../src/loop_closure_falko.h" - +#include "testData2.h" +#include "gtest/utils_gtest.h" using namespace laserscanutils; -TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) -{ - int scanSize = 1440; - - LaserScan scan; - LaserScanParams laserParams; +TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { + int scanSize = 1440; + LaserScan scan; + LaserScanParams laserParams; - laserParams.angle_min_=0; - laserParams.angle_max_=2.0 * M_PI; + laserParams.angle_min_ = 0; + laserParams.angle_max_ = 2.0 * M_PI; - for (int i=0; i<scanSize; i++) { - scan.ranges_raw_.push_back(testRanges1[i]); - } + for (int i = 0; i < scanSize; i++) { + scan.ranges_raw_.push_back(testRanges1[i]); + } loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko; - auto new_scene=LCFalko.extractScene(scan, laserParams); + auto new_scene = LCFalko.extractScene(scan, laserParams); - int detectedKeypoints =((*new_scene).keypointsList).size(); + int detectedKeypoints = ((*new_scene).keypointsList).size(); - int detectedDescriptors =new_scene->descriptorsList.size(); + int detectedDescriptors = new_scene->descriptorsList.size(); ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedDescriptors, 18); - //Test matcher + // Test matcher - auto new_match =LCFalko.matchScene(new_scene,new_scene); + auto new_match = LCFalko.matchScene(new_scene, new_scene); int keypoints_matched = new_match->keypointsNumberMatch; ASSERT_EQ(keypoints_matched, 18); - //TEST findLoopClosure + // TEST findLoopClosure std::list<std::shared_ptr<sceneFalko<bsc>>> ref_scenes; @@ -48,9 +45,9 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ref_scenes.push_back(new_scene); - auto matchings = LCFalko.findLoopClosure(ref_scenes,new_scene); + auto matchings = LCFalko.findLoopClosure(ref_scenes, new_scene); - int matchings_number =matchings.size(); + int matchings_number = matchings.size(); ASSERT_EQ(matchings_number, 2); @@ -60,36 +57,35 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ASSERT_EQ(there_is_match, true); -// PRINTF("All good at TestTest::DummyTestExample !\n"); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -TEST(loop_closure_falko2, convert2laserScanFalko) -{ - int scanSize = 1440; +TEST(loop_closure_falko2, convert2laserScanFalko) { + int scanSize = 1440; - LaserScan scan; - LaserScanParams laserParams; + LaserScan scan; + LaserScanParams laserParams; - laserParams.angle_min_=0; - laserParams.angle_max_=2.0 * M_PI; + laserParams.angle_min_ = 0; + laserParams.angle_max_ = 2.0 * M_PI; - for (int i=0; i<scanSize; i++) { - scan.ranges_raw_.push_back(testRanges1[i]); - } + for (int i = 0; i < scanSize; i++) { + scan.ranges_raw_.push_back(testRanges1[i]); + } loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko; - std::shared_ptr<falkolib::LaserScan> scanFALKO=LCFalko.convert2LaserScanFALKO(scan, laserParams); + std::shared_ptr<falkolib::LaserScan> scanFALKO = + LCFalko.convert2LaserScanFALKO(scan, laserParams); int firstPoint = ((*scanFALKO).ranges)[0]; ASSERT_EQ(firstPoint, 250); -// PRINTF("All good at TestTest::DummyTestExample !\n"); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } -- GitLab