diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 80a2d494aceb660fbc75b0fe71275362c9be9c71..2d1689e5aff3f510e8d1e8fef029dfa55ec855b3 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -46,6 +46,25 @@ typedef falkolib::CGH cgh; typedef falkolib::CGHExtractor<falkolib::FALKO> cghExtractor; typedef falkolib::AHTMatcher<falkolib::FALKO> AHTatcher; +struct parameterLoopClosureFalko { + // Keypoints extractor + double _min_extraction_range=0.1; + double _max_extraction_range=25; + bool _enable_subbeam=true; + double _nms_radius=0.1; + double _neigh_b=0.01; + double _b_ratio=4; + int _grid_sectors=16; + + //Descriptors parameters + int _circularSectorNumber = 16; + int _radialRingNumber = 8; + + //matcher threshold + double matcher_distance_th=0.1; + int keypoints_number_th = 5; +}; + /** \brief A base class for loop closure using falko library **/ @@ -63,20 +82,21 @@ public: /** \brief Constructor **/ - loopClosureFalko(int _circularSectorNumber = 16, int _radialRingNumber = 8) + loopClosureFalko(parameterLoopClosureFalko param) : loopClosureBase2d(), falkolib::FALKOExtractor(), - extractor_(_circularSectorNumber, _radialRingNumber), matcher_() { + extractor_(param._circularSectorNumber, param._radialRingNumber), matcher_() { // FALKO Extractor Parameters - setMinExtractionRange(0.1); - setMaxExtractionRange(25); - enableSubbeam(true); - setNMSRadius(0.1); - setNeighB(0.01); - setBRatio(4); - setGridSectors(16); + setMinExtractionRange(param._min_extraction_range); + setMaxExtractionRange(param._max_extraction_range); + enableSubbeam(param._enable_subbeam); + setNMSRadius(param._nms_radius); + setNeighB(param._neigh_b); + setBRatio(param._b_ratio); + setGridSectors(param._grid_sectors); // Matcher Extractor Parameters - matcher_.setDistanceThreshold(0.1); + matcher_.setDistanceThreshold(param.matcher_distance_th); + keypoints_number_th = param.keypoints_number_th; }; /** \brief Destructor @@ -100,13 +120,13 @@ public: /** \brief Convert scans from laserscanutils::LaserScan to *falkolib::LaserScan object **/ - 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; + laserScanPtr convert2LaserScanFALKO(LaserScan &_scan, LaserScanParams &_scan_params) { + auto scan_falko = std::make_shared<falkolib::LaserScan>( + _scan_params.angle_min_, _scan_params.angle_max_, _scan.ranges_raw_.size()); + std::vector<double> double_ranges(_scan.ranges_raw_.begin(), + _scan.ranges_raw_.end()); + scan_falko->fromRanges(double_ranges); + return scan_falko; }; /** \brief Create and update a matchLoopClosure struct with the info that is @@ -119,7 +139,6 @@ public: 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 { @@ -145,6 +164,9 @@ public: } return matchings; } + + int keypoints_number_th; + }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index caa15c5550837d4ecaca171c1e68ebba1095bb12..0373da06354febb8c3970a587df735adde135d6b 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -17,7 +17,9 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { scan.ranges_raw_.push_back(testRanges1[i]); } - loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko; + parameterLoopClosureFalko param; + loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko(param); + auto new_scene = LCFalko.extractScene(scan, laserParams); @@ -73,7 +75,9 @@ TEST(loop_closure_falko2, convert2laserScanFalko) { scan.ranges_raw_.push_back(testRanges1[i]); } - loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko; + parameterLoopClosureFalko param; + loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko(param); + std::shared_ptr<falkolib::LaserScan> scanFALKO = LCFalko.convert2LaserScanFALKO(scan, laserParams);