diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index c07eb6330764cad4c9999e494decbeed93efec80..0a41ac0a67ffa02e37b9b7f56bc7374a98d00b26 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -10,6 +10,7 @@ namespace laserscanutils { // CONSTRUCTOR +/* template <typename D, typename Extr, typename M> loopClosureFalko<D, Extr, M>::loopClosureFalko(int _circularSectorNumber, int _radialRingNumber) @@ -27,11 +28,14 @@ loopClosureFalko<D, Extr, M>::loopClosureFalko(int _circularSectorNumber, // 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( @@ -43,6 +47,7 @@ loopClosureFalko<D, Extr, M>::convert2LaserScanFALKO( ((*scanFALKO).fromRanges)(doubleRanges); return scanFALKO; } +*/ /* template <typename D,typename Extr, typename M> typename loopClosureFalko<D,Extr, M>::sceneFalkoBSCPtr @@ -86,5 +91,5 @@ loopClosureFalko<D, Extr, M>::convert2LaserScanFALKO( // newScene){} // Explicitly compile all the templates -template class loopClosureFalko<bsc, bscExtractor, NNMatcher>; +//template class loopClosureFalko<bsc, bscExtractor, NNMatcher>; } diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 0e28aeed209b632e3875059fc053a4480281024b..80a2d494aceb660fbc75b0fe71275362c9be9c71 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -42,6 +42,10 @@ typedef falkolib::BSC bsc; typedef falkolib::BSCExtractor<falkolib::FALKO> bscExtractor; typedef falkolib::NNMatcher<falkolib::FALKO> NNMatcher; +typedef falkolib::CGH cgh; +typedef falkolib::CGHExtractor<falkolib::FALKO> cghExtractor; +typedef falkolib::AHTMatcher<falkolib::FALKO> AHTatcher; + /** \brief A base class for loop closure using falko library **/ @@ -52,13 +56,28 @@ private: public: typedef std::shared_ptr<sceneFalko<D>> sceneFalkoBSCPtr; typedef std::shared_ptr<matchLoopClosure<D>> matchLoopClosurePtr; + typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr; Extr extractor_; M matcher_; /** \brief Constructor **/ - loopClosureFalko(int _circularSectorNumber = 16, int _radialRingNumber = 8); + loopClosureFalko(int _circularSectorNumber = 16, int _radialRingNumber = 8) + : 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); +}; /** \brief Destructor **/ @@ -81,8 +100,14 @@ public: /** \brief Convert scans from laserscanutils::LaserScan to *falkolib::LaserScan object **/ - std::shared_ptr<falkolib::LaserScan> - convert2LaserScanFALKO(LaserScan &scan, LaserScanParams &scanParams); + laserScanPtr 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; +}; /** \brief Create and update a matchLoopClosure struct with the info that is *produced when matching two given scenes diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index 2753a4bcd882aac2e5ab1f1857c663a057ec3542..41d1009085c2c548312bb882eb91efa8c837fbc4 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -21,8 +21,6 @@ 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>>>