From 66ce29a71fdd1b4b6e001e43939bd78891c6fc7a Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 26 Jan 2021 16:03:12 +0100 Subject: [PATCH 001/100] corner_falko_class created. CMakelists updated --- src/CMakeLists.txt | 31 ++++++++++++++++ src/corner_falko_2d.cpp | 10 ++++++ src/corner_falko_2d.h | 58 ++++++++++++++++++++++++++++++ src/examples/CMakeLists.txt | 6 +++- src/examples/corner_falko_demo.cpp | 36 +++++++++++++++++++ 5 files changed, 140 insertions(+), 1 deletion(-) create mode 100644 src/corner_falko_2d.cpp create mode 100644 src/corner_falko_2d.h create mode 100644 src/examples/corner_falko_demo.cpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5a9977b..54c7fbd 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,6 +13,13 @@ ENDIF(faramotics_FOUND) FIND_PACKAGE(Eigen3 3.3 REQUIRED) FIND_PACKAGE(csm QUIET) +find_package(falkolib REQUIRED) +message(STATUS "falkolib_FOUND ${falkolib_FOUND}") +message(STATUS "falkolib_INCLUDE_DIRS ${falkolib_INCLUDE_DIRS}") +message(STATUS "falkolib_LIBRARY_DIRS ${falkolib_LIBRARY_DIRS}") +message(STATUS "falkolib_LIBRARIES ${falkolib_LIBRARIES}") + + #include directories INCLUDE_DIRECTORIES(. /usr/local/include) INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) @@ -23,6 +30,11 @@ IF(faramotics_FOUND) INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS}) ENDIF(faramotics_FOUND) +if(${falkolib_FOUND}) + include_directories(${falkolib_INCLUDE_DIRS}) + link_directories(${falkolib_LIBRARY_DIRS}) +endif() + #headers SET(HDRS_BASE laser_scan_utils.h) @@ -43,12 +55,18 @@ SET(HDRS point_set.h polyline.h scan_segment.h + corner_falko_2d.h ) IF(csm_FOUND) SET(HDRS ${HDRS} icp.h) ENDIF(csm_FOUND) + IF(falkolib_FOUND) + SET(HDRS ${HDRS} + corner_falko_2d.h) + ENDIF(falkolib_FOUND) + #sources SET(SRCS corner_finder.cpp @@ -71,6 +89,13 @@ SET(SRCS SET(SRCS ${SRCS} icp.cpp) ENDIF(csm_FOUND) + + IF(falkolib_FOUND) + SET(SRCS ${SRCS} + corner_falko_2d.cpp) + ENDIF(falkolib_FOUND) + + # create the shared library ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS}) @@ -78,6 +103,12 @@ IF(csm_FOUND) target_link_libraries(${PROJECT_NAME} ${csm_LIBRARY}) ENDIF(csm_FOUND) +IF(falkolib_FOUND) + target_link_libraries(${PROJECT_NAME} ${falkolib_LIBRARY}) +ENDIF(falkolib_FOUND) + + + # Examples IF(BUILD_DEMOS) MESSAGE("Building examples.") diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp new file mode 100644 index 0000000..d0c8c90 --- /dev/null +++ b/src/corner_falko_2d.cpp @@ -0,0 +1,10 @@ +/** + * \file corner_falko_2d.cpp + * + * Created on: Jan 26, 2021 + * \author: spujol + */ + +#include "corner_falko_2d.h" + + diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h new file mode 100644 index 0000000..476584a --- /dev/null +++ b/src/corner_falko_2d.h @@ -0,0 +1,58 @@ +/** + * \file corner_falko_2d.h + * + * Created on: Jan 26, 2021 + * \author: spujol + */ + +#ifndef CORNER_FALKO_2D_H_ +#define CORNER_FALKO_2D_H_ + +#include +#include + +/************************** + * WOLF includes * + **************************/ +//#include "core/landmark/landmark_base.h" + +/************************** + * Falko includes * + **************************/ +#include +#include +#include +#include + +#include +#include + +#include +#include + + +namespace wolf +{ +class LandmarkCorner2d +{ +public: + std::vector> cornerSet; + std::vector> descriptorSet; + + /** \brief Gets a set of landmarks/scenes to use as trained set. + **/ + void train(); + + /** \brief Extract landmark/scene (list of corners) from a given 2D scan + **/ + void extract(); + + /** \brief compare new scans against the training set in order to find loop closures + **/ + void findLoopClosure(); + +}; + +} /* namespace wolf */ + +#endif /* LANDMARK_POLYLINE_2d_H_ */ diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 005644f..86ea3bc 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -2,4 +2,8 @@ INCLUDE_DIRECTORIES(../src) # polylines demo ADD_EXECUTABLE(demo_polylines polyline_demo.cpp) -TARGET_LINK_LIBRARIES(demo_polylines ${PROJECT_NAME}) \ No newline at end of file +TARGET_LINK_LIBRARIES(demo_polylines ${PROJECT_NAME}) + +# falkocorners demo +ADD_EXECUTABLE(demo_corner_falko corner_falko_demo.cpp) +TARGET_LINK_LIBRARIES(demo_corner_falko ${PROJECT_NAME}) \ No newline at end of file diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp new file mode 100644 index 0000000..b9a39af --- /dev/null +++ b/src/examples/corner_falko_demo.cpp @@ -0,0 +1,36 @@ +/** + * \file corner_falko_2d_test.cpp + * + * Created on: Jan 26, 2021 + * \author: spujol + */ + +//LaserScanUtils includes +#include "../line_finder_iterative.h" +#include "../corner_falko_2d.h" +#include "../laser_scan.h" + +//std includes +#include +#include +using namespace laserscanutils; + +int main(int argc, char** argv) +{ + LaserScanParams scan_params({-1.57079994678, 1.57079994678, 0.00875097513199, 0.0, 0.10000000149, 50.0, 0.1, 0.1}); + + std::vector ranges({49.97591781616211, 49.996429443359375, 49.999759674072266, 50.0, 50.0, 50.0, 49.998634338378906, 50.0, 50.0, 49.99236297607422, 49.99384307861328, 50.0, 49.9869270324707, 50.0, 49.99005889892578, 49.99773406982422, 50.0, 49.98741912841797, 50.0, 49.99842071533203, 50.0, 49.99243927001953, 49.997291564941406, 50.0, 50.0, 49.98580551147461, 49.991844177246094, 49.98896026611328, 50.0, 50.0, 49.9897346496582, 49.998111724853516, 49.99882125854492, 50.0, 50.0, 50.0, 50.0, 49.999698638916016, 50.0, 50.0, 50.0, 50.0, 49.991397857666016, 49.99360275268555, 49.999027252197266, 49.99750900268555, 49.99100112915039, 49.998714447021484, 49.98794174194336, 50.0, 50.0, 50.0, 50.0, 50.0, 49.98186492919922, 50.0, 50.0, 50.0, 49.99155807495117, 49.997196197509766, 49.98872375488281, 49.99138259887695, 50.0, 49.99021530151367, 49.99164581298828, 50.0, 49.991390228271484, 50.0, 50.0, 50.0, 49.997249603271484, 50.0, 49.991851806640625, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.97983169555664, 49.98630142211914, 50.0, 20.6888370513916, 49.9797477722168, 49.98846435546875, 49.99418640136719, 50.0, 50.0, 50.0, 49.99342346191406, 50.0, 49.9906005859375, 50.0, 49.99853515625, 49.989444732666016, 38.552852630615234, 38.28703689575195, 38.04865264892578, 37.78112030029297, 37.54747772216797, 37.28171157836914, 37.206565856933594, 36.835411071777344, 36.61864471435547, 36.39655685424805, 36.1579475402832, 35.95964431762695, 35.75605773925781, 35.552188873291016, 35.75926208496094, 36.27781677246094, 35.16993713378906, 34.99699401855469, 34.82910919189453, 34.6483039855957, 34.48637390136719, 34.32539749145508, 34.16202163696289, 34.232051849365234, 33.86043167114258, 33.71691131591797, 33.566650390625, 33.42384338378906, 33.2882080078125, 33.16693115234375, 33.041419982910156, 32.906009674072266, 33.008323669433594, 33.706356048583984, 34.43825149536133, 35.25088119506836, 36.05652618408203, 36.930118560791016, 37.83384704589844, 33.12321472167969, 33.02351760864258, 32.9192008972168, 32.82925796508789, 32.74382781982422, 32.64959716796875, 32.580204010009766, 32.49120330810547, 33.05714797973633, 32.343536376953125, 32.26381301879883, 32.21063232421875, 32.12488555908203, 32.06965255737305, 32.0222282409668, 31.954957962036133, 31.903532028198242, 31.83578872680664, 32.51456832885742, 34.189456939697266, 31.12668228149414, 31.076339721679688, 31.047151565551758, 30.967018127441406, 30.956220626831055, 30.924589157104492, 30.893285751342773, 30.869199752807617, 30.843050003051758, 32.791847229003906, 30.809431076049805, 30.79128074645996, 30.779237747192383, 30.776460647583008, 30.74305534362793, 30.74994468688965, 30.7137393951416, 30.734609603881836, 30.719928741455078, 30.71673011779785, 49.99970626831055, 50.0, 49.987911224365234, 33.68583679199219, 31.76846694946289, 31.8026123046875, 31.802202224731445, 31.818490982055664, 31.85223960876465, 31.86141014099121, 31.906801223754883, 31.93423843383789, 31.964210510253906, 33.567230224609375, 32.055015563964844, 32.07001876831055, 32.13076400756836, 32.16000747680664, 32.22781753540039, 32.26890563964844, 32.323944091796875, 32.36326217651367, 32.430908203125, 49.980655670166016, 34.32135772705078, 33.09465789794922, 32.27247619628906, 32.33710479736328, 32.41763687133789, 32.498661041259766, 32.57213592529297, 32.67158126831055, 32.74591827392578, 32.814476013183594, 32.93477249145508, 33.04751968383789, 33.136863708496094, 33.23999786376953, 33.34675979614258, 33.42970657348633, 33.53573226928711, 33.66716003417969, 33.78378677368164, 33.905670166015625, 34.02836608886719, 34.151817321777344, 34.2794189453125, 34.41516876220703, 34.551273345947266, 34.702728271484375, 34.84151840209961, 34.986881256103516, 35.162757873535156, 35.332794189453125, 35.47941970825195, 35.65633010864258, 35.82624435424805, 36.00060272216797, 36.17729187011719, 36.36515808105469, 36.55763626098633, 36.744773864746094, 38.46407699584961, 37.869808197021484, 37.767921447753906, 37.958900451660156, 38.20857620239258, 38.38622283935547, 38.68323516845703, 38.871334075927734, 39.151519775390625, 39.377750396728516, 39.68268966674805, 39.89873123168945, 40.197330474853516, 40.47549819946289, 40.73743438720703, 41.04566955566406, 42.33650207519531, 41.92591094970703, 41.43912124633789, 41.045528411865234, 41.32114028930664, 41.581878662109375, 41.944580078125, 42.318912506103516, 42.6725959777832, 43.07264709472656, 43.443477630615234, 43.83216094970703, 44.19996643066406, 44.63225555419922, 45.06049346923828, 45.468536376953125, 45.89896774291992, 46.330604553222656, 46.778343200683594, 47.31666946411133, 47.789310455322266, 48.26376724243164, 48.826602935791016, 49.33188247680664, 49.990909576416016, 50.0, 50.0, 50.0, 50.0, 49.995697021484375, 49.99568176269531, 49.98163986206055, 50.0, 50.0, 49.9764289855957, 50.0, 50.0, 49.98639678955078, 49.99431228637695, 50.0, 50.0, 50.0, 50.0, 49.9874267578125, 50.0, 49.98714828491211, 50.0, 49.99470901489258, 49.99113464355469, 50.0, 50.0, 50.0, 49.985504150390625, 49.99067306518555, 50.0, 49.997161865234375, 50.0, 50.0, 50.0, 49.995513916015625, 49.993038177490234, 50.0, 49.99763107299805, 50.0, 49.98752975463867, 50.0, 49.988037109375, 50.0, 50.0, 50.0, 49.9975700378418, 50.0, 49.998756408691406, 49.97819519042969, 49.99104690551758, 49.99087905883789, 49.94268798828125, 49.85968017578125, 49.786617279052734, 49.70594787597656, 49.62562561035156, 49.56686782836914, 49.50475311279297, 49.416934967041016, 49.35699462890625, 49.308589935302734, 49.990482330322266, 50.0, 49.998836517333984, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.980472564697266, 49.99903869628906, 50.0, 50.0, 49.989845275878906, 49.98395919799805, 50.0, 49.99302673339844, 49.99530792236328, 49.99745559692383, 50.0, 49.99560546875, 21.569303512573242}); + LaserScan scan(ranges); + + std::cout << "scan created!" << std::endl; + + LineFinderIterative line_finder(LineFinderIterativeParams({1, 5, 1, 1})); + + std::cout << "line_finder created!" << std::endl; + + std::list polyline_list; + line_finder.findPolylines(scan, scan_params, polyline_list); + + std::cout << polyline_list.size() << " polylines found!" << std::endl; + +} -- GitLab From 9af5aa0ed50b902ff3ff235a18783da0affb92ac Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 26 Jan 2021 17:00:56 +0100 Subject: [PATCH 002/100] CornerFalko2d class updated --- src/corner_falko_2d.cpp | 28 ++++++++++++++++++++++++++++ src/corner_falko_2d.h | 22 ++++++++++++++++++---- 2 files changed, 46 insertions(+), 4 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index d0c8c90..80e5b84 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -7,4 +7,32 @@ #include "corner_falko_2d.h" +namespace wolf{ + +void CornerFalko2d::extract (falkolib::LaserScan scan){ + + + + fe.setMinExtractionRange(0.25); + fe.setMaxExtractionRange(25); + fe.enableSubbeam(true); + fe.setNMSRadius(0.1); + fe.setNeighB(0.01); + fe.setBRatio(4); + fe.setGridSectors(16); + + std::vector keypoints; + + /* + fe.extract(scan, keypoints); + + + falkolib::BSCExtractor bsc(16,8); + std::vector bscDesc; + bsc.compute(scan, keypoints, bscDesc); + */ +} + +} + diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 476584a..b5986b3 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -33,11 +33,22 @@ namespace wolf { -class LandmarkCorner2d +class CornerFalko2d { public: - std::vector> cornerSet; - std::vector> descriptorSet; + /** \brief Constructor + * + * Constructor + * + **/ + CornerFalko2d(); + + /** \brief Destructor + * + * Destructor + * + **/ + ~CornerFalko2d(); /** \brief Gets a set of landmarks/scenes to use as trained set. **/ @@ -45,12 +56,15 @@ public: /** \brief Extract landmark/scene (list of corners) from a given 2D scan **/ - void extract(); + void extract(falkolib::LaserScan scan); /** \brief compare new scans against the training set in order to find loop closures **/ void findLoopClosure(); + std::vector> CornerSet; + std::vector> descriptorSets; + falkolib::FALKOExtractor fe; }; } /* namespace wolf */ -- GitLab From cdbaaee8f1906b1bb278ec4cfb16d17b5acd063b Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 28 Jan 2021 13:29:18 +0100 Subject: [PATCH 003/100] CornerFalko2d class refactorized with inheritances --- src/corner_falko_2d.cpp | 45 +++++++++++++++++++++-------------------- src/corner_falko_2d.h | 41 +++++++++++++++++++++++++++---------- 2 files changed, 53 insertions(+), 33 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 80e5b84..6e7b42f 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -9,30 +9,31 @@ namespace wolf{ -void CornerFalko2d::extract (falkolib::LaserScan scan){ - - - - fe.setMinExtractionRange(0.25); - fe.setMaxExtractionRange(25); - fe.enableSubbeam(true); - fe.setNMSRadius(0.1); - fe.setNeighB(0.01); - fe.setBRatio(4); - fe.setGridSectors(16); - - std::vector keypoints; - - /* - fe.extract(scan, keypoints); - - - falkolib::BSCExtractor bsc(16,8); - std::vector bscDesc; - bsc.compute(scan, keypoints, bscDesc); - */ +CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, bool _useKeypointRadius, double _radius) :BSCExtractor( _circularSectorNumber, _radialRingNumber, _useKeypointRadius ,_radius ) { + // FALKO EXTRACTOR PARAMS + setMinExtractionRange(0.25); + setMaxExtractionRange(25); + enableSubbeam(true); + setNMSRadius(0.1); + setNeighB(0.01); + setBRatio(4); + setGridSectors(16); + } + +void CornerFalko2d::train (falkolib::LaserScan scan){ + + // Extract keypoints + lastKeypointSet.clear(); + extract(scan, lastKeypointSet); + + //Compute descriptors + lastDescriptorSet.clear(); + bsc.compute(scan, lastKeypointSet, lastDescriptorSet); + } +} // wolf namespace + diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index b5986b3..13d5289 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -33,15 +33,25 @@ namespace wolf { -class CornerFalko2d + +/** \brief A 2D corner extractor and loop closure computing class + * + * + * + * + */ + +class CornerFalko2d: public falkolib::FALKOExtractor, falkolib::BSCExtractor { public: - /** \brief Constructor - * - * Constructor - * - **/ - CornerFalko2d(); + /** + * @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 * @@ -52,19 +62,28 @@ public: /** \brief Gets a set of landmarks/scenes to use as trained set. **/ - void train(); + void train(falkolib::LaserScan scan); /** \brief Extract landmark/scene (list of corners) from a given 2D scan **/ - void extract(falkolib::LaserScan scan); + //void extract(falkolib::LaserScan scan); /** \brief compare new scans against the training set in order to find loop closures **/ void findLoopClosure(); - std::vector> CornerSet; + std::vector> keypointSets; + std::vector lastKeypointSet; std::vector> descriptorSets; - falkolib::FALKOExtractor fe; + std::vectorlastDescriptorSet; + + //int _circularSectorNumber; + //int _radialRingNumber; + // Keypoints extractor + //falkolib::FALKOExtractor fe; + // BSC descriptor extractor + //falkolib::BSCExtractor bsc(_circularSectorNumber,_radialRingNumber); + }; } /* namespace wolf */ -- GitLab From 22ef78f6bea79d7d36cf869e7da993b718ac1b6a Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 28 Jan 2021 15:33:11 +0100 Subject: [PATCH 004/100] added src/examples/corner_falko_demo.cpp and testData --- src/corner_falko_2d.cpp | 10 +++++++--- src/corner_falko_2d.h | 2 +- src/examples/CMakeLists.txt | 4 ++-- src/examples/corner_falko_demo.cpp | 17 ++++++++--------- src/examples/testData.cpp | 23 +++++++++++++++++++++++ src/examples/testData.h | 28 ++++++++++++++++++++++++++++ 6 files changed, 69 insertions(+), 15 deletions(-) create mode 100644 src/examples/testData.cpp create mode 100644 src/examples/testData.h diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 6e7b42f..4b298a5 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -7,7 +7,7 @@ #include "corner_falko_2d.h" -namespace wolf{ +namespace laserscanutils{ CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, bool _useKeypointRadius, double _radius) :BSCExtractor( _circularSectorNumber, _radialRingNumber, _useKeypointRadius ,_radius ) { // FALKO EXTRACTOR PARAMS @@ -21,6 +21,10 @@ CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, b } +CornerFalko2d::~CornerFalko2d() +{ + +} void CornerFalko2d::train (falkolib::LaserScan scan){ @@ -30,10 +34,10 @@ void CornerFalko2d::train (falkolib::LaserScan scan){ //Compute descriptors lastDescriptorSet.clear(); - bsc.compute(scan, lastKeypointSet, lastDescriptorSet); + compute(scan, lastKeypointSet, lastDescriptorSet); } -} // wolf namespace +} // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 13d5289..cb1b675 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -31,7 +31,7 @@ #include -namespace wolf +namespace laserscanutils { /** \brief A 2D corner extractor and loop closure computing class diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 86ea3bc..40bd1f1 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -5,5 +5,5 @@ ADD_EXECUTABLE(demo_polylines polyline_demo.cpp) TARGET_LINK_LIBRARIES(demo_polylines ${PROJECT_NAME}) # falkocorners demo -ADD_EXECUTABLE(demo_corner_falko corner_falko_demo.cpp) -TARGET_LINK_LIBRARIES(demo_corner_falko ${PROJECT_NAME}) \ No newline at end of file +ADD_EXECUTABLE(demo_corner_falko corner_falko_demo.cpp testData.cpp) +TARGET_LINK_LIBRARIES(demo_corner_falko ${PROJECT_NAME}) diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index b9a39af..acfdfe4 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -9,6 +9,8 @@ #include "../line_finder_iterative.h" #include "../corner_falko_2d.h" #include "../laser_scan.h" +#include "testData.h" + //std includes #include @@ -17,20 +19,17 @@ using namespace laserscanutils; int main(int argc, char** argv) { - LaserScanParams scan_params({-1.57079994678, 1.57079994678, 0.00875097513199, 0.0, 0.10000000149, 50.0, 0.1, 0.1}); - std::vector ranges({49.97591781616211, 49.996429443359375, 49.999759674072266, 50.0, 50.0, 50.0, 49.998634338378906, 50.0, 50.0, 49.99236297607422, 49.99384307861328, 50.0, 49.9869270324707, 50.0, 49.99005889892578, 49.99773406982422, 50.0, 49.98741912841797, 50.0, 49.99842071533203, 50.0, 49.99243927001953, 49.997291564941406, 50.0, 50.0, 49.98580551147461, 49.991844177246094, 49.98896026611328, 50.0, 50.0, 49.9897346496582, 49.998111724853516, 49.99882125854492, 50.0, 50.0, 50.0, 50.0, 49.999698638916016, 50.0, 50.0, 50.0, 50.0, 49.991397857666016, 49.99360275268555, 49.999027252197266, 49.99750900268555, 49.99100112915039, 49.998714447021484, 49.98794174194336, 50.0, 50.0, 50.0, 50.0, 50.0, 49.98186492919922, 50.0, 50.0, 50.0, 49.99155807495117, 49.997196197509766, 49.98872375488281, 49.99138259887695, 50.0, 49.99021530151367, 49.99164581298828, 50.0, 49.991390228271484, 50.0, 50.0, 50.0, 49.997249603271484, 50.0, 49.991851806640625, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.97983169555664, 49.98630142211914, 50.0, 20.6888370513916, 49.9797477722168, 49.98846435546875, 49.99418640136719, 50.0, 50.0, 50.0, 49.99342346191406, 50.0, 49.9906005859375, 50.0, 49.99853515625, 49.989444732666016, 38.552852630615234, 38.28703689575195, 38.04865264892578, 37.78112030029297, 37.54747772216797, 37.28171157836914, 37.206565856933594, 36.835411071777344, 36.61864471435547, 36.39655685424805, 36.1579475402832, 35.95964431762695, 35.75605773925781, 35.552188873291016, 35.75926208496094, 36.27781677246094, 35.16993713378906, 34.99699401855469, 34.82910919189453, 34.6483039855957, 34.48637390136719, 34.32539749145508, 34.16202163696289, 34.232051849365234, 33.86043167114258, 33.71691131591797, 33.566650390625, 33.42384338378906, 33.2882080078125, 33.16693115234375, 33.041419982910156, 32.906009674072266, 33.008323669433594, 33.706356048583984, 34.43825149536133, 35.25088119506836, 36.05652618408203, 36.930118560791016, 37.83384704589844, 33.12321472167969, 33.02351760864258, 32.9192008972168, 32.82925796508789, 32.74382781982422, 32.64959716796875, 32.580204010009766, 32.49120330810547, 33.05714797973633, 32.343536376953125, 32.26381301879883, 32.21063232421875, 32.12488555908203, 32.06965255737305, 32.0222282409668, 31.954957962036133, 31.903532028198242, 31.83578872680664, 32.51456832885742, 34.189456939697266, 31.12668228149414, 31.076339721679688, 31.047151565551758, 30.967018127441406, 30.956220626831055, 30.924589157104492, 30.893285751342773, 30.869199752807617, 30.843050003051758, 32.791847229003906, 30.809431076049805, 30.79128074645996, 30.779237747192383, 30.776460647583008, 30.74305534362793, 30.74994468688965, 30.7137393951416, 30.734609603881836, 30.719928741455078, 30.71673011779785, 49.99970626831055, 50.0, 49.987911224365234, 33.68583679199219, 31.76846694946289, 31.8026123046875, 31.802202224731445, 31.818490982055664, 31.85223960876465, 31.86141014099121, 31.906801223754883, 31.93423843383789, 31.964210510253906, 33.567230224609375, 32.055015563964844, 32.07001876831055, 32.13076400756836, 32.16000747680664, 32.22781753540039, 32.26890563964844, 32.323944091796875, 32.36326217651367, 32.430908203125, 49.980655670166016, 34.32135772705078, 33.09465789794922, 32.27247619628906, 32.33710479736328, 32.41763687133789, 32.498661041259766, 32.57213592529297, 32.67158126831055, 32.74591827392578, 32.814476013183594, 32.93477249145508, 33.04751968383789, 33.136863708496094, 33.23999786376953, 33.34675979614258, 33.42970657348633, 33.53573226928711, 33.66716003417969, 33.78378677368164, 33.905670166015625, 34.02836608886719, 34.151817321777344, 34.2794189453125, 34.41516876220703, 34.551273345947266, 34.702728271484375, 34.84151840209961, 34.986881256103516, 35.162757873535156, 35.332794189453125, 35.47941970825195, 35.65633010864258, 35.82624435424805, 36.00060272216797, 36.17729187011719, 36.36515808105469, 36.55763626098633, 36.744773864746094, 38.46407699584961, 37.869808197021484, 37.767921447753906, 37.958900451660156, 38.20857620239258, 38.38622283935547, 38.68323516845703, 38.871334075927734, 39.151519775390625, 39.377750396728516, 39.68268966674805, 39.89873123168945, 40.197330474853516, 40.47549819946289, 40.73743438720703, 41.04566955566406, 42.33650207519531, 41.92591094970703, 41.43912124633789, 41.045528411865234, 41.32114028930664, 41.581878662109375, 41.944580078125, 42.318912506103516, 42.6725959777832, 43.07264709472656, 43.443477630615234, 43.83216094970703, 44.19996643066406, 44.63225555419922, 45.06049346923828, 45.468536376953125, 45.89896774291992, 46.330604553222656, 46.778343200683594, 47.31666946411133, 47.789310455322266, 48.26376724243164, 48.826602935791016, 49.33188247680664, 49.990909576416016, 50.0, 50.0, 50.0, 50.0, 49.995697021484375, 49.99568176269531, 49.98163986206055, 50.0, 50.0, 49.9764289855957, 50.0, 50.0, 49.98639678955078, 49.99431228637695, 50.0, 50.0, 50.0, 50.0, 49.9874267578125, 50.0, 49.98714828491211, 50.0, 49.99470901489258, 49.99113464355469, 50.0, 50.0, 50.0, 49.985504150390625, 49.99067306518555, 50.0, 49.997161865234375, 50.0, 50.0, 50.0, 49.995513916015625, 49.993038177490234, 50.0, 49.99763107299805, 50.0, 49.98752975463867, 50.0, 49.988037109375, 50.0, 50.0, 50.0, 49.9975700378418, 50.0, 49.998756408691406, 49.97819519042969, 49.99104690551758, 49.99087905883789, 49.94268798828125, 49.85968017578125, 49.786617279052734, 49.70594787597656, 49.62562561035156, 49.56686782836914, 49.50475311279297, 49.416934967041016, 49.35699462890625, 49.308589935302734, 49.990482330322266, 50.0, 49.998836517333984, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.980472564697266, 49.99903869628906, 50.0, 50.0, 49.989845275878906, 49.98395919799805, 50.0, 49.99302673339844, 49.99530792236328, 49.99745559692383, 50.0, 49.99560546875, 21.569303512573242}); - LaserScan scan(ranges); + int scanSize = 1440; - std::cout << "scan created!" << std::endl; + falkolib::LaserScan scan1(0, 2.0 * M_PI, scanSize); - LineFinderIterative line_finder(LineFinderIterativeParams({1, 5, 1, 1})); + scan1.fromRanges(testRanges1); - std::cout << "line_finder created!" << std::endl; + CornerFalko2d CornerMatching; - std::list polyline_list; - line_finder.findPolylines(scan, scan_params, polyline_list); + CornerMatching.extract(scan1, CornerMatching.lastKeypointSet); - std::cout << polyline_list.size() << " polylines found!" << std::endl; + std::cout << "num keypoints1 extracted: " << CornerMatching.lastKeypointSet.size() << std::endl; } diff --git a/src/examples/testData.cpp b/src/examples/testData.cpp new file mode 100644 index 0000000..f71191c --- /dev/null +++ b/src/examples/testData.cpp @@ -0,0 +1,23 @@ +/** + * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant + * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. + * + * This file is part of FALKOLib. + * + * FALKOLib is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * FALKOLib is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with FALKOLib. If not, see . + */ + + double testRanges1[]= {250, 250, 250, 250, 250, 17.263999938964844, 17.263999938964844, 17.263999938964844, 17.263999938964844, 13.552000045776367, 13.552000045776367, 13.552000045776367, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.51200008392334, 11.567999839782715, 11.567999839782715, 10.720000267028809, 10.720000267028809, 10.720000267028809, 10.359999656677246, 10.336000442504883, 10.343999862670898, 10.35200023651123, 10.35200023651123, 10.32800006866455, 10.319999694824219, 10.383999824523926, 10.407999992370605, 10.432000160217285, 10.432000160217285, 10.447999954223633, 10.46399974822998, 10.46399974822998, 10.503999710083008, 10.503999710083008, 10.527999877929688, 10.592000007629395, 10.62399959564209, 8.368000030517578, 8.343999862670898, 8.343999862670898, 8.031999588012695, 7.831999778747559, 7.6479997634887695, 7.552000045776367, 7.415999889373779, 7.415999889373779, 7.263999938964844, 7.144000053405762, 7.0320000648498535, 6.920000076293945, 6.776000022888184, 6.776000022888184, 6.671999931335449, 6.576000213623047, 6.440000057220459, 6.328000068664551, 6.271999835968018, 6.271999835968018, 6.263999938964844, 6.271999835968018, 6.296000003814697, 6.320000171661377, 6.328000068664551, 6.328000068664551, 6.335999965667725, 6.320000171661377, 3.187999963760376, 3.187999963760376, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.504000186920166, 4.504000186920166, 4.504000186920166, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.599999904632568, 4.576000213623047, 4.495999813079834, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.4079999923706055, 4.368000030517578, 4.320000171661377, 4.271999835968018, 4.271999835968018, 4.223999977111816, 4.184000015258789, 4.168000221252441, 4.159999847412109, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.184000015258789, 4.199999809265137, 4.199999809265137, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.223999977111816, 4.23199987411499, 4.23199987411499, 4.176000118255615, 250, 250, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.099999904632568, 4.099999904632568, 4.099999904632568, 4.079999923706055, 4.0320000648498535, 3.98799991607666, 3.9719998836517334, 3.9719998836517334, 3.9560000896453857, 3.9240000247955322, 3.888000011444092, 3.859999895095825, 3.8320000171661377, 3.8320000171661377, 3.803999900817871, 3.7880001068115234, 3.259999990463257, 3.240000009536743, 3.2200000286102295, 3.2200000286102295, 3.200000047683716, 3.1760001182556152, 3.1559998989105225, 3.1440000534057617, 3.128000020980835, 3.128000020980835, 3.119999885559082, 2.5959999561309814, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.496000051498413, 2.5320000648498535, 250, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 250, 250, 250, 250, 250, 250, 250, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.640000104904175, 2.640000104904175, 2.640000104904175, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.555999994277954, 2.5439999103546143, 2.5320000648498535, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.503999948501587, 2.492000102996826, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4600000381469727, 2.4519999027252197, 2.440000057220459, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4079999923706055, 2.427999973297119, 2.444000005722046, 2.444000005722046, 2.4600000381469727, 2.4719998836517334, 2.48799991607666, 2.5, 2.5160000324249268, 2.5160000324249268, 2.5280001163482666, 2.5399999618530273, 2.559999942779541, 2.5799999237060547, 2.5840001106262207, 2.5840001106262207, 3.0480000972747803, 3.0480000972747803, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5959999561309814, 2.5920000076293945, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.572000026702881, 2.563999891281128, 2.555999994277954, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5399999618530273, 2.5360000133514404, 2.5320000648498535, 2.5239999294281006, 2.5199999809265137, 2.507999897003174, 2.507999897003174, 2.503999948501587, 2.5, 2.496000051498413, 2.492000102996826, 2.48799991607666, 2.48799991607666, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4639999866485596, 2.4560000896453857, 2.4560000896453857, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.444000005722046, 2.436000108718872, 2.436000108718872, 2.431999921798706, 2.427999973297119, 2.4240000247955322, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4040000438690186, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.3559999465942383, 2.3559999465942383, 2.3559999465942383, 2.6600000858306885, 2.6600000858306885, 2.6600000858306885, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.0, 2.0, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 2.0, 2.0, 2.009999990463257, 2.0220000743865967, 2.0460000038146973, 2.0460000038146973, 2.0260000228881836, 2.0260000228881836, 2.00600004196167, 1.996000051498413, 1.99399995803833, 1.9900000095367432, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.9980000257492065, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9919999837875366, 1.9520000219345093, 2.0320000648498535, 2.0999999046325684, 2.1040000915527344, 2.1040000915527344, 2.0280001163482666, 1.9040000438690186, 1.9279999732971191, 1.9520000219345093, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9600000381469727, 1.9620000123977661, 1.9639999866485596, 1.9639999866485596, 1.965999960899353, 1.965999960899353, 1.968000054359436, 1.972000002861023, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9800000190734863, 1.9839999675750732, 1.9800000190734863, 1.9800000190734863, 1.9819999933242798, 1.9880000352859497, 1.996000051498413, 2.009999990463257, 2.00600004196167, 2.00600004196167, 1.99399995803833, 2.687999963760376, 2.687999963760376, 2.687999963760376, 1.74399995803833, 1.74399995803833, 2.063999891281128, 2.115999937057495, 2.196000099182129, 2.191999912261963, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5199999809265137, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5399999618530273, 2.5480000972747803, 2.5480000972747803, 2.5480000972747803, 2.5399999618530273, 2.5439999103546143, 2.552000045776367, 2.555999994277954, 2.555999994277954, 2.552000045776367, 2.5480000972747803, 250, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3079999685287476, 1.2999999523162842, 1.2940000295639038, 1.2940000295639038, 1.2879999876022339, 1.2860000133514404, 1.2940000295639038, 1.3040000200271606, 1.3040000200271606, 1.3040000200271606, 1.2999999523162842, 1.2979999780654907, 1.3020000457763672, 1.309999942779541, 1.3140000104904175, 1.3140000104904175, 1.3140000104904175, 1.3240000009536743, 1.3359999656677246, 1.3420000076293945, 1.343999981880188, 1.343999981880188, 1.2200000286102295, 250, 250, 250, 250, 250, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.090000033378601, 1.090000033378601, 1.0920000076293945, 1.0980000495910645, 1.0980000495910645, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.1019999980926514, 1.1119999885559082, 1.121999979019165, 1.1260000467300415, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.776000022888184, 5.776000022888184, 5.776000022888184, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.736000061035156, 5.679999828338623, 5.671999931335449, 5.671999931335449, 5.631999969482422, 5.631999969482422, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.559999942779541, 5.584000110626221, 6.104000091552734, 6.783999919891357, 6.831999778747559, 6.831999778747559, 6.831999778747559, 6.760000228881836, 6.616000175476074, 6.455999851226807, 6.343999862670898, 6.343999862670898, 6.320000171661377, 6.311999797821045, 9.192000389099121, 9.175999641418457, 9.15999984741211, 9.15999984741211, 9.128000259399414, 9.119999885559082, 7.992000102996826, 7.9120001792907715, 7.8480000495910645, 7.8480000495910645, 7.800000190734863, 7.74399995803833, 7.71999979019165, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.71999979019165, 7.71999979019165, 7.711999893188477, 7.703999996185303, 7.703999996185303, 7.703999996185303, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.671999931335449, 8.67199993133545, 8.640000343322754, 8.607999801635742, 8.583999633789062, 8.583999633789062, 8.5600004196167, 8.552000045776367, 8.53600025177002, 8.520000457763672, 8.503999710083008, 8.503999710083008, 8.472000122070312, 8.447999954223633, 8.416000366210938, 8.383999824523926, 8.383999824523926, 8.383999824523926, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.336000442504883, 8.303999900817871, 8.303999900817871, 8.263999938964844, 8.239999771118164, 8.279999732971191, 8.392000198364258, 8.496000289916992, 8.496000289916992, 8.656000137329102, 8.807999610900879, 8.928000450134277, 9.015999794006348, 11.767999649047852, 11.767999649047852, 12.071999549865723, 12.175999641418457, 12.567999839782715, 12.663999557495117, 13.064000129699707, 13.064000129699707, 13.168000221252441, 13.607999801635742, 13.640000343322754, 13.53600025177002, 13.968000411987305, 13.968000411987305, 14.008000373840332, 14.104000091552734, 14.175999641418457, 14.104000091552734, 15.4399995803833, 15.4399995803833, 15.543999671936035, 15.48799991607666, 15.48799991607666, 15.503999710083008, 15.479999542236328, 15.479999542236328, 15.447999954223633, 15.399999618530273, 15.37600040435791, 15.35200023651123, 15.319999694824219, 15.319999694824219, 15.272000312805176, 15.272000312805176, 15.336000442504883, 15.303999900817871, 15.248000144958496, 15.248000144958496, 14.744000434875488, 15.303999900817871, 15.272000312805176, 15.248000144958496, 15.215999603271484, 15.223999977111816, 15.223999977111816, 15.248000144958496, 16.784000396728516, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.672000885009766, 16.687999725341797, 16.719999313354492, 16.70400047302246, 16.607999801635742, 16.607999801635742, 16.624000549316406, 16.6560001373291, 16.23200035095215, 16.143999099731445, 16.06399917602539, 16.06399917602539, 16.016000747680664, 17.472000122070312, 19.007999420166016, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250}; + + diff --git a/src/examples/testData.h b/src/examples/testData.h new file mode 100644 index 0000000..d0e07f0 --- /dev/null +++ b/src/examples/testData.h @@ -0,0 +1,28 @@ +/** + * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant + * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. + * + * This file is part of FALKOLib. + * + * FALKOLib is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * FALKOLib is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with FALKOLib. If not, see . + */ +#pragma once + +extern double testRanges1[]; + +extern double testRanges2[]; + +extern double testRangesOrtho1[]; + +extern double testRangesOrtho2[]; -- GitLab From 65e79c90ffdfb657704f7d2176b34e5a197cc0f8 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 28 Jan 2021 20:44:29 +0100 Subject: [PATCH 005/100] 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 keypointSet2; + extract(scan, keypointSet2); //Compute descriptors - lastDescriptorSet.clear(); - compute(scan, lastKeypointSet, lastDescriptorSet); + std::vector descriptorSet2; + compute(scan, keypointSet2, descriptorSet2); + + //Matching + + for (int i=0; i > 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 +class CornerFalko2d: public falkolib::FALKOExtractor, falkolib::BSCExtractor, falkolib::NNMatcher { 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> keypointSets; std::vector lastKeypointSet; std::vector> descriptorSets; std::vectorlastDescriptorSet; - //int _circularSectorNumber; - //int _radialRingNumber; - // Keypoints extractor - //falkolib::FALKOExtractor fe; - // BSC descriptor extractor - //falkolib::BSCExtractor 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 From 1abdf4abcb2ea1234f28c43bb22632b6df39422e Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Fri, 29 Jan 2021 10:54:03 +0100 Subject: [PATCH 006/100] findloopClosure function segmentation fault error corrected --- src/corner_falko_2d.cpp | 12 ++++++++---- src/corner_falko_2d.h | 2 ++ src/examples/corner_falko_demo.cpp | 2 ++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 8f1e6b7..deebc2d 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -42,6 +42,8 @@ void CornerFalko2d::storeCorners (falkolib::LaserScan scan, int scanInterval){ keypointSets.push_back(lastKeypointSet); descriptorSets.push_back(lastDescriptorSet); + std::cout << "sizeof : " << lastKeypointSet.size() << std::endl; + } } @@ -56,13 +58,15 @@ void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){ compute(scan, keypointSet2, descriptorSet2); //Matching + int rows = sizeof(keypointSets)/sizeof(keypointSets[0]); + int cols = sizeof(keypointSets)/(sizeof(int)*rows); - for (int i=0; i > assoNN; - int matchingNumber = match(keypointSets[i], keypointSet2, assoNN); - } - + matchingNumber = match(keypointSets[i], keypointSet2, assoNN); + } } diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 079db34..02c0474 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -77,6 +77,8 @@ public: std::vector> descriptorSets; std::vectorlastDescriptorSet; + int matchingNumber; + int scanNumber=0; }; diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index cda359c..2b94b98 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -35,4 +35,6 @@ int main(int argc, char** argv) CornerMatching.findLoopClosure(scan1); + std::cout << "matching number : " << CornerMatching.matchingNumber << std::endl; + } -- GitLab From eeb02a1b32df49b752003dd7b6b776f17ac94cae Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 1 Feb 2021 11:55:01 +0100 Subject: [PATCH 007/100] CMakeLists corrected, corner_falko_demo.cpp corrected --- src/CMakeLists.txt | 1 - src/corner_falko_2d.cpp | 4 ++-- src/corner_falko_2d.h | 2 ++ src/examples/corner_falko_demo.cpp | 10 +++++----- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 54c7fbd..9c9cf60 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -55,7 +55,6 @@ SET(HDRS point_set.h polyline.h scan_segment.h - corner_falko_2d.h ) IF(csm_FOUND) SET(HDRS ${HDRS} diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index deebc2d..8143a7d 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -19,6 +19,7 @@ CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, b setBRatio(4); setGridSectors(16); + } CornerFalko2d::~CornerFalko2d() @@ -42,7 +43,7 @@ void CornerFalko2d::storeCorners (falkolib::LaserScan scan, int scanInterval){ keypointSets.push_back(lastKeypointSet); descriptorSets.push_back(lastDescriptorSet); - std::cout << "sizeof : " << lastKeypointSet.size() << std::endl; + scansExtracted=scansExtracted+1; } @@ -65,7 +66,6 @@ void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){ setDistanceThreshold(0.1); std::vector > assoNN; matchingNumber = match(keypointSets[i], keypointSet2, assoNN); - } } diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 02c0474..5d90703 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -80,6 +80,8 @@ public: int matchingNumber; int scanNumber=0; + + int scansExtracted=0; }; } /* namespace laserscanutils */ diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index 2b94b98..8ebac1e 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -27,14 +27,14 @@ int main(int argc, char** argv) scan1.fromRanges(testRanges1); - CornerFalko2d CornerMatching; + CornerFalko2d cornerMatching; - CornerMatching.storeCorners(scan1, scanInterval); + cornerMatching.storeCorners(scan1, scanInterval); - std::cout << "num keypoints1 extracted: " << CornerMatching.lastKeypointSet.size() << std::endl; + std::cout << "num keypoints1 extracted: " << cornerMatching.lastKeypointSet.size() << std::endl; - CornerMatching.findLoopClosure(scan1); + cornerMatching.findLoopClosure(scan1); - std::cout << "matching number : " << CornerMatching.matchingNumber << std::endl; + std::cout << "matching number : " << cornerMatching.matchingNumber << std::endl; } -- GitLab From 416428f0f0f20b98bfd39da87bd5c6491643c0d6 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 1 Feb 2021 14:05:24 +0100 Subject: [PATCH 008/100] corrected public inheritance. Added init params to constructor --- src/corner_falko_2d.cpp | 7 ++++--- src/corner_falko_2d.h | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 8143a7d..661e18f 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -10,8 +10,8 @@ namespace laserscanutils{ CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, bool _useKeypointRadius, double _radius) :BSCExtractor( _circularSectorNumber, _radialRingNumber, _useKeypointRadius ,_radius ) { - // FALKO EXTRACTOR PARAMS - setMinExtractionRange(0.25); + // FALKO Extractor Parameters + setMinExtractionRange(0.1); setMaxExtractionRange(25); enableSubbeam(true); setNMSRadius(0.1); @@ -19,6 +19,8 @@ CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, b setBRatio(4); setGridSectors(16); + // Matcher Extractor Parameters + setDistanceThreshold(0.1); } @@ -63,7 +65,6 @@ void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){ int cols = sizeof(keypointSets)/(sizeof(int)*rows); for (int i=0; i > assoNN; matchingNumber = match(keypointSets[i], keypointSet2, assoNN); } diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 5d90703..5ce667e 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::NNMatcher +class CornerFalko2d: public falkolib::FALKOExtractor, public falkolib::BSCExtractor, public falkolib::NNMatcher { public: /** -- GitLab From e6ef7245738c8246ad6814b287207f6f427d5b66 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 1 Feb 2021 15:45:10 +0100 Subject: [PATCH 009/100] improved function cornerFalko2d->findLoopClosure --- src/corner_falko_2d.cpp | 23 +++++++++++++++++++---- src/corner_falko_2d.h | 2 ++ 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 661e18f..09bf36f 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -61,13 +61,28 @@ void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){ compute(scan, keypointSet2, descriptorSet2); //Matching - int rows = sizeof(keypointSets)/sizeof(keypointSets[0]); - int cols = sizeof(keypointSets)/(sizeof(int)*rows); + int rows = keypointSets.size(); + matchingNumber=0; + matchingPosition=-1; for (int i=0; i > assoNN; - matchingNumber = match(keypointSets[i], keypointSet2, assoNN); - } + + int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN); + + if (NewMatchingNumber > matchingNumber ) { + matchingNumber=NewMatchingNumber; + matchingPosition=i; + } + +/* for (auto& match : assoNN) { + if (match.second >= 0) { + int i1 = match.first; + int i2 = match.second; + std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl; + } + } + */ } } diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 5ce667e..8bc7639 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -82,6 +82,8 @@ public: int scanNumber=0; int scansExtracted=0; + + int matchingPosition=-1; }; } /* namespace laserscanutils */ -- GitLab From 0bac60c89f2113a0d342489cb79d8c725c50de72 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 1 Feb 2021 17:54:45 +0100 Subject: [PATCH 010/100] matching number threshold added --- src/corner_falko_2d.cpp | 2 +- src/corner_falko_2d.h | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 09bf36f..ba3c7f9 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -70,7 +70,7 @@ void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){ int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN); - if (NewMatchingNumber > matchingNumber ) { + if (NewMatchingNumber > matchingNumber && NewMatchingNumber>keypointsNumberTh) { matchingNumber=NewMatchingNumber; matchingPosition=i; } diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 8bc7639..0e355c1 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -72,11 +72,17 @@ public: **/ void findLoopClosure(falkolib::LaserScan scan); + /** @brief set euclidean distance threshold for keypoints distance measurements*/ + void setKeypointsNumberTh(int _th) { + keypointsNumberTh = _th; + } + std::vector> keypointSets; std::vector lastKeypointSet; std::vector> descriptorSets; std::vectorlastDescriptorSet; + int matchingNumber; int scanNumber=0; @@ -84,6 +90,8 @@ public: int scansExtracted=0; int matchingPosition=-1; + + int keypointsNumberTh=2; }; } /* namespace laserscanutils */ -- GitLab From 2d8ef428f9a71e98a02f752bcd3ebd580afea8d2 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Feb 2021 09:45:21 +0100 Subject: [PATCH 011/100] falkolib added as submodule --- .gitmodules | 3 +++ deps/falkolib | 1 + 2 files changed, 4 insertions(+) create mode 100644 .gitmodules create mode 160000 deps/falkolib diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..3d61e7a --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "deps/falkolib"] + path = deps/falkolib + url = https://gitlab.iri.upc.edu/mobile_robotics/falkolib diff --git a/deps/falkolib b/deps/falkolib new file mode 160000 index 0000000..327bafa --- /dev/null +++ b/deps/falkolib @@ -0,0 +1 @@ +Subproject commit 327bafa8ee1d9ea545d9766415986ea13a1064d1 -- GitLab From 464099aa6d3b499f5382b51523cfb3cef10b6831 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Feb 2021 10:02:53 +0100 Subject: [PATCH 012/100] README updated with falkolib compilation as submodule --- README.md | 42 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0e8f786..d929a07 100644 --- a/README.md +++ b/README.md @@ -39,4 +39,44 @@ sudo make install CSM depends on GSL that you probably have installed already. To install it: ``` sudo apt install libgsl-dev -``` \ No newline at end of file +``` + +**FALKO Library - falkolib** + +To enbale scan matching for loop closure purposes. The library is added as submodule in the deps folder. In order to initialize the submodule use the following commands: +``` +cd +git submodule init +git submodule update +``` +In order to compile and install the repository: +``` +cd +cd deps/openslam_falkolib/ +mkdir build +cd build +cmake .. +make +``` + +You can also install the library into a system directory. +To change the install directory you must set cmake environment +variable ${CMAKE_INSTALL_PREFIX} (e.g. using command "ccmake .." +before calling "cmake .."). +Its default value on UNIX-like/Linux systems is "/usr/local". +After compiling library falkolib, run the command: +``` +sudo make install +``` +The command "sudo" is required only if ${CMAKE_INSTALL_PREFIX} +is a system diretory managed by administrator user root. +Such command copies: +- header files of ${falkolib_ROOT}/include/falkolib to + ${CMAKE_INSTALL_PREFIX}/include/falkolib/ +- library files ${falkolib_ROOT}/lib/libfalkolib.a to + ${CMAKE_INSTALL_PREFIX}/lib/ +- cmake script ${falkolib_ROOT}/cmake_modules/falkolibConfig.cmake to + ${CMAKE_INSTALL_PREFIX}/share/falkolib/ + + + -- GitLab From e057cb00fe3c596a843ab02c4fef2e63bb633245 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Feb 2021 15:47:09 +0100 Subject: [PATCH 013/100] Changed input to cornerFalko2D to laserscanutils::LaserScan instead of falkolib:LaserScan --- src/corner_falko_2d.cpp | 24 ++++++++++++++++++------ src/corner_falko_2d.h | 13 +++++++++---- src/examples/corner_falko_demo.cpp | 15 ++++++++++----- 3 files changed, 37 insertions(+), 15 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index ba3c7f9..64b4b5a 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -29,18 +29,24 @@ CornerFalko2d::~CornerFalko2d() } -void CornerFalko2d::storeCorners (falkolib::LaserScan scan, int scanInterval){ +void CornerFalko2d::storeCorners (LaserScan scan,LaserScanParams scanParams, int scanInterval){ scanNumber=scanNumber+1; if (scanNumber % scanInterval == 0){ + + //Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object + falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); + scanFALKO.fromRanges(doubleRanges); + // Extract keypoints lastKeypointSet.clear(); - extract(scan, lastKeypointSet); + extract(scanFALKO, lastKeypointSet); //Compute descriptors lastDescriptorSet.clear(); - compute(scan, lastKeypointSet, lastDescriptorSet); + compute(scanFALKO, lastKeypointSet, lastDescriptorSet); keypointSets.push_back(lastKeypointSet); descriptorSets.push_back(lastDescriptorSet); @@ -51,14 +57,20 @@ void CornerFalko2d::storeCorners (falkolib::LaserScan scan, int scanInterval){ } -void CornerFalko2d::findLoopClosure(falkolib::LaserScan scan){ +void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ + + //Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object + falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); + scanFALKO.fromRanges(doubleRanges); + //Compute descriptors std::vector keypointSet2; - extract(scan, keypointSet2); + extract(scanFALKO, keypointSet2); //Compute descriptors std::vector descriptorSet2; - compute(scan, keypointSet2, descriptorSet2); + compute(scanFALKO, keypointSet2, descriptorSet2); //Matching int rows = keypointSets.size(); diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 0e355c1..ffcff43 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -11,6 +11,11 @@ #include #include +/************************** + * laser_scan_utils includes * + **************************/ +#include "laser_scan.h" + /************************** * WOLF includes * **************************/ @@ -58,11 +63,11 @@ public: * Destructor * **/ - ~CornerFalko2d(); + ~CornerFalko2d(); /** \brief Gets a set of landmarks/scenes to use as trained set. **/ - void storeCorners(falkolib::LaserScan scan, int scanInterval); + void storeCorners(LaserScan scan, LaserScanParams scanParams, int scanInterval); /** \brief Extract landmark/scene (list of corners) from a given 2D scan **/ @@ -70,9 +75,9 @@ public: /** \brief compare new scans against the training set in order to find loop closures **/ - void findLoopClosure(falkolib::LaserScan scan); + void findLoopClosure(LaserScan scan,LaserScanParams scanParams); - /** @brief set euclidean distance threshold for keypoints distance measurements*/ + /** @brief set a threshold for the minimum number of matched keypoints to consider a matching correct*/ void setKeypointsNumberTh(int _th) { keypointsNumberTh = _th; } diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index 8ebac1e..4a969bd 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -23,18 +23,23 @@ int main(int argc, char** argv) int scanSize = 1440; int scanInterval =1; - falkolib::LaserScan scan1(0, 2.0 * M_PI, scanSize); + LaserScan scan; + LaserScanParams laserParams; - scan1.fromRanges(testRanges1); + laserParams.angle_min_=0; + laserParams.angle_max_=2.0 * M_PI; + + for (int i=0; i Date: Thu, 4 Feb 2021 16:59:15 +0100 Subject: [PATCH 014/100] refactor. function created convert2LaserScanFALKO. Added line in CMakelists to solve eigen Error --- src/CMakeLists.txt | 1 + src/corner_falko_2d.cpp | 23 +++++++++++++---------- src/corner_falko_2d.h | 5 ++++- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9c9cf60..b187780 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -31,6 +31,7 @@ IF(faramotics_FOUND) ENDIF(faramotics_FOUND) if(${falkolib_FOUND}) + add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) include_directories(${falkolib_INCLUDE_DIRS}) link_directories(${falkolib_LIBRARY_DIRS}) endif() diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 64b4b5a..5c1193a 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -34,11 +34,8 @@ void CornerFalko2d::storeCorners (LaserScan scan,LaserScanParams scanParams, int scanNumber=scanNumber+1; if (scanNumber % scanInterval == 0){ - - //Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object - falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); - scanFALKO.fromRanges(doubleRanges); + + falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); // Extract keypoints lastKeypointSet.clear(); @@ -59,11 +56,8 @@ void CornerFalko2d::storeCorners (LaserScan scan,LaserScanParams scanParams, int void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ - //Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object - falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); - scanFALKO.fromRanges(doubleRanges); - + falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); + //Compute descriptors std::vector keypointSet2; extract(scanFALKO, keypointSet2); @@ -98,6 +92,15 @@ void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ } +falkolib::LaserScan CornerFalko2d::convert2LaserScanFALKO(LaserScan scan,LaserScanParams scanParams){ + + falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); + scanFALKO.fromRanges(doubleRanges); + return scanFALKO; + +} + } // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index ffcff43..4d707cd 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -71,7 +71,6 @@ public: /** \brief Extract landmark/scene (list of corners) from a given 2D scan **/ - //void extract(falkolib::LaserScan scan); /** \brief compare new scans against the training set in order to find loop closures **/ @@ -82,6 +81,10 @@ public: keypointsNumberTh = _th; } + /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object + **/ + falkolib::LaserScan convert2LaserScanFALKO(LaserScan scan,LaserScanParams scanParams); + std::vector> keypointSets; std::vector lastKeypointSet; std::vector> descriptorSets; -- GitLab From 0c88cc695ed0316f82191ac860813f9e3fb68632 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 4 Feb 2021 17:39:17 +0100 Subject: [PATCH 015/100] scene candidate evaluation added --- src/corner_falko_2d.cpp | 40 ++++++++++++++++-------------- src/corner_falko_2d.h | 16 +++++++++--- src/examples/corner_falko_demo.cpp | 2 +- 3 files changed, 35 insertions(+), 23 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 5c1193a..c48b928 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -29,13 +29,7 @@ CornerFalko2d::~CornerFalko2d() } -void CornerFalko2d::storeCorners (LaserScan scan,LaserScanParams scanParams, int scanInterval){ - - scanNumber=scanNumber+1; - - if (scanNumber % scanInterval == 0){ - - falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); +void CornerFalko2d::AddNewReferenceScene (falkolib::LaserScan scanFALKO){ // Extract keypoints lastKeypointSet.clear(); @@ -50,8 +44,6 @@ void CornerFalko2d::storeCorners (LaserScan scan,LaserScanParams scanParams, int scansExtracted=scansExtracted+1; - } - } void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ @@ -80,15 +72,7 @@ void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ matchingNumber=NewMatchingNumber; matchingPosition=i; } - -/* for (auto& match : assoNN) { - if (match.second >= 0) { - int i1 = match.first; - int i2 = match.second; - std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl; - } - } - */ } + } } @@ -101,6 +85,26 @@ falkolib::LaserScan CornerFalko2d::convert2LaserScanFALKO(LaserScan scan,LaserSc } +int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scanParams, int scanInterval){ + + scanNumber=scanNumber+1; + + int NewSceneAdded=0; + + if (scanNumber % scanInterval == 0){ + + falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); + + AddNewReferenceScene(scanFALKO); + + NewSceneAdded=1; + + } + + return NewSceneAdded; + +} + } // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 4d707cd..bdab934 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -48,6 +48,11 @@ namespace laserscanutils class CornerFalko2d: public falkolib::FALKOExtractor, public falkolib::BSCExtractor, public falkolib::NNMatcher { +private: + /** \brief Get and stores a scene to use as trained/Reference set of keypoints. + **/ + void AddNewReferenceScene (falkolib::LaserScan scanFALKO); + public: /** * @brief Constructor @@ -65,10 +70,6 @@ public: **/ ~CornerFalko2d(); - /** \brief Gets a set of landmarks/scenes to use as trained set. - **/ - void storeCorners(LaserScan scan, LaserScanParams scanParams, int scanInterval); - /** \brief Extract landmark/scene (list of corners) from a given 2D scan **/ @@ -85,6 +86,13 @@ public: **/ 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> keypointSets; std::vector lastKeypointSet; std::vector> descriptorSets; diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index 4a969bd..e9af47f 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -35,7 +35,7 @@ int main(int argc, char** argv) CornerFalko2d cornerMatching; - cornerMatching.storeCorners(scan, laserParams, scanInterval); + cornerMatching.evaluateNewReferenceScene(scan, laserParams, scanInterval); std::cout << "num keypoints1 extracted: " << cornerMatching.lastKeypointSet.size() << std::endl; -- GitLab From cc9c706110615ac926ec42b3ba1374192aea6d04 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sat, 6 Feb 2021 11:56:42 +0100 Subject: [PATCH 016/100] evaluateNewRefScene function modified --- src/corner_falko_2d.cpp | 13 ++++++++----- src/corner_falko_2d.h | 3 +++ src/examples/corner_falko_demo.cpp | 6 +++++- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index c48b928..8bd0c3a 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -91,14 +91,18 @@ int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scan int NewSceneAdded=0; - if (scanNumber % scanInterval == 0){ + if (scanNumber % scanInterval == 0 || scanNumber == 1){ - falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); + findLoopClosure(scan, scanParams); - AddNewReferenceScene(scanFALKO); + if (matchingNumber < refSceneAddingTh) { - NewSceneAdded=1; + falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); + AddNewReferenceScene(scanFALKO); + + NewSceneAdded=1; + } } return NewSceneAdded; @@ -106,5 +110,4 @@ int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scan } - } // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index bdab934..a3a3048 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -108,6 +108,9 @@ public: 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 =6; }; } /* namespace laserscanutils */ diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index e9af47f..30358e4 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -35,10 +35,14 @@ int main(int argc, char** argv) CornerFalko2d cornerMatching; - cornerMatching.evaluateNewReferenceScene(scan, laserParams, scanInterval); + int sceneAdded = cornerMatching.evaluateNewReferenceScene(scan, laserParams, scanInterval); std::cout << "num keypoints1 extracted: " << cornerMatching.lastKeypointSet.size() << std::endl; + if (sceneAdded==1){ + std::cout << "NewRefSceneAdded!!! " << std::endl; + } + cornerMatching.findLoopClosure(scan, laserParams); std::cout << "matching number : " << cornerMatching.matchingNumber << std::endl; -- GitLab From ace29c24d71f0c61880447bcbe3377759124e006 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 8 Feb 2021 10:44:24 +0100 Subject: [PATCH 017/100] added the prohibition of two matches for scan --- src/corner_falko_2d.cpp | 16 ++++++++++++---- src/corner_falko_2d.h | 5 ++++- src/examples/corner_falko_demo.cpp | 2 +- 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 8bd0c3a..426370e 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -61,18 +61,26 @@ void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ //Matching int rows = keypointSets.size(); - matchingNumber=0; + numberKeypointsMatch=0; + numberSceneMatch=0; matchingPosition=-1; for (int i=0; i > assoNN; int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN); - if (NewMatchingNumber > matchingNumber && NewMatchingNumber>keypointsNumberTh) { - matchingNumber=NewMatchingNumber; + if (NewMatchingNumber > numberKeypointsMatch && NewMatchingNumber>keypointsNumberTh) { + numberKeypointsMatch=NewMatchingNumber; matchingPosition=i; + numberSceneMatch=numberSceneMatch+1; + } } + if (numberSceneMatch>1){ + numberKeypointsMatch=0; + numberSceneMatch=0; + matchingPosition=-1; + } } @@ -95,7 +103,7 @@ int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scan findLoopClosure(scan, scanParams); - if (matchingNumber < refSceneAddingTh) { + if (numberKeypointsMatch < refSceneAddingTh) { falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index a3a3048..95dd564 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -98,8 +98,11 @@ public: std::vector> descriptorSets; std::vectorlastDescriptorSet; + // Number of Scene matched when performing the loop closure + int numberKeypointsMatch=0; - int matchingNumber; + //Number of keypoints that are matched between 2 scenes when performing the loop closure + int numberSceneMatch=0; int scanNumber=0; diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index 30358e4..8baa356 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -45,5 +45,5 @@ int main(int argc, char** argv) cornerMatching.findLoopClosure(scan, laserParams); - std::cout << "matching number : " << cornerMatching.matchingNumber << std::endl; + std::cout << "matching number : " << cornerMatching.numberKeypointsMatch << std::endl; } -- GitLab From 60bd46d282a2d5f6d8d52fde283926dfac796651 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 8 Feb 2021 11:10:03 +0100 Subject: [PATCH 018/100] Scene detection th modified --- src/corner_falko_2d.cpp | 1 - src/corner_falko_2d.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 426370e..6f105e8 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -77,7 +77,6 @@ void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){ } } if (numberSceneMatch>1){ - numberKeypointsMatch=0; numberSceneMatch=0; matchingPosition=-1; } diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 95dd564..3c35597 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -113,7 +113,7 @@ public: int keypointsNumberTh=2; // Max number of matched keypoints between 2 scenes for the candidate scene be considered a good New reference scene - int refSceneAddingTh =6; + int refSceneAddingTh =4; }; } /* namespace laserscanutils */ -- GitLab From 837e56b72065b532295383d1f0fde29941458a4c Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 9 Feb 2021 14:21:45 +0100 Subject: [PATCH 019/100] class loopClosureBase2d created --- src/corner_falko_2d.h | 12 +----------- src/loop_closure_base_2d.cpp | 0 src/loop_closure_base_2d.h | 34 ++++++++++++++++++++++++++++++++++ 3 files changed, 35 insertions(+), 11 deletions(-) create mode 100644 src/loop_closure_base_2d.cpp create mode 100644 src/loop_closure_base_2d.h diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index 3c35597..746e4e1 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -40,11 +40,7 @@ namespace laserscanutils { /** \brief A 2D corner extractor and loop closure computing class - * - * - * - * - */ +**/ class CornerFalko2d: public falkolib::FALKOExtractor, public falkolib::BSCExtractor, public falkolib::NNMatcher { @@ -64,15 +60,9 @@ public: CornerFalko2d(int _circularSectorNumber=16, int _radialRingNumber=8, bool _useKeypointRadius = true, double _radius = 0.1) ; /** \brief Destructor - * - * Destructor - * **/ ~CornerFalko2d(); - /** \brief Extract landmark/scene (list of corners) from a given 2D scan - **/ - /** \brief compare new scans against the training set in order to find loop closures **/ void findLoopClosure(LaserScan scan,LaserScanParams scanParams); diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base_2d.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h new file mode 100644 index 0000000..0c36cf1 --- /dev/null +++ b/src/loop_closure_base_2d.h @@ -0,0 +1,34 @@ +/** + * \file loop_closure_base_2d.h + * + * Created on: Feb 9, 2021 + * \author: spujol + */ + +#ifndef LOOP_CLOSURE_BASE_2D_H_ +#define LOOP_CLOSURE_BASE_2D_H_ + +#include +#include + +namespace laserscanutils +{ + /** \brief A 2base class for loop closure using falko library +*/ +class loopClosureBase2d{ +private: + +public: + /** \brief Constructor + **/ + + loopClosureBase2d(); + + /** \brief Destructor + **/ + ~loopClosureBase2d(); + +}; +} /* namespace laserscanutils */ + +#endif /* LANDMARK_POLYLINE_2d_H_ */ \ No newline at end of file -- GitLab From b7ed32b136f5a9c96b437435937495a2e0c2b712 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 9 Feb 2021 15:04:21 +0100 Subject: [PATCH 020/100] added cornerScene struct --- src/CMakeLists.txt | 8 ++++++-- src/corner_scene.cpp | 0 src/corner_scene.h | 23 +++++++++++++++++++++++ src/loop_closure_base_2d.cpp | 16 ++++++++++++++++ src/loop_closure_base_2d.h | 23 +++++++++++++++++++++-- 5 files changed, 66 insertions(+), 4 deletions(-) create mode 100644 src/corner_scene.cpp create mode 100644 src/corner_scene.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b187780..cec2f98 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -56,6 +56,7 @@ SET(HDRS point_set.h polyline.h scan_segment.h + loop_closure_base_2d.h ) IF(csm_FOUND) SET(HDRS ${HDRS} @@ -64,7 +65,8 @@ SET(HDRS IF(falkolib_FOUND) SET(HDRS ${HDRS} - corner_falko_2d.h) + corner_falko_2d.h + corner_scene.h) ENDIF(falkolib_FOUND) #sources @@ -84,6 +86,7 @@ SET(SRCS point_set.cpp polyline.cpp scan_segment.cpp + loop_closure_base_2d.cpp ) IF(csm_FOUND) SET(SRCS ${SRCS} @@ -92,7 +95,8 @@ SET(SRCS IF(falkolib_FOUND) SET(SRCS ${SRCS} - corner_falko_2d.cpp) + corner_falko_2d.cpp + corner_scene.cpp) ENDIF(falkolib_FOUND) diff --git a/src/corner_scene.cpp b/src/corner_scene.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/corner_scene.h b/src/corner_scene.h new file mode 100644 index 0000000..a2cd43c --- /dev/null +++ b/src/corner_scene.h @@ -0,0 +1,23 @@ +/** + * \file corner_scene.h + * + * Created on: Feb 9, 2021 + * \author: spujol + */ + +#ifndef CORNER_SCENE_H_ +#define CORNER_SCENE_H_ + +#include +#include + +namespace laserscanutils +{ +struct cornerScene{ + std::vector keypointsList; + std::vector descriptorsList; +}; + +} /* namespace laserscanutils */ + +#endif /* CORNER_SCENE_H_ */ \ No newline at end of file diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base_2d.cpp index e69de29..1693e91 100644 --- a/src/loop_closure_base_2d.cpp +++ b/src/loop_closure_base_2d.cpp @@ -0,0 +1,16 @@ +/** + * \file loop_closure_base_2d.h + * + * Created on: Feb 9, 2021 + * \author: spujol + */ + +# include "loop_closure_base_2d.h" + +namespace laserscanutils{ + + cornerScene extractScene(LaserScan scan,LaserScanParams scanParams){ + cornerScene NewScene; + return NewScene; + } +} \ No newline at end of file diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index 0c36cf1..73edd7d 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -11,8 +11,20 @@ #include #include +/************************** + * laser_scan_utils includes * + **************************/ +#include "laser_scan.h" +#include "corner_scene.h" + namespace laserscanutils { + /* +struct cornerScene{ + std::vector keypointsList; + std::vector descriptorsList; +};*/ + /** \brief A 2base class for loop closure using falko library */ class loopClosureBase2d{ @@ -21,14 +33,21 @@ private: public: /** \brief Constructor **/ - loopClosureBase2d(); /** \brief Destructor **/ ~loopClosureBase2d(); + /** \brief compare new scans against the trained set in order to find loop closures + **/ + virtual void findLoopClosure(); + + /** \brief update the scene struct with keypoints and descriptors + **/ + virtual cornerScene extractScene(LaserScan scan,LaserScanParams scanParams); + }; } /* namespace laserscanutils */ -#endif /* LANDMARK_POLYLINE_2d_H_ */ \ No newline at end of file +#endif /* LOOP_CLOSURE_BASE_2D_H_ */ \ No newline at end of file -- GitLab From 1f0390d9ad3d32978130ef214181f5544accd9ac Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 9 Feb 2021 15:31:42 +0100 Subject: [PATCH 021/100] added shared ptr for scene extraction --- src/loop_closure_base_2d.cpp | 4 ++-- src/loop_closure_base_2d.h | 10 +++------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base_2d.cpp index 1693e91..6642d75 100644 --- a/src/loop_closure_base_2d.cpp +++ b/src/loop_closure_base_2d.cpp @@ -9,8 +9,8 @@ namespace laserscanutils{ - cornerScene extractScene(LaserScan scan,LaserScanParams scanParams){ - cornerScene NewScene; + std::shared_ptr loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + std::shared_ptr NewScene; return NewScene; } } \ No newline at end of file diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index 73edd7d..8433d88 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -10,6 +10,7 @@ #include #include +#include /************************** * laser_scan_utils includes * @@ -19,11 +20,6 @@ namespace laserscanutils { - /* -struct cornerScene{ - std::vector keypointsList; - std::vector descriptorsList; -};*/ /** \brief A 2base class for loop closure using falko library */ @@ -41,11 +37,11 @@ public: /** \brief compare new scans against the trained set in order to find loop closures **/ - virtual void findLoopClosure(); + virtual void findLoopClosure(std::list& scenes, const cornerScene newScene); /** \brief update the scene struct with keypoints and descriptors **/ - virtual cornerScene extractScene(LaserScan scan,LaserScanParams scanParams); + virtual std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams); }; } /* namespace laserscanutils */ -- GitLab From e24b92e251650852f613435cb20e5f4ab2d3ada7 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 9 Feb 2021 15:57:21 +0100 Subject: [PATCH 022/100] added convert2LaserScanFALKO --- src/loop_closure_base_2d.cpp | 14 ++++++++++++++ src/loop_closure_base_2d.h | 19 +++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base_2d.cpp index 6642d75..37d3b0e 100644 --- a/src/loop_closure_base_2d.cpp +++ b/src/loop_closure_base_2d.cpp @@ -10,7 +10,21 @@ namespace laserscanutils{ std::shared_ptr loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + + + std::shared_ptr NewScene; + return NewScene; } +/* + std::shared_ptr loopClosureBase2d::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ + //falkolib::laserScan scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + //std::make_shared scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + //std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); + //scanFALKO.fromRanges(doubleRanges); + std::shared_ptr scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + return scanFALKO; + } +*/ } \ No newline at end of file diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index 8433d88..96bc952 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -18,6 +18,20 @@ #include "laser_scan.h" #include "corner_scene.h" +/************************** + * Falko includes * + **************************/ +#include +#include +#include +#include + +#include +#include + +#include +#include + namespace laserscanutils { @@ -43,6 +57,11 @@ public: **/ virtual std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams); + /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object + **/ + std::shared_ptr convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams); + + }; } /* namespace laserscanutils */ -- GitLab From c5c16e6ffebdb2214b3dbcf23c578457a0e778f6 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 09:25:54 +0100 Subject: [PATCH 023/100] added gtest for scan2scanFALKO --- src/loop_closure_base_2d.cpp | 19 +++++++------ src/loop_closure_base_2d.h | 4 +-- test/CMakeLists.txt | 11 +++++++- test/gtest_loop_closure_base_2d.cpp | 41 +++++++++++++++++++++++++++++ test/testData2.cpp | 23 ++++++++++++++++ test/testData2.h | 28 ++++++++++++++++++++ 6 files changed, 113 insertions(+), 13 deletions(-) create mode 100644 test/gtest_loop_closure_base_2d.cpp create mode 100644 test/testData2.cpp create mode 100644 test/testData2.h diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base_2d.cpp index 37d3b0e..bc79c6b 100644 --- a/src/loop_closure_base_2d.cpp +++ b/src/loop_closure_base_2d.cpp @@ -9,22 +9,21 @@ namespace laserscanutils{ - std::shared_ptr loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ - + loopClosureBase2d::loopClosureBase2d(){} + loopClosureBase2d::~loopClosureBase2d(){} + std::shared_ptr loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ std::shared_ptr NewScene; return NewScene; } -/* + std::shared_ptr loopClosureBase2d::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ - //falkolib::laserScan scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - //std::make_shared scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - //std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); - //scanFALKO.fromRanges(doubleRanges); - std::shared_ptr scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + auto scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); + ((*scanFALKO).fromRanges)(doubleRanges); return scanFALKO; } -*/ -} \ No newline at end of file + +} diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index 96bc952..3a2dfd1 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -51,7 +51,7 @@ public: /** \brief compare new scans against the trained set in order to find loop closures **/ - virtual void findLoopClosure(std::list& scenes, const cornerScene newScene); + //virtual void findLoopClosure(std::list& scenes, const cornerScene newScene); /** \brief update the scene struct with keypoints and descriptors **/ @@ -65,4 +65,4 @@ public: }; } /* namespace laserscanutils */ -#endif /* LOOP_CLOSURE_BASE_2D_H_ */ \ No newline at end of file +#endif /* LOOP_CLOSURE_BASE_2D_H_ */ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6cd8581..386aea8 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -4,6 +4,12 @@ add_subdirectory(gtest) # Include gtest directory. include_directories(${GTEST_INCLUDE_DIRS}) +#Include directories +INCLUDE_DIRECTORIES(../src) +INCLUDE_DIRECTORIES(../test) +FIND_PACKAGE(Eigen3 3.3 REQUIRED) +INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) + ############# USE THIS TEST AS AN EXAMPLE #################### # # # Create a specific test executable for gtest_example # @@ -14,4 +20,7 @@ include_directories(${GTEST_INCLUDE_DIRS}) # gtest example gnss_utils_add_gtest(gtest_example gtest_example.cpp) -target_link_libraries(gtest_example ${PROJECT_NAME}) \ No newline at end of file +target_link_libraries(gtest_example ${PROJECT_NAME}) + +gnss_utils_add_gtest(gtest_loop_closure_base_2d gtest_loop_closure_base_2d.cpp ${PROJECT_SOURCE_DIR}/test/testData2.cpp) +target_link_libraries(gtest_loop_closure_base_2d ${PROJECT_NAME}) diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp new file mode 100644 index 0000000..d60cab9 --- /dev/null +++ b/test/gtest_loop_closure_base_2d.cpp @@ -0,0 +1,41 @@ +#include "gtest/utils_gtest.h" +#include "testData2.h" +#include "../src/loop_closure_base_2d.h" +//#include "laser_scan_utils.h" + +using namespace laserscanutils; + +TEST(loop_closure_base_2d, convert2laserScanFalko) +{ + int scanSize = 1440; + + LaserScan scan; + LaserScanParams laserParams; + + laserParams.angle_min_=0; + laserParams.angle_max_=2.0 * M_PI; + + for (int i=0; i scanFALKO=LCBase.convert2LaserScanFALKO(scan, laserParams); + + int firstPoint = ((*scanFALKO).ranges)[0]; + + EXPECT_FALSE(false); + + ASSERT_TRUE(true); + + ASSERT_EQ(firstPoint, 250); + +// PRINTF("All good at TestTest::DummyTestExample !\n"); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/testData2.cpp b/test/testData2.cpp new file mode 100644 index 0000000..f71191c --- /dev/null +++ b/test/testData2.cpp @@ -0,0 +1,23 @@ +/** + * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant + * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. + * + * This file is part of FALKOLib. + * + * FALKOLib is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * FALKOLib is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with FALKOLib. If not, see . + */ + + double testRanges1[]= {250, 250, 250, 250, 250, 17.263999938964844, 17.263999938964844, 17.263999938964844, 17.263999938964844, 13.552000045776367, 13.552000045776367, 13.552000045776367, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.51200008392334, 11.567999839782715, 11.567999839782715, 10.720000267028809, 10.720000267028809, 10.720000267028809, 10.359999656677246, 10.336000442504883, 10.343999862670898, 10.35200023651123, 10.35200023651123, 10.32800006866455, 10.319999694824219, 10.383999824523926, 10.407999992370605, 10.432000160217285, 10.432000160217285, 10.447999954223633, 10.46399974822998, 10.46399974822998, 10.503999710083008, 10.503999710083008, 10.527999877929688, 10.592000007629395, 10.62399959564209, 8.368000030517578, 8.343999862670898, 8.343999862670898, 8.031999588012695, 7.831999778747559, 7.6479997634887695, 7.552000045776367, 7.415999889373779, 7.415999889373779, 7.263999938964844, 7.144000053405762, 7.0320000648498535, 6.920000076293945, 6.776000022888184, 6.776000022888184, 6.671999931335449, 6.576000213623047, 6.440000057220459, 6.328000068664551, 6.271999835968018, 6.271999835968018, 6.263999938964844, 6.271999835968018, 6.296000003814697, 6.320000171661377, 6.328000068664551, 6.328000068664551, 6.335999965667725, 6.320000171661377, 3.187999963760376, 3.187999963760376, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.504000186920166, 4.504000186920166, 4.504000186920166, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.599999904632568, 4.576000213623047, 4.495999813079834, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.4079999923706055, 4.368000030517578, 4.320000171661377, 4.271999835968018, 4.271999835968018, 4.223999977111816, 4.184000015258789, 4.168000221252441, 4.159999847412109, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.184000015258789, 4.199999809265137, 4.199999809265137, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.223999977111816, 4.23199987411499, 4.23199987411499, 4.176000118255615, 250, 250, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.099999904632568, 4.099999904632568, 4.099999904632568, 4.079999923706055, 4.0320000648498535, 3.98799991607666, 3.9719998836517334, 3.9719998836517334, 3.9560000896453857, 3.9240000247955322, 3.888000011444092, 3.859999895095825, 3.8320000171661377, 3.8320000171661377, 3.803999900817871, 3.7880001068115234, 3.259999990463257, 3.240000009536743, 3.2200000286102295, 3.2200000286102295, 3.200000047683716, 3.1760001182556152, 3.1559998989105225, 3.1440000534057617, 3.128000020980835, 3.128000020980835, 3.119999885559082, 2.5959999561309814, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.496000051498413, 2.5320000648498535, 250, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 250, 250, 250, 250, 250, 250, 250, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.640000104904175, 2.640000104904175, 2.640000104904175, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.555999994277954, 2.5439999103546143, 2.5320000648498535, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.503999948501587, 2.492000102996826, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4600000381469727, 2.4519999027252197, 2.440000057220459, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4079999923706055, 2.427999973297119, 2.444000005722046, 2.444000005722046, 2.4600000381469727, 2.4719998836517334, 2.48799991607666, 2.5, 2.5160000324249268, 2.5160000324249268, 2.5280001163482666, 2.5399999618530273, 2.559999942779541, 2.5799999237060547, 2.5840001106262207, 2.5840001106262207, 3.0480000972747803, 3.0480000972747803, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5959999561309814, 2.5920000076293945, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.572000026702881, 2.563999891281128, 2.555999994277954, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5399999618530273, 2.5360000133514404, 2.5320000648498535, 2.5239999294281006, 2.5199999809265137, 2.507999897003174, 2.507999897003174, 2.503999948501587, 2.5, 2.496000051498413, 2.492000102996826, 2.48799991607666, 2.48799991607666, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4639999866485596, 2.4560000896453857, 2.4560000896453857, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.444000005722046, 2.436000108718872, 2.436000108718872, 2.431999921798706, 2.427999973297119, 2.4240000247955322, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4040000438690186, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.3559999465942383, 2.3559999465942383, 2.3559999465942383, 2.6600000858306885, 2.6600000858306885, 2.6600000858306885, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.0, 2.0, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 2.0, 2.0, 2.009999990463257, 2.0220000743865967, 2.0460000038146973, 2.0460000038146973, 2.0260000228881836, 2.0260000228881836, 2.00600004196167, 1.996000051498413, 1.99399995803833, 1.9900000095367432, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.9980000257492065, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9919999837875366, 1.9520000219345093, 2.0320000648498535, 2.0999999046325684, 2.1040000915527344, 2.1040000915527344, 2.0280001163482666, 1.9040000438690186, 1.9279999732971191, 1.9520000219345093, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9600000381469727, 1.9620000123977661, 1.9639999866485596, 1.9639999866485596, 1.965999960899353, 1.965999960899353, 1.968000054359436, 1.972000002861023, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9800000190734863, 1.9839999675750732, 1.9800000190734863, 1.9800000190734863, 1.9819999933242798, 1.9880000352859497, 1.996000051498413, 2.009999990463257, 2.00600004196167, 2.00600004196167, 1.99399995803833, 2.687999963760376, 2.687999963760376, 2.687999963760376, 1.74399995803833, 1.74399995803833, 2.063999891281128, 2.115999937057495, 2.196000099182129, 2.191999912261963, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5199999809265137, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5399999618530273, 2.5480000972747803, 2.5480000972747803, 2.5480000972747803, 2.5399999618530273, 2.5439999103546143, 2.552000045776367, 2.555999994277954, 2.555999994277954, 2.552000045776367, 2.5480000972747803, 250, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3079999685287476, 1.2999999523162842, 1.2940000295639038, 1.2940000295639038, 1.2879999876022339, 1.2860000133514404, 1.2940000295639038, 1.3040000200271606, 1.3040000200271606, 1.3040000200271606, 1.2999999523162842, 1.2979999780654907, 1.3020000457763672, 1.309999942779541, 1.3140000104904175, 1.3140000104904175, 1.3140000104904175, 1.3240000009536743, 1.3359999656677246, 1.3420000076293945, 1.343999981880188, 1.343999981880188, 1.2200000286102295, 250, 250, 250, 250, 250, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.090000033378601, 1.090000033378601, 1.0920000076293945, 1.0980000495910645, 1.0980000495910645, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.1019999980926514, 1.1119999885559082, 1.121999979019165, 1.1260000467300415, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.776000022888184, 5.776000022888184, 5.776000022888184, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.736000061035156, 5.679999828338623, 5.671999931335449, 5.671999931335449, 5.631999969482422, 5.631999969482422, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.559999942779541, 5.584000110626221, 6.104000091552734, 6.783999919891357, 6.831999778747559, 6.831999778747559, 6.831999778747559, 6.760000228881836, 6.616000175476074, 6.455999851226807, 6.343999862670898, 6.343999862670898, 6.320000171661377, 6.311999797821045, 9.192000389099121, 9.175999641418457, 9.15999984741211, 9.15999984741211, 9.128000259399414, 9.119999885559082, 7.992000102996826, 7.9120001792907715, 7.8480000495910645, 7.8480000495910645, 7.800000190734863, 7.74399995803833, 7.71999979019165, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.71999979019165, 7.71999979019165, 7.711999893188477, 7.703999996185303, 7.703999996185303, 7.703999996185303, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.671999931335449, 8.67199993133545, 8.640000343322754, 8.607999801635742, 8.583999633789062, 8.583999633789062, 8.5600004196167, 8.552000045776367, 8.53600025177002, 8.520000457763672, 8.503999710083008, 8.503999710083008, 8.472000122070312, 8.447999954223633, 8.416000366210938, 8.383999824523926, 8.383999824523926, 8.383999824523926, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.336000442504883, 8.303999900817871, 8.303999900817871, 8.263999938964844, 8.239999771118164, 8.279999732971191, 8.392000198364258, 8.496000289916992, 8.496000289916992, 8.656000137329102, 8.807999610900879, 8.928000450134277, 9.015999794006348, 11.767999649047852, 11.767999649047852, 12.071999549865723, 12.175999641418457, 12.567999839782715, 12.663999557495117, 13.064000129699707, 13.064000129699707, 13.168000221252441, 13.607999801635742, 13.640000343322754, 13.53600025177002, 13.968000411987305, 13.968000411987305, 14.008000373840332, 14.104000091552734, 14.175999641418457, 14.104000091552734, 15.4399995803833, 15.4399995803833, 15.543999671936035, 15.48799991607666, 15.48799991607666, 15.503999710083008, 15.479999542236328, 15.479999542236328, 15.447999954223633, 15.399999618530273, 15.37600040435791, 15.35200023651123, 15.319999694824219, 15.319999694824219, 15.272000312805176, 15.272000312805176, 15.336000442504883, 15.303999900817871, 15.248000144958496, 15.248000144958496, 14.744000434875488, 15.303999900817871, 15.272000312805176, 15.248000144958496, 15.215999603271484, 15.223999977111816, 15.223999977111816, 15.248000144958496, 16.784000396728516, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.672000885009766, 16.687999725341797, 16.719999313354492, 16.70400047302246, 16.607999801635742, 16.607999801635742, 16.624000549316406, 16.6560001373291, 16.23200035095215, 16.143999099731445, 16.06399917602539, 16.06399917602539, 16.016000747680664, 17.472000122070312, 19.007999420166016, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250}; + + diff --git a/test/testData2.h b/test/testData2.h new file mode 100644 index 0000000..d0e07f0 --- /dev/null +++ b/test/testData2.h @@ -0,0 +1,28 @@ +/** + * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant + * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. + * + * This file is part of FALKOLib. + * + * FALKOLib is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * FALKOLib is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with FALKOLib. If not, see . + */ +#pragma once + +extern double testRanges1[]; + +extern double testRanges2[]; + +extern double testRangesOrtho1[]; + +extern double testRangesOrtho2[]; -- GitLab From 4c532674229a91268c88a6ddc5ea8dbc16652055 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 09:52:08 +0100 Subject: [PATCH 024/100] added virtual function in loop_closure_base --- src/loop_closure_base_2d.cpp | 11 +++++++---- src/loop_closure_base_2d.h | 4 ++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base_2d.cpp index bc79c6b..aae465d 100644 --- a/src/loop_closure_base_2d.cpp +++ b/src/loop_closure_base_2d.cpp @@ -11,14 +11,14 @@ namespace laserscanutils{ loopClosureBase2d::loopClosureBase2d(){} loopClosureBase2d::~loopClosureBase2d(){} - +/* std::shared_ptr loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ - - std::shared_ptr NewScene; + + auto NewScene=std::make_shared(); return NewScene; } - +*/ std::shared_ptr loopClosureBase2d::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ auto scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); @@ -26,4 +26,7 @@ namespace laserscanutils{ return scanFALKO; } + //void findLoopClosure(std::list& scenes, const cornerScene newScene){} + + } diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index 3a2dfd1..ec48fe6 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -51,11 +51,11 @@ public: /** \brief compare new scans against the trained set in order to find loop closures **/ - //virtual void findLoopClosure(std::list& scenes, const cornerScene newScene); + virtual void findLoopClosure(std::list& scenes, const cornerScene newScene){} /** \brief update the scene struct with keypoints and descriptors **/ - virtual std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams); + virtual std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams){} /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ -- GitLab From a7b3b977fe3e5936359874e227551f28825dbd84 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 10:00:10 +0100 Subject: [PATCH 025/100] added class loopClosureFalko --- src/CMakeLists.txt | 2 ++ src/loop_closure_falko.cpp | 26 ++++++++++++++++ src/loop_closure_falko.h | 64 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 92 insertions(+) create mode 100644 src/loop_closure_falko.cpp create mode 100644 src/loop_closure_falko.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index cec2f98..3860988 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -66,6 +66,7 @@ SET(HDRS IF(falkolib_FOUND) SET(HDRS ${HDRS} corner_falko_2d.h + loop_closure_falko.h corner_scene.h) ENDIF(falkolib_FOUND) @@ -96,6 +97,7 @@ SET(SRCS IF(falkolib_FOUND) SET(SRCS ${SRCS} corner_falko_2d.cpp + loop_closure_falko.cpp corner_scene.cpp) ENDIF(falkolib_FOUND) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp new file mode 100644 index 0000000..2008e28 --- /dev/null +++ b/src/loop_closure_falko.cpp @@ -0,0 +1,26 @@ +/** + * \file loop_closure_base_2d.h + * + * Created on: Feb 9, 2021 + * \author: spujol + */ + +# include "loop_closure_falko.h" + +namespace laserscanutils{ + + loopClosureFalko::loopClosureFalko(){} + loopClosureFalko::~loopClosureFalko(){} +/* + std::shared_ptr loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + + auto NewScene=std::make_shared(); + + return NewScene; + } +*/ + + //void findLoopClosure(std::list& scenes, const cornerScene newScene){} + + +} diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h new file mode 100644 index 0000000..6053c7b --- /dev/null +++ b/src/loop_closure_falko.h @@ -0,0 +1,64 @@ +/** + * \file loop_closure_base_2d.h + * + * Created on: Feb 9, 2021 + * \author: spujol + */ + +#ifndef LOOP_CLOSURE_FALKO_H_ +#define LOOP_CLOSURE_FALKO_H_ + +#include +#include +#include + +/************************** + * laser_scan_utils includes * + **************************/ +#include "laser_scan.h" +#include "corner_scene.h" + +/************************** + * Falko includes * + **************************/ +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace laserscanutils +{ + + /** \brief A 2base class for loop closure using falko library +*/ +class loopClosureFalko{ +private: + +public: + /** \brief Constructor + **/ + loopClosureFalko(); + + /** \brief Destructor + **/ + ~loopClosureFalko(); + + /** \brief compare new scans against the trained set in order to find loop closures + **/ + void findLoopClosure(std::list& scenes, const cornerScene newScene){} + + /** \brief update the scene struct with keypoints and descriptors + **/ + std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams){} + + +}; +} /* namespace laserscanutils */ + +#endif /* LOOP_CLOSURE_FALKO_H_ */ -- GitLab From 2910aa3f66283f96abe46bd4898f11899b562cbf Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 19:37:37 +0100 Subject: [PATCH 026/100] added typedef for loopClosurFalko class --- src/loop_closure_falko.cpp | 16 +++++++++++----- src/loop_closure_falko.h | 12 ++++++++---- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 2008e28..ebf430a 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -9,16 +9,22 @@ namespace laserscanutils{ - loopClosureFalko::loopClosureFalko(){} - loopClosureFalko::~loopClosureFalko(){} -/* - std::shared_ptr loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + template + loopClosureFalko::loopClosureFalko(){ + + } + + template + loopClosureFalko::~loopClosureFalko(){} + + template + std::shared_ptr loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ auto NewScene=std::make_shared(); return NewScene; } -*/ + //void findLoopClosure(std::list& scenes, const cornerScene newScene){} diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 6053c7b..ea7f16d 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -34,9 +34,12 @@ namespace laserscanutils { +typedef falkolib::BSCExtractor bscExtractor; +typedef falkolib::NNMatcher NNMatcher; - /** \brief A 2base class for loop closure using falko library -*/ + /** \brief A base class for loop closure using falko library + **/ +template class loopClosureFalko{ private: @@ -55,9 +58,10 @@ public: /** \brief update the scene struct with keypoints and descriptors **/ - std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams){} - + std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams); + Extractor extractor; + Matcher matcher; }; } /* namespace laserscanutils */ -- GitLab From 017ef00788b00cc43e26c8d3f4b631c29da6cdcb Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 19:44:11 +0100 Subject: [PATCH 027/100] added parameters to loopClosureFalko constructor --- src/loop_closure_falko.cpp | 14 +++++++++++++- src/loop_closure_falko.h | 2 +- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index ebf430a..d64e96b 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -9,11 +9,23 @@ namespace laserscanutils{ + //CONSTRUCTOR template loopClosureFalko::loopClosureFalko(){ - + // 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 loopClosureFalko::~loopClosureFalko(){} diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index ea7f16d..2472753 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -40,7 +40,7 @@ typedef falkolib::NNMatcher NNMatcher; /** \brief A base class for loop closure using falko library **/ template -class loopClosureFalko{ +class loopClosureFalko : public falkolib::FALKOExtractor{ private: public: -- GitLab From a3c7a7a3ee4eeb34a013117df1717ecfc2f4cea5 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 20:00:06 +0100 Subject: [PATCH 028/100] redefined template parameters name --- src/loop_closure_falko.cpp | 14 ++++++++------ src/loop_closure_falko.h | 6 +++--- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index d64e96b..9e9b3c4 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -10,8 +10,8 @@ namespace laserscanutils{ //CONSTRUCTOR - template - loopClosureFalko::loopClosureFalko(){ + template + loopClosureFalko::loopClosureFalko(){ // FALKO Extractor Parameters setMinExtractionRange(0.1); setMaxExtractionRange(25); @@ -26,11 +26,13 @@ namespace laserscanutils{ } // DESTRUCTOR - template - loopClosureFalko::~loopClosureFalko(){} + template + loopClosureFalko::~loopClosureFalko(){} - template - std::shared_ptr loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + template + std::shared_ptr loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + + auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); auto NewScene=std::make_shared(); diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 2472753..8a7aabd 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -39,7 +39,7 @@ typedef falkolib::NNMatcher NNMatcher; /** \brief A base class for loop closure using falko library **/ -template +template class loopClosureFalko : public falkolib::FALKOExtractor{ private: @@ -60,8 +60,8 @@ public: **/ std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams); - Extractor extractor; - Matcher matcher; + E extractor; + M matcher; }; } /* namespace laserscanutils */ -- GitLab From e1c5b86d644b9da7e4095076bd96b8b77b2a0cb2 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 20:43:48 +0100 Subject: [PATCH 029/100] added template to cornerScene --- src/corner_scene.h | 22 +++++++++++++++++++--- src/loop_closure_base_2d.h | 5 +++-- src/loop_closure_falko.cpp | 19 +++++++++++-------- src/loop_closure_falko.h | 9 ++++++--- 4 files changed, 39 insertions(+), 16 deletions(-) diff --git a/src/corner_scene.h b/src/corner_scene.h index a2cd43c..19e45d3 100644 --- a/src/corner_scene.h +++ b/src/corner_scene.h @@ -11,13 +11,29 @@ #include #include +/************************** + * Falko includes * + **************************/ +#include +#include +#include +#include + +#include +#include + +#include +#include + namespace laserscanutils { + +template struct cornerScene{ - std::vector keypointsList; - std::vector descriptorsList; + std::vector keypointsList; + std::vector descriptorsList; }; } /* namespace laserscanutils */ -#endif /* CORNER_SCENE_H_ */ \ No newline at end of file +#endif /* CORNER_SCENE_H_ */ diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index ec48fe6..def39c0 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -51,11 +51,12 @@ public: /** \brief compare new scans against the trained set in order to find loop closures **/ - virtual void findLoopClosure(std::list& scenes, const cornerScene newScene){} + //virtual void findLoopClosure(std::list& scenes, const cornerScene newScene){} + virtual void findLoopClosure(){} /** \brief update the scene struct with keypoints and descriptors **/ - virtual std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams){} + virtual void extractScene(LaserScan &scan,LaserScanParams &scanParams){} /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 9e9b3c4..38fb164 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -10,8 +10,8 @@ namespace laserscanutils{ //CONSTRUCTOR - template - loopClosureFalko::loopClosureFalko(){ + template + loopClosureFalko::loopClosureFalko(){ // FALKO Extractor Parameters setMinExtractionRange(0.1); setMaxExtractionRange(25); @@ -23,18 +23,21 @@ namespace laserscanutils{ // Matcher Extractor Parameters matcher.setDistanceThreshold(0.1); + } // DESTRUCTOR - template - loopClosureFalko::~loopClosureFalko(){} + template + loopClosureFalko::~loopClosureFalko(){} + + template + std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + + auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); - template - std::shared_ptr loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ - auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); + auto NewScene=std::make_shared>(); - auto NewScene=std::make_shared(); return NewScene; } diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 8a7aabd..451cb82 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -34,12 +34,15 @@ namespace laserscanutils { + +typedef falkolib::BSC bsc; typedef falkolib::BSCExtractor bscExtractor; typedef falkolib::NNMatcher NNMatcher; + /** \brief A base class for loop closure using falko library **/ -template +template class loopClosureFalko : public falkolib::FALKOExtractor{ private: @@ -54,11 +57,11 @@ public: /** \brief compare new scans against the trained set in order to find loop closures **/ - void findLoopClosure(std::list& scenes, const cornerScene newScene){} + //void findLoopClosure(std::list& scenes, const cornerScene newScene){} /** \brief update the scene struct with keypoints and descriptors **/ - std::shared_ptr extractScene(LaserScan &scan,LaserScanParams &scanParams); + std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams); E extractor; M matcher; -- GitLab From 9398ade14c127f5696739634844983c3191434ee Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Sun, 14 Feb 2021 22:34:43 +0100 Subject: [PATCH 030/100] gtest_loop_closure_falko added but not implemented --- src/loop_closure_base_2d.h | 2 +- src/loop_closure_falko.cpp | 9 +++++--- src/loop_closure_falko.h | 8 ++++--- test/gtest_loop_closure_base_2d.cpp | 34 +++++++++++++++++++++++++++++ 4 files changed, 46 insertions(+), 7 deletions(-) diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index def39c0..58ddc4e 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -56,7 +56,7 @@ public: /** \brief update the scene struct with keypoints and descriptors **/ - virtual void extractScene(LaserScan &scan,LaserScanParams &scanParams){} + //virtual void extractScene(LaserScan &scan,LaserScanParams &scanParams){} /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 38fb164..f03732a 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -22,10 +22,11 @@ namespace laserscanutils{ setGridSectors(16); // Matcher Extractor Parameters - matcher.setDistanceThreshold(0.1); + //matcher.setDistanceThreshold(0.1); } + // DESTRUCTOR template loopClosureFalko::~loopClosureFalko(){} @@ -35,9 +36,10 @@ namespace laserscanutils{ auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); - auto NewScene=std::make_shared>(); + //((*NewScene).keypointsList)=((*scanFALKO).ranges); + return NewScene; } @@ -45,5 +47,6 @@ namespace laserscanutils{ //void findLoopClosure(std::list& scenes, const cornerScene newScene){} - + //Explicitly compile all the templates + template class loopClosureFalko ; } diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 451cb82..4150397 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -17,6 +17,7 @@ **************************/ #include "laser_scan.h" #include "corner_scene.h" +#include "loop_closure_base_2d.h" /************************** * Falko includes * @@ -42,8 +43,9 @@ typedef falkolib::NNMatcher NNMatcher; /** \brief A base class for loop closure using falko library **/ + template -class loopClosureFalko : public falkolib::FALKOExtractor{ +class loopClosureFalko : public loopClosureBase2d, public falkolib::FALKOExtractor{ private: public: @@ -63,8 +65,8 @@ public: **/ std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams); - E extractor; - M matcher; + //E extractor; + //M matcher; }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index d60cab9..7ba6372 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -1,10 +1,44 @@ #include "gtest/utils_gtest.h" #include "testData2.h" #include "../src/loop_closure_base_2d.h" +#include "../src/loop_closure_falko.h" + //#include "laser_scan_utils.h" using namespace laserscanutils; +TEST(loop_closure_fako, extractScene) +{ + int scanSize = 1440; + + LaserScan scan; + LaserScanParams laserParams; + + laserParams.angle_min_=0; + laserParams.angle_max_=2.0 * M_PI; + + for (int i=0; i LCFalko; + + std::cout << "AAA" << std::endl; +/* + + std::shared_ptr scanFALKO=LCBase.convert2LaserScanFALKO(scan, laserParams); + + int firstPoint = ((*scanFALKO).ranges)[0]; + + EXPECT_FALSE(false); + + ASSERT_TRUE(true); + + ASSERT_EQ(firstPoint, 250); +*/ +// PRINTF("All good at TestTest::DummyTestExample !\n"); +} + TEST(loop_closure_base_2d, convert2laserScanFalko) { int scanSize = 1440; -- GitLab From 0656f5e6022ce6ca4bfd24f995ad3aaab70f881a Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 10:10:12 +0100 Subject: [PATCH 031/100] gtest for loopClosureFalko.extractScene implemented --- src/loop_closure_falko.cpp | 12 ++++-------- test/gtest_loop_closure_base_2d.cpp | 11 ++++------- 2 files changed, 8 insertions(+), 15 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index f03732a..3a5a20c 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -33,15 +33,11 @@ namespace laserscanutils{ template std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ - + auto newScene=std::make_shared>(); auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); - - auto NewScene=std::make_shared>(); - - //((*NewScene).keypointsList)=((*scanFALKO).ranges); - - - return NewScene; + // Extract keypoints + extract((*scanFALKO), ((*newScene).keypointsList)); + return newScene; } diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index 7ba6372..f45d07f 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -23,19 +23,16 @@ TEST(loop_closure_fako, extractScene) loopClosureFalko LCFalko; - std::cout << "AAA" << std::endl; -/* + auto newScene=LCFalko.extractScene(scan, laserParams); - std::shared_ptr scanFALKO=LCBase.convert2LaserScanFALKO(scan, laserParams); - - int firstPoint = ((*scanFALKO).ranges)[0]; + int detectedKeypoints =((*newScene).keypointsList).size(); EXPECT_FALSE(false); ASSERT_TRUE(true); - ASSERT_EQ(firstPoint, 250); -*/ + ASSERT_EQ(detectedKeypoints, 18); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From 8c173a8846bd76e4e72813049263ec46e865e291 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 11:53:57 +0100 Subject: [PATCH 032/100] modified loopClosureFalko constructor --- src/corner_scene.h | 5 +++++ src/loop_closure_falko.cpp | 27 +++++++++++++++++++-------- src/loop_closure_falko.h | 11 +++++++---- test/gtest_loop_closure_base_2d.cpp | 8 ++++---- 4 files changed, 35 insertions(+), 16 deletions(-) diff --git a/src/corner_scene.h b/src/corner_scene.h index 19e45d3..fb1932c 100644 --- a/src/corner_scene.h +++ b/src/corner_scene.h @@ -28,12 +28,17 @@ namespace laserscanutils { + + template struct cornerScene{ std::vector keypointsList; std::vector descriptorsList; }; +typedef std::shared_ptr> cornerSceneBSCPtr; + + } /* namespace laserscanutils */ #endif /* CORNER_SCENE_H_ */ diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 3a5a20c..caf5518 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -10,8 +10,13 @@ namespace laserscanutils{ //CONSTRUCTOR - template - loopClosureFalko::loopClosureFalko(){ + template + loopClosureFalko::loopClosureFalko(int _circularSectorNumber, int _radialRingNumber) : + loopClosureBase2d(), + falkolib::FALKOExtractor(), + extractor_(_circularSectorNumber, _radialRingNumber), + matcher_() + { // FALKO Extractor Parameters setMinExtractionRange(0.1); setMaxExtractionRange(25); @@ -28,15 +33,21 @@ namespace laserscanutils{ // DESTRUCTOR - template - loopClosureFalko::~loopClosureFalko(){} + template + loopClosureFalko::~loopClosureFalko(){} - template - std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + template + std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ auto newScene=std::make_shared>(); - auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); + auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); // Extract keypoints - extract((*scanFALKO), ((*newScene).keypointsList)); + extract((*scanFALKO), (newScene->keypointsList)); + + // Compute descriptors + //loopClosureFalko::extractor: + extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList); + //extractor.compute((*scanFALKO),((*newScene).keypointsList), ((*newScene).descriptorsList)); + //loopClosureFalko::extractor::compute((*scanFALKO),((*newScene).keypointsList), ((*newScene).descriptorsList)); return newScene; } diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 4150397..379a49d 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -44,14 +44,18 @@ typedef falkolib::NNMatcher NNMatcher; /** \brief A base class for loop closure using falko library **/ -template +template class loopClosureFalko : public loopClosureBase2d, public falkolib::FALKOExtractor{ private: public: + Extr extractor_; + M matcher_; + int test=1; + /** \brief Constructor **/ - loopClosureFalko(); + loopClosureFalko(int _circularSectorNumber=16, int _radialRingNumber=8); /** \brief Destructor **/ @@ -65,8 +69,7 @@ public: **/ std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams); - //E extractor; - //M matcher; + }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index f45d07f..66aca01 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -7,7 +7,7 @@ using namespace laserscanutils; -TEST(loop_closure_fako, extractScene) +TEST(loop_closure_falko, extractScene) { int scanSize = 1440; @@ -27,12 +27,12 @@ TEST(loop_closure_fako, extractScene) int detectedKeypoints =((*newScene).keypointsList).size(); - EXPECT_FALSE(false); - - ASSERT_TRUE(true); + int detectedDescriptors =newScene->descriptorsList.size(); ASSERT_EQ(detectedKeypoints, 18); + ASSERT_EQ(detectedDescriptors, 18); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From ed840a595bfa02bc3546e255ada9ea2d801a7eb1 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 12:14:35 +0100 Subject: [PATCH 033/100] added scene_base --- src/CMakeLists.txt | 4 +-- src/corner_scene.cpp | 0 src/loop_closure_falko.cpp | 5 +-- src/scene_base.h | 26 ++++++++++++++++ src/scene_falko.h | 48 +++++++++++++++++++++++++++++ test/gtest_loop_closure_base_2d.cpp | 4 --- 6 files changed, 77 insertions(+), 10 deletions(-) delete mode 100644 src/corner_scene.cpp create mode 100644 src/scene_base.h create mode 100644 src/scene_falko.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3860988..ea63227 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -57,6 +57,7 @@ SET(HDRS polyline.h scan_segment.h loop_closure_base_2d.h + scene_base.h ) IF(csm_FOUND) SET(HDRS ${HDRS} @@ -97,8 +98,7 @@ SET(SRCS IF(falkolib_FOUND) SET(SRCS ${SRCS} corner_falko_2d.cpp - loop_closure_falko.cpp - corner_scene.cpp) + loop_closure_falko.cpp) ENDIF(falkolib_FOUND) diff --git a/src/corner_scene.cpp b/src/corner_scene.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index caf5518..5e36326 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -27,7 +27,7 @@ namespace laserscanutils{ setGridSectors(16); // Matcher Extractor Parameters - //matcher.setDistanceThreshold(0.1); + matcher_.setDistanceThreshold(0.1); } @@ -44,10 +44,7 @@ namespace laserscanutils{ extract((*scanFALKO), (newScene->keypointsList)); // Compute descriptors - //loopClosureFalko::extractor: extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList); - //extractor.compute((*scanFALKO),((*newScene).keypointsList), ((*newScene).descriptorsList)); - //loopClosureFalko::extractor::compute((*scanFALKO),((*newScene).keypointsList), ((*newScene).descriptorsList)); return newScene; } diff --git a/src/scene_base.h b/src/scene_base.h new file mode 100644 index 0000000..4be0f75 --- /dev/null +++ b/src/scene_base.h @@ -0,0 +1,26 @@ +/** + * \file scene_base.h + * + * Created on: Feb 9, 2021 + * \author: spujol + */ + +#ifndef SCENE_BASE_H_ +#define SCENE_BASE_H_ + +#include +#include + + +namespace laserscanutils +{ + +struct sceneBase{ + //std::vector keypointsList; + //std::vector descriptorsList; +}; + + +} /* namespace laserscanutils */ + +#endif /* SCENE_BASE_H_ */ diff --git a/src/scene_falko.h b/src/scene_falko.h new file mode 100644 index 0000000..a7da9e1 --- /dev/null +++ b/src/scene_falko.h @@ -0,0 +1,48 @@ +/** + * \file scene_falko.h + * + * Created on: Feb 9, 2021 + * \author: spujol + */ + +#ifndef SCENE_FALKO_H_ +#define SCENE_FALKO_H_ + +#include +#include + +/************************** + * LaserScanUtils includes * + **************************/ +#inlcude "sceneBase.h" + + +/************************** + * Falko includes * + **************************/ +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace laserscanutils +{ + +template +struct sceneFalko : sceneBase{ + std::vector keypointsList; + std::vector descriptorsList; +}; + +typedef std::shared_ptr> sceneFalkoBSCPtr; + + +} /* namespace laserscanutils */ + +#endif /* SCENE_FALKO_H_ */ diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index 66aca01..b343922 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -56,10 +56,6 @@ TEST(loop_closure_base_2d, convert2laserScanFalko) int firstPoint = ((*scanFALKO).ranges)[0]; - EXPECT_FALSE(false); - - ASSERT_TRUE(true); - ASSERT_EQ(firstPoint, 250); // PRINTF("All good at TestTest::DummyTestExample !\n"); -- GitLab From 58109c7da3bee1fbab727a02c1ce05d99a40364c Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 12:19:58 +0100 Subject: [PATCH 034/100] added sceneFalko --- src/CMakeLists.txt | 4 +++- src/loop_closure_base_2d.h | 1 + src/loop_closure_falko.h | 1 + src/scene_falko.h | 2 +- 4 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ea63227..aef6c39 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -58,6 +58,7 @@ SET(HDRS scan_segment.h loop_closure_base_2d.h scene_base.h + ) IF(csm_FOUND) SET(HDRS ${HDRS} @@ -68,7 +69,8 @@ SET(HDRS SET(HDRS ${HDRS} corner_falko_2d.h loop_closure_falko.h - corner_scene.h) + corner_scene.h + scene_falko.h) ENDIF(falkolib_FOUND) #sources diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index 58ddc4e..d3d9b36 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -17,6 +17,7 @@ **************************/ #include "laser_scan.h" #include "corner_scene.h" +#include "scene_base.h" /************************** * Falko includes * diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 379a49d..5069bcc 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -17,6 +17,7 @@ **************************/ #include "laser_scan.h" #include "corner_scene.h" +#include "scene_falko.h" #include "loop_closure_base_2d.h" /************************** diff --git a/src/scene_falko.h b/src/scene_falko.h index a7da9e1..1fa8722 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -14,7 +14,7 @@ /************************** * LaserScanUtils includes * **************************/ -#inlcude "sceneBase.h" +#include "scene_base.h" /************************** -- GitLab From 5c065da68d4914c0306403ce117b5231940e541f Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 12:27:42 +0100 Subject: [PATCH 035/100] corner_scene.h deleted and substituted by scene_falko and scene_base --- src/CMakeLists.txt | 1 - src/corner_scene.h | 44 -------------------------------------- src/loop_closure_base_2d.h | 1 - src/loop_closure_falko.cpp | 6 +++--- src/loop_closure_falko.h | 5 ++--- src/scene_falko.h | 2 +- 6 files changed, 6 insertions(+), 53 deletions(-) delete mode 100644 src/corner_scene.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index aef6c39..6a432f5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -69,7 +69,6 @@ SET(HDRS SET(HDRS ${HDRS} corner_falko_2d.h loop_closure_falko.h - corner_scene.h scene_falko.h) ENDIF(falkolib_FOUND) diff --git a/src/corner_scene.h b/src/corner_scene.h deleted file mode 100644 index fb1932c..0000000 --- a/src/corner_scene.h +++ /dev/null @@ -1,44 +0,0 @@ -/** - * \file corner_scene.h - * - * Created on: Feb 9, 2021 - * \author: spujol - */ - -#ifndef CORNER_SCENE_H_ -#define CORNER_SCENE_H_ - -#include -#include - -/************************** - * Falko includes * - **************************/ -#include -#include -#include -#include - -#include -#include - -#include -#include - -namespace laserscanutils -{ - - - -template -struct cornerScene{ - std::vector keypointsList; - std::vector descriptorsList; -}; - -typedef std::shared_ptr> cornerSceneBSCPtr; - - -} /* namespace laserscanutils */ - -#endif /* CORNER_SCENE_H_ */ diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index d3d9b36..a634af4 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -16,7 +16,6 @@ * laser_scan_utils includes * **************************/ #include "laser_scan.h" -#include "corner_scene.h" #include "scene_base.h" /************************** diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 5e36326..dadc9a1 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -37,8 +37,8 @@ namespace laserscanutils{ loopClosureFalko::~loopClosureFalko(){} template - std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ - auto newScene=std::make_shared>(); + std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + auto newScene=std::make_shared>(); auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); // Extract keypoints extract((*scanFALKO), (newScene->keypointsList)); @@ -49,7 +49,7 @@ namespace laserscanutils{ } - //void findLoopClosure(std::list& scenes, const cornerScene newScene){} + //void findLoopClosure(std::list& scenes, const cornerScene newScene){} //Explicitly compile all the templates template class loopClosureFalko ; diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 5069bcc..35be401 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -16,7 +16,6 @@ * laser_scan_utils includes * **************************/ #include "laser_scan.h" -#include "corner_scene.h" #include "scene_falko.h" #include "loop_closure_base_2d.h" @@ -64,11 +63,11 @@ public: /** \brief compare new scans against the trained set in order to find loop closures **/ - //void findLoopClosure(std::list& scenes, const cornerScene newScene){} + //void findLoopClosure(std::list& scenes, const cornerScene newScene){} /** \brief update the scene struct with keypoints and descriptors **/ - std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams); + std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams); }; diff --git a/src/scene_falko.h b/src/scene_falko.h index 1fa8722..73f857f 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -40,7 +40,7 @@ struct sceneFalko : sceneBase{ std::vector descriptorsList; }; -typedef std::shared_ptr> sceneFalkoBSCPtr; +typedef std::shared_ptr> sceneFalkoBSCPtr; } /* namespace laserscanutils */ -- GitLab From 756eb9881832c43ca1cf4294a2785f184cc9a3a5 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 13:57:54 +0100 Subject: [PATCH 036/100] implemented loopClosureFalko::extractScene with override --- src/loop_closure_base_2d.h | 5 +++-- src/loop_closure_falko.h | 3 +-- src/scene_base.h | 1 + src/scene_falko.h | 2 +- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index a634af4..97965a4 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -17,6 +17,7 @@ **************************/ #include "laser_scan.h" #include "scene_base.h" +#include "scene_falko.h" /************************** * Falko includes * @@ -52,11 +53,11 @@ public: /** \brief compare new scans against the trained set in order to find loop closures **/ //virtual void findLoopClosure(std::list& scenes, const cornerScene newScene){} - virtual void findLoopClosure(){} + //virtual void findLoopClosure(){} /** \brief update the scene struct with keypoints and descriptors **/ - //virtual void extractScene(LaserScan &scan,LaserScanParams &scanParams){} + virtual std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams){} /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 35be401..e75c91d 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -51,7 +51,6 @@ private: public: Extr extractor_; M matcher_; - int test=1; /** \brief Constructor **/ @@ -67,7 +66,7 @@ public: /** \brief update the scene struct with keypoints and descriptors **/ - std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams); + std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams) override; }; diff --git a/src/scene_base.h b/src/scene_base.h index 4be0f75..7e7dd89 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -18,6 +18,7 @@ namespace laserscanutils struct sceneBase{ //std::vector keypointsList; //std::vector descriptorsList; + std::vector test; }; diff --git a/src/scene_falko.h b/src/scene_falko.h index 73f857f..65b7253 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -35,7 +35,7 @@ namespace laserscanutils { template -struct sceneFalko : sceneBase{ +struct sceneFalko : public sceneBase{ std::vector keypointsList; std::vector descriptorsList; }; -- GitLab From b1cac066df46d769cf5c57da683f10b6f4238ef2 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 14:07:16 +0100 Subject: [PATCH 037/100] changed function convert2LaserScanFALKO to loopClosureFalko --- src/loop_closure_base_2d.cpp | 7 +------ src/loop_closure_base_2d.h | 5 ----- src/loop_closure_falko.cpp | 8 ++++++++ src/loop_closure_falko.h | 4 ++++ test/gtest_loop_closure_base_2d.cpp | 6 +++--- 5 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base_2d.cpp index aae465d..5274d20 100644 --- a/src/loop_closure_base_2d.cpp +++ b/src/loop_closure_base_2d.cpp @@ -19,12 +19,7 @@ namespace laserscanutils{ return NewScene; } */ - std::shared_ptr loopClosureBase2d::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ - auto scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); - ((*scanFALKO).fromRanges)(doubleRanges); - return scanFALKO; - } + //void findLoopClosure(std::list& scenes, const cornerScene newScene){} diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base_2d.h index 97965a4..cc50265 100644 --- a/src/loop_closure_base_2d.h +++ b/src/loop_closure_base_2d.h @@ -59,11 +59,6 @@ public: **/ virtual std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams){} - /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object - **/ - std::shared_ptr convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams); - - }; } /* namespace laserscanutils */ diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index dadc9a1..6bb979f 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -36,6 +36,14 @@ namespace laserscanutils{ template loopClosureFalko::~loopClosureFalko(){} + template + std::shared_ptr loopClosureFalko::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ + auto scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); + ((*scanFALKO).fromRanges)(doubleRanges); + return scanFALKO; + } + template std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ auto newScene=std::make_shared>(); diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index e75c91d..620c638 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -68,6 +68,10 @@ public: **/ std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams) override; + /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object + **/ + std::shared_ptr convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams); + }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index b343922..e6ee4e1 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -36,7 +36,7 @@ TEST(loop_closure_falko, extractScene) // PRINTF("All good at TestTest::DummyTestExample !\n"); } -TEST(loop_closure_base_2d, convert2laserScanFalko) +TEST(loop_closure_falko, convert2laserScanFalko) { int scanSize = 1440; @@ -50,9 +50,9 @@ TEST(loop_closure_base_2d, convert2laserScanFalko) scan.ranges_raw_.push_back(testRanges1[i]); } - loopClosureBase2d LCBase; + loopClosureFalko LCFalko; - std::shared_ptr scanFALKO=LCBase.convert2LaserScanFALKO(scan, laserParams); + std::shared_ptr scanFALKO=LCFalko.convert2LaserScanFALKO(scan, laserParams); int firstPoint = ((*scanFALKO).ranges)[0]; -- GitLab From 75df9800593e5fc595b3e36ac707c6874fb08449 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 14:33:26 +0100 Subject: [PATCH 038/100] added typedef for sceneFalkoBSCPtr --- src/loop_closure_falko.cpp | 2 +- src/loop_closure_falko.h | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 6bb979f..ff3c887 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -45,7 +45,7 @@ namespace laserscanutils{ } template - std::shared_ptr> loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + typename loopClosureFalko::sceneFalkoBSCPtr loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ auto newScene=std::make_shared>(); auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); // Extract keypoints diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 620c638..4c0623c 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -49,6 +49,9 @@ class loopClosureFalko : public loopClosureBase2d, public falkolib::FALKOExtract private: public: + + typedef std::shared_ptr> sceneFalkoBSCPtr; + Extr extractor_; M matcher_; @@ -66,12 +69,14 @@ public: /** \brief update the scene struct with keypoints and descriptors **/ - std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams) override; + sceneFalkoBSCPtr extractScene(LaserScan &scan,LaserScanParams &scanParams) override; /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ std::shared_ptr convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams); + //std::share + }; } /* namespace laserscanutils */ -- GitLab From 6d22d275852d468efdc87feb39342acf4883ab4b Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 14:44:31 +0100 Subject: [PATCH 039/100] added struct matchLoopClosure --- src/CMakeLists.txt | 3 ++- src/loop_closure_falko.h | 6 +++++- src/match_loop_closure.h | 26 ++++++++++++++++++++++++++ src/scene_base.h | 1 - 4 files changed, 33 insertions(+), 3 deletions(-) create mode 100644 src/match_loop_closure.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6a432f5..b913435 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -69,7 +69,8 @@ SET(HDRS SET(HDRS ${HDRS} corner_falko_2d.h loop_closure_falko.h - scene_falko.h) + scene_falko.h + match_loop_closure.h) ENDIF(falkolib_FOUND) #sources diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 4c0623c..b9b6c27 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -18,6 +18,7 @@ #include "laser_scan.h" #include "scene_falko.h" #include "loop_closure_base_2d.h" +#include "match_loop_closure.h" /************************** * Falko includes * @@ -51,6 +52,7 @@ private: public: typedef std::shared_ptr> sceneFalkoBSCPtr; + typedef std::shared_ptr matchLoopClosurePtr; Extr extractor_; M matcher_; @@ -75,7 +77,9 @@ public: **/ std::shared_ptr convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams); - //std::share + /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object + **/ + matchLoopClosurePtr matchScene(LaserScan &scan,LaserScanParams &scanParams); }; diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h new file mode 100644 index 0000000..21ce891 --- /dev/null +++ b/src/match_loop_closure.h @@ -0,0 +1,26 @@ +/** + * \file match_loop_closure.h + * + * Created on: Feb 15, 2021 + * \author: spujol + */ + +#ifndef MATCH_LOOP_CLOSURE_H_ +#define MATCH_LOOP_CLOSURE_H_ + +#include +#include + + +namespace laserscanutils +{ + +struct matchLoopClosure{ + //std::vector keypointsList; + //std::vector descriptorsList; +}; + + +} /* namespace laserscanutils */ + +#endif /* MATCH_LOOP_CLOSURE_H_ */ diff --git a/src/scene_base.h b/src/scene_base.h index 7e7dd89..4be0f75 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -18,7 +18,6 @@ namespace laserscanutils struct sceneBase{ //std::vector keypointsList; //std::vector descriptorsList; - std::vector test; }; -- GitLab From df3776e795086a973814a78c4101fc7bb89e2312 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 15:10:13 +0100 Subject: [PATCH 040/100] added match_loop_closure struct --- src/loop_closure_falko.h | 2 +- src/match_loop_closure.h | 16 ++++++++++++++-- src/scene_falko.h | 2 +- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index b9b6c27..bd3336a 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -52,7 +52,7 @@ private: public: typedef std::shared_ptr> sceneFalkoBSCPtr; - typedef std::shared_ptr matchLoopClosurePtr; + typedef std::shared_ptr> matchLoopClosurePtr; Extr extractor_; M matcher_; diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index 21ce891..57fb6e8 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -10,14 +10,26 @@ #include #include +#include +/************************** + * laser_scan_utils includes * + **************************/ +#include "scene_falko.h" namespace laserscanutils { +template struct matchLoopClosure{ - //std::vector keypointsList; - //std::vector descriptorsList; + //std::shared_ptr> scene1; + //std::shared_ptr> scene2; + // tuple that stores the pointers of two matched scenes + std::tuple>, std::shared_ptr>> sceneTuple; + bool match; + int keypointsNumberMatch; + double score; + }; diff --git a/src/scene_falko.h b/src/scene_falko.h index 65b7253..9d041f8 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -40,7 +40,7 @@ struct sceneFalko : public sceneBase{ std::vector descriptorsList; }; -typedef std::shared_ptr> sceneFalkoBSCPtr; + } /* namespace laserscanutils */ -- GitLab From 8c539f6cb9d1dd4470778d3654d015af715154b7 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 15:31:05 +0100 Subject: [PATCH 041/100] added function match --- src/loop_closure_falko.cpp | 5 ++++- src/loop_closure_falko.h | 2 +- src/match_loop_closure.h | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index ff3c887..af17f23 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -31,7 +31,6 @@ namespace laserscanutils{ } - // DESTRUCTOR template loopClosureFalko::~loopClosureFalko(){} @@ -56,6 +55,10 @@ namespace laserscanutils{ return newScene; } + template + typename loopClosureFalko::matchLoopClosurePtr matchScene(std::shared_ptr> scene1,LaserScanParams &scanParams){ + + } //void findLoopClosure(std::list& scenes, const cornerScene newScene){} diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index bd3336a..5afcbd0 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -79,7 +79,7 @@ public: /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ - matchLoopClosurePtr matchScene(LaserScan &scan,LaserScanParams &scanParams); + matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,LaserScanParams &scanParams); }; diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index 57fb6e8..c4f0205 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -24,6 +24,7 @@ template struct matchLoopClosure{ //std::shared_ptr> scene1; //std::shared_ptr> scene2; + // tuple that stores the pointers of two matched scenes std::tuple>, std::shared_ptr>> sceneTuple; bool match; -- GitLab From 9e1e17d3bf69a1cfd0890c2d4626ade351fa8efe Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 15:53:21 +0100 Subject: [PATCH 042/100] function matchLoopClosurePtr implemented --- src/loop_closure_falko.cpp | 17 +++++++++++++++-- src/loop_closure_falko.h | 2 +- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index af17f23..d0628a1 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -56,8 +56,21 @@ namespace laserscanutils{ } template - typename loopClosureFalko::matchLoopClosurePtr matchScene(std::shared_ptr> scene1,LaserScanParams &scanParams){ - + typename loopClosureFalko::matchLoopClosurePtr matchScene(std::shared_ptr> scene1,std::shared_ptr> scene2){ + std::vector > assoNN; + int matching_number = match(scene1->keypointsList,scene2->keypointsList, assoNN); + auto new_match=std::make_shared>(); + 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[0] = scene1; + new_match -> sceneTuple[1] = scene2; + + return new_match; } //void findLoopClosure(std::list& scenes, const cornerScene newScene){} diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 5afcbd0..d8b21fa 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -79,7 +79,7 @@ public: /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ - matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,LaserScanParams &scanParams); + matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,std::shared_ptr> scene2); }; -- GitLab From 06b90b5bc575a51b4b6c13e1eb86a3a9ddd9d1fa Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 15 Feb 2021 16:10:18 +0100 Subject: [PATCH 043/100] added gtest for match function --- test/gtest_loop_closure_base_2d.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index e6ee4e1..ed2fbdc 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -7,7 +7,7 @@ using namespace laserscanutils; -TEST(loop_closure_falko, extractScene) +TEST(loop_closure_falko, TestExtractSceneAndMatcher) { int scanSize = 1440; @@ -32,7 +32,15 @@ TEST(loop_closure_falko, extractScene) ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedDescriptors, 18); +/* + //Test matcher + auto new_match =LCFalko.matchScene(newScene,newScene); + + int keypoints_matched = new_match->keypointsNumberMatch; + + ASSERT_EQ(keypoints_matched, 18); +*/ // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From 2992ef0fd2cf8786374853e0535717cd7dac25fb Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 16 Feb 2021 11:20:32 +0100 Subject: [PATCH 044/100] function matchScene finished and gtest added --- src/loop_closure_falko.cpp | 4 ++-- src/loop_closure_falko.h | 15 ++++++++++++++- src/match_loop_closure.h | 1 + test/gtest_loop_closure_base_2d.cpp | 12 ++++++------ 4 files changed, 23 insertions(+), 9 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index d0628a1..c0ab0fb 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -54,7 +54,7 @@ namespace laserscanutils{ extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList); return newScene; } - +/* template typename loopClosureFalko::matchLoopClosurePtr matchScene(std::shared_ptr> scene1,std::shared_ptr> scene2){ std::vector > assoNN; @@ -72,7 +72,7 @@ namespace laserscanutils{ return new_match; } - +*/ //void findLoopClosure(std::list& scenes, const cornerScene newScene){} //Explicitly compile all the templates diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index d8b21fa..bfbcb78 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -79,7 +79,20 @@ public: /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object **/ - matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,std::shared_ptr> scene2); + matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,sceneFalkoBSCPtr scene2){ + std::vector > assoNN; + int matching_number = matcher_.match(scene1->keypointsList,scene2->keypointsList, assoNN); + auto new_match=std::make_shared>(); + 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; + } }; diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index c4f0205..bf85aee 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -11,6 +11,7 @@ #include #include #include +#include /************************** * laser_scan_utils includes * diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index ed2fbdc..e2cab5c 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -23,24 +23,24 @@ TEST(loop_closure_falko, TestExtractSceneAndMatcher) loopClosureFalko LCFalko; - auto newScene=LCFalko.extractScene(scan, laserParams); + auto new_scene=LCFalko.extractScene(scan, laserParams); - int detectedKeypoints =((*newScene).keypointsList).size(); + int detectedKeypoints =((*new_scene).keypointsList).size(); - int detectedDescriptors =newScene->descriptorsList.size(); + int detectedDescriptors =new_scene->descriptorsList.size(); ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedDescriptors, 18); -/* + //Test matcher - auto new_match =LCFalko.matchScene(newScene,newScene); + auto new_match =LCFalko.matchScene(new_scene,new_scene); int keypoints_matched = new_match->keypointsNumberMatch; ASSERT_EQ(keypoints_matched, 18); -*/ + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From 93c8821ae243a300c6a3ec6dba9e57eb3bdb61c6 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 16 Feb 2021 11:36:05 +0100 Subject: [PATCH 045/100] sceneExtract moved to .h file --- src/loop_closure_falko.cpp | 3 ++- src/loop_closure_falko.h | 15 +++++++++++---- test/gtest_loop_closure_base_2d.cpp | 2 +- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index c0ab0fb..824400a 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -42,7 +42,7 @@ namespace laserscanutils{ ((*scanFALKO).fromRanges)(doubleRanges); return scanFALKO; } - +/* template typename loopClosureFalko::sceneFalkoBSCPtr loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ auto newScene=std::make_shared>(); @@ -54,6 +54,7 @@ namespace laserscanutils{ extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList); return newScene; } + */ /* template typename loopClosureFalko::matchLoopClosurePtr matchScene(std::shared_ptr> scene1,std::shared_ptr> scene2){ diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index bfbcb78..95c534e 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -69,15 +69,23 @@ public: **/ //void findLoopClosure(std::list& scenes, const cornerScene newScene){} - /** \brief update the scene struct with keypoints and descriptors + /** \brief Create and update the scene struct with keypoints and descriptors **/ - sceneFalkoBSCPtr extractScene(LaserScan &scan,LaserScanParams &scanParams) override; + sceneFalkoBSCPtr extractScene(LaserScan &scan,LaserScanParams &scanParams) { + auto newScene=std::make_shared>(); + auto scanFALKO =loopClosureFalko::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 convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams); - /** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object + /** \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 > assoNN; @@ -93,7 +101,6 @@ public: new_match -> sceneTuple = std::make_tuple(scene1,scene2); return new_match; } - }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_base_2d.cpp index e2cab5c..d1777d5 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_base_2d.cpp @@ -44,7 +44,7 @@ TEST(loop_closure_falko, TestExtractSceneAndMatcher) // PRINTF("All good at TestTest::DummyTestExample !\n"); } -TEST(loop_closure_falko, convert2laserScanFalko) +TEST(loop_closure_falko2, convert2laserScanFalko) { int scanSize = 1440; -- GitLab From c19c19a9fe241f57c20efc3551699fbd2f77032c Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Wed, 17 Feb 2021 11:50:32 +0100 Subject: [PATCH 046/100] added gtest for findLoopClosure function --- src/loop_closure_falko.cpp | 6 ++++-- src/loop_closure_falko.h | 31 +++++++++++++++++++++++------ test/gtest_loop_closure_base_2d.cpp | 10 ++++++++++ 3 files changed, 39 insertions(+), 8 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index 824400a..c798fa2 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -30,11 +30,11 @@ namespace laserscanutils{ matcher_.setDistanceThreshold(0.1); } - +/* // DESTRUCTOR template loopClosureFalko::~loopClosureFalko(){} - +*/ template std::shared_ptr loopClosureFalko::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ auto scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); @@ -76,6 +76,8 @@ namespace laserscanutils{ */ //void findLoopClosure(std::list& scenes, const cornerScene newScene){} + + //Explicitly compile all the templates template class loopClosureFalko ; } diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 95c534e..f92b6dd 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -11,6 +11,8 @@ #include #include #include +#include +#include /************************** * laser_scan_utils includes * @@ -63,11 +65,7 @@ public: /** \brief Destructor **/ - ~loopClosureFalko(); - - /** \brief compare new scans against the trained set in order to find loop closures - **/ - //void findLoopClosure(std::list& scenes, const cornerScene newScene){} + ~loopClosureFalko(){} /** \brief Create and update the scene struct with keypoints and descriptors **/ @@ -101,7 +99,28 @@ public: 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 findLoopClosure(std::list &l_scenes, const sceneFalkoBSCPtr &new_scene){ + int number_ref_sc = l_scenes.size(); + + std::list matchings; + + for (int i=0; i>> ref_scenes; + + ref_scenes.push_back(new_scene); + + ref_scenes.push_back(new_scene); + + auto matchings = LCFalko.findLoopClosure(ref_scenes,new_scene); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From 395dd68b48d0177b4f16227af06e770d894ba530 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Wed, 17 Feb 2021 18:54:09 +0100 Subject: [PATCH 047/100] findLoopClosure function finished --- src/loop_closure_falko.h | 12 ++---------- test/gtest_loop_closure_base_2d.cpp | 10 ++++++++++ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index f92b6dd..0785cb1 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -104,22 +104,14 @@ public: **/ std::list findLoopClosure(std::list &l_scenes, const sceneFalkoBSCPtr &new_scene){ int number_ref_sc = l_scenes.size(); - std::list matchings; - for (int i=0; iget()->match; + + ASSERT_EQ(there_is_match, true); + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From a0b2def97fc535b3318c6e79f46a440aed38bd1d Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 18 Feb 2021 09:43:59 +0100 Subject: [PATCH 048/100] renaming of loop_closure_base --- src/CMakeLists.txt | 4 ++-- src/{loop_closure_base_2d.cpp => loop_closure_base.cpp} | 2 +- src/{loop_closure_base_2d.h => loop_closure_base.h} | 0 src/loop_closure_falko.h | 2 +- test/CMakeLists.txt | 4 ++-- ...loop_closure_base_2d.cpp => gtest_loop_closure_falko.cpp} | 5 ++--- 6 files changed, 8 insertions(+), 9 deletions(-) rename src/{loop_closure_base_2d.cpp => loop_closure_base.cpp} (93%) rename src/{loop_closure_base_2d.h => loop_closure_base.h} (100%) rename test/{gtest_loop_closure_base_2d.cpp => gtest_loop_closure_falko.cpp} (94%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b913435..2ce8fcf 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -56,7 +56,7 @@ SET(HDRS point_set.h polyline.h scan_segment.h - loop_closure_base_2d.h + loop_closure_base.h scene_base.h ) @@ -90,7 +90,7 @@ SET(SRCS point_set.cpp polyline.cpp scan_segment.cpp - loop_closure_base_2d.cpp + loop_closure_base.cpp ) IF(csm_FOUND) SET(SRCS ${SRCS} diff --git a/src/loop_closure_base_2d.cpp b/src/loop_closure_base.cpp similarity index 93% rename from src/loop_closure_base_2d.cpp rename to src/loop_closure_base.cpp index 5274d20..adf300d 100644 --- a/src/loop_closure_base_2d.cpp +++ b/src/loop_closure_base.cpp @@ -5,7 +5,7 @@ * \author: spujol */ -# include "loop_closure_base_2d.h" +# include "loop_closure_base.h" namespace laserscanutils{ diff --git a/src/loop_closure_base_2d.h b/src/loop_closure_base.h similarity index 100% rename from src/loop_closure_base_2d.h rename to src/loop_closure_base.h diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 0785cb1..c4ee28a 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -19,7 +19,7 @@ **************************/ #include "laser_scan.h" #include "scene_falko.h" -#include "loop_closure_base_2d.h" +#include "loop_closure_base.h" #include "match_loop_closure.h" /************************** diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 386aea8..89062a0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -22,5 +22,5 @@ INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) gnss_utils_add_gtest(gtest_example gtest_example.cpp) target_link_libraries(gtest_example ${PROJECT_NAME}) -gnss_utils_add_gtest(gtest_loop_closure_base_2d gtest_loop_closure_base_2d.cpp ${PROJECT_SOURCE_DIR}/test/testData2.cpp) -target_link_libraries(gtest_loop_closure_base_2d ${PROJECT_NAME}) +gnss_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/testData2.cpp) +target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME}) diff --git a/test/gtest_loop_closure_base_2d.cpp b/test/gtest_loop_closure_falko.cpp similarity index 94% rename from test/gtest_loop_closure_base_2d.cpp rename to test/gtest_loop_closure_falko.cpp index f7f7457..e004f63 100644 --- a/test/gtest_loop_closure_base_2d.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -1,13 +1,12 @@ #include "gtest/utils_gtest.h" #include "testData2.h" -#include "../src/loop_closure_base_2d.h" +#include "../src/loop_closure_base.h" #include "../src/loop_closure_falko.h" -//#include "laser_scan_utils.h" using namespace laserscanutils; -TEST(loop_closure_falko, TestExtractSceneAndMatcher) +TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { int scanSize = 1440; -- GitLab From e5730d54b8da3629bb551a0cabe4ab7994fdb7fd Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 18 Feb 2021 10:03:58 +0100 Subject: [PATCH 049/100] added another scan to testData in test folder --- test/testData2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/testData2.cpp b/test/testData2.cpp index f71191c..5cca59d 100644 --- a/test/testData2.cpp +++ b/test/testData2.cpp @@ -20,4 +20,4 @@ double testRanges1[]= {250, 250, 250, 250, 250, 17.263999938964844, 17.263999938964844, 17.263999938964844, 17.263999938964844, 13.552000045776367, 13.552000045776367, 13.552000045776367, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.51200008392334, 11.567999839782715, 11.567999839782715, 10.720000267028809, 10.720000267028809, 10.720000267028809, 10.359999656677246, 10.336000442504883, 10.343999862670898, 10.35200023651123, 10.35200023651123, 10.32800006866455, 10.319999694824219, 10.383999824523926, 10.407999992370605, 10.432000160217285, 10.432000160217285, 10.447999954223633, 10.46399974822998, 10.46399974822998, 10.503999710083008, 10.503999710083008, 10.527999877929688, 10.592000007629395, 10.62399959564209, 8.368000030517578, 8.343999862670898, 8.343999862670898, 8.031999588012695, 7.831999778747559, 7.6479997634887695, 7.552000045776367, 7.415999889373779, 7.415999889373779, 7.263999938964844, 7.144000053405762, 7.0320000648498535, 6.920000076293945, 6.776000022888184, 6.776000022888184, 6.671999931335449, 6.576000213623047, 6.440000057220459, 6.328000068664551, 6.271999835968018, 6.271999835968018, 6.263999938964844, 6.271999835968018, 6.296000003814697, 6.320000171661377, 6.328000068664551, 6.328000068664551, 6.335999965667725, 6.320000171661377, 3.187999963760376, 3.187999963760376, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.504000186920166, 4.504000186920166, 4.504000186920166, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.599999904632568, 4.576000213623047, 4.495999813079834, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.4079999923706055, 4.368000030517578, 4.320000171661377, 4.271999835968018, 4.271999835968018, 4.223999977111816, 4.184000015258789, 4.168000221252441, 4.159999847412109, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.184000015258789, 4.199999809265137, 4.199999809265137, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.223999977111816, 4.23199987411499, 4.23199987411499, 4.176000118255615, 250, 250, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.099999904632568, 4.099999904632568, 4.099999904632568, 4.079999923706055, 4.0320000648498535, 3.98799991607666, 3.9719998836517334, 3.9719998836517334, 3.9560000896453857, 3.9240000247955322, 3.888000011444092, 3.859999895095825, 3.8320000171661377, 3.8320000171661377, 3.803999900817871, 3.7880001068115234, 3.259999990463257, 3.240000009536743, 3.2200000286102295, 3.2200000286102295, 3.200000047683716, 3.1760001182556152, 3.1559998989105225, 3.1440000534057617, 3.128000020980835, 3.128000020980835, 3.119999885559082, 2.5959999561309814, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.496000051498413, 2.5320000648498535, 250, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 250, 250, 250, 250, 250, 250, 250, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.640000104904175, 2.640000104904175, 2.640000104904175, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.555999994277954, 2.5439999103546143, 2.5320000648498535, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.503999948501587, 2.492000102996826, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4600000381469727, 2.4519999027252197, 2.440000057220459, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4079999923706055, 2.427999973297119, 2.444000005722046, 2.444000005722046, 2.4600000381469727, 2.4719998836517334, 2.48799991607666, 2.5, 2.5160000324249268, 2.5160000324249268, 2.5280001163482666, 2.5399999618530273, 2.559999942779541, 2.5799999237060547, 2.5840001106262207, 2.5840001106262207, 3.0480000972747803, 3.0480000972747803, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5959999561309814, 2.5920000076293945, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.572000026702881, 2.563999891281128, 2.555999994277954, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5399999618530273, 2.5360000133514404, 2.5320000648498535, 2.5239999294281006, 2.5199999809265137, 2.507999897003174, 2.507999897003174, 2.503999948501587, 2.5, 2.496000051498413, 2.492000102996826, 2.48799991607666, 2.48799991607666, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4639999866485596, 2.4560000896453857, 2.4560000896453857, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.444000005722046, 2.436000108718872, 2.436000108718872, 2.431999921798706, 2.427999973297119, 2.4240000247955322, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4040000438690186, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.3559999465942383, 2.3559999465942383, 2.3559999465942383, 2.6600000858306885, 2.6600000858306885, 2.6600000858306885, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.0, 2.0, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 2.0, 2.0, 2.009999990463257, 2.0220000743865967, 2.0460000038146973, 2.0460000038146973, 2.0260000228881836, 2.0260000228881836, 2.00600004196167, 1.996000051498413, 1.99399995803833, 1.9900000095367432, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.9980000257492065, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9919999837875366, 1.9520000219345093, 2.0320000648498535, 2.0999999046325684, 2.1040000915527344, 2.1040000915527344, 2.0280001163482666, 1.9040000438690186, 1.9279999732971191, 1.9520000219345093, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9600000381469727, 1.9620000123977661, 1.9639999866485596, 1.9639999866485596, 1.965999960899353, 1.965999960899353, 1.968000054359436, 1.972000002861023, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9800000190734863, 1.9839999675750732, 1.9800000190734863, 1.9800000190734863, 1.9819999933242798, 1.9880000352859497, 1.996000051498413, 2.009999990463257, 2.00600004196167, 2.00600004196167, 1.99399995803833, 2.687999963760376, 2.687999963760376, 2.687999963760376, 1.74399995803833, 1.74399995803833, 2.063999891281128, 2.115999937057495, 2.196000099182129, 2.191999912261963, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5199999809265137, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5399999618530273, 2.5480000972747803, 2.5480000972747803, 2.5480000972747803, 2.5399999618530273, 2.5439999103546143, 2.552000045776367, 2.555999994277954, 2.555999994277954, 2.552000045776367, 2.5480000972747803, 250, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3079999685287476, 1.2999999523162842, 1.2940000295639038, 1.2940000295639038, 1.2879999876022339, 1.2860000133514404, 1.2940000295639038, 1.3040000200271606, 1.3040000200271606, 1.3040000200271606, 1.2999999523162842, 1.2979999780654907, 1.3020000457763672, 1.309999942779541, 1.3140000104904175, 1.3140000104904175, 1.3140000104904175, 1.3240000009536743, 1.3359999656677246, 1.3420000076293945, 1.343999981880188, 1.343999981880188, 1.2200000286102295, 250, 250, 250, 250, 250, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.090000033378601, 1.090000033378601, 1.0920000076293945, 1.0980000495910645, 1.0980000495910645, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.1019999980926514, 1.1119999885559082, 1.121999979019165, 1.1260000467300415, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.776000022888184, 5.776000022888184, 5.776000022888184, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.736000061035156, 5.679999828338623, 5.671999931335449, 5.671999931335449, 5.631999969482422, 5.631999969482422, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.559999942779541, 5.584000110626221, 6.104000091552734, 6.783999919891357, 6.831999778747559, 6.831999778747559, 6.831999778747559, 6.760000228881836, 6.616000175476074, 6.455999851226807, 6.343999862670898, 6.343999862670898, 6.320000171661377, 6.311999797821045, 9.192000389099121, 9.175999641418457, 9.15999984741211, 9.15999984741211, 9.128000259399414, 9.119999885559082, 7.992000102996826, 7.9120001792907715, 7.8480000495910645, 7.8480000495910645, 7.800000190734863, 7.74399995803833, 7.71999979019165, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.71999979019165, 7.71999979019165, 7.711999893188477, 7.703999996185303, 7.703999996185303, 7.703999996185303, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.671999931335449, 8.67199993133545, 8.640000343322754, 8.607999801635742, 8.583999633789062, 8.583999633789062, 8.5600004196167, 8.552000045776367, 8.53600025177002, 8.520000457763672, 8.503999710083008, 8.503999710083008, 8.472000122070312, 8.447999954223633, 8.416000366210938, 8.383999824523926, 8.383999824523926, 8.383999824523926, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.336000442504883, 8.303999900817871, 8.303999900817871, 8.263999938964844, 8.239999771118164, 8.279999732971191, 8.392000198364258, 8.496000289916992, 8.496000289916992, 8.656000137329102, 8.807999610900879, 8.928000450134277, 9.015999794006348, 11.767999649047852, 11.767999649047852, 12.071999549865723, 12.175999641418457, 12.567999839782715, 12.663999557495117, 13.064000129699707, 13.064000129699707, 13.168000221252441, 13.607999801635742, 13.640000343322754, 13.53600025177002, 13.968000411987305, 13.968000411987305, 14.008000373840332, 14.104000091552734, 14.175999641418457, 14.104000091552734, 15.4399995803833, 15.4399995803833, 15.543999671936035, 15.48799991607666, 15.48799991607666, 15.503999710083008, 15.479999542236328, 15.479999542236328, 15.447999954223633, 15.399999618530273, 15.37600040435791, 15.35200023651123, 15.319999694824219, 15.319999694824219, 15.272000312805176, 15.272000312805176, 15.336000442504883, 15.303999900817871, 15.248000144958496, 15.248000144958496, 14.744000434875488, 15.303999900817871, 15.272000312805176, 15.248000144958496, 15.215999603271484, 15.223999977111816, 15.223999977111816, 15.248000144958496, 16.784000396728516, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.672000885009766, 16.687999725341797, 16.719999313354492, 16.70400047302246, 16.607999801635742, 16.607999801635742, 16.624000549316406, 16.6560001373291, 16.23200035095215, 16.143999099731445, 16.06399917602539, 16.06399917602539, 16.016000747680664, 17.472000122070312, 19.007999420166016, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250}; - + double testRanges2[]={250, 250, 250, 250, 16.799999237060547, 16.799999237060547, 16.799999237060547, 16.799999237060547, 14.095999717712402, 14.095999717712402, 14.095999717712402, 14.095999717712402, 13.255999565124512, 13.255999565124512, 13.255999565124512, 12.767999649047852, 12.175999641418457, 12.175999641418457, 10.911999702453613, 10.4399995803833, 10.4399995803833, 10.4399995803833, 10.32800006866455, 10.32800006866455, 10.32800006866455, 10.343999862670898, 10.336000442504883, 10.312000274658203, 10.368000030517578, 10.368000030517578, 10.407999992370605, 10.472000122070312, 10.48799991607666, 10.520000457763672, 10.48799991607666, 10.48799991607666, 10.472000122070312, 10.503999710083008, 10.51200008392334, 10.51200008392334, 10.520000457763672, 10.520000457763672, 8.295999526977539, 8.13599967956543, 7.984000205993652, 7.791999816894531, 7.624000072479248, 7.624000072479248, 7.5279998779296875, 7.4079999923706055, 7.263999938964844, 7.119999885559082, 7.007999897003174, 7.007999897003174, 6.9120001792907715, 6.760000228881836, 6.639999866485596, 6.544000148773193, 6.415999889373779, 6.415999889373779, 6.311999797821045, 6.271999835968018, 6.263999938964844, 6.288000106811523, 6.303999900817871, 6.303999900817871, 6.320000171661377, 6.335999965667725, 6.343999862670898, 6.335999965667725, 6.303999900817871, 6.303999900817871, 6.271999835968018, 4.5920000076293945, 250, 4.480000019073486, 4.480000019073486, 4.480000019073486, 4.480000019073486, 4.4720001220703125, 4.504000186920166, 4.519999980926514, 4.5279998779296875, 4.5279998779296875, 4.5279998779296875, 4.5279998779296875, 4.567999839782715, 4.559999942779541, 250, 4.48799991607666, 4.48799991607666, 4.48799991607666, 4.48799991607666, 4.440000057220459, 4.392000198364258, 4.360000133514404, 4.360000133514404, 4.320000171661377, 4.263999938964844, 4.223999977111816, 4.191999912261963, 4.168000221252441, 4.168000221252441, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.191999912261963, 4.208000183105469, 4.208000183105469, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.216000080108643, 4.23199987411499, 4.23199987411499, 4.223999977111816, 4.184000015258789, 250, 4.136000156402588, 4.136000156402588, 4.136000156402588, 4.136000156402588, 4.111999988555908, 4.111999988555908, 4.0960001945495605, 4.064000129699707, 4.015999794006348, 4.015999794006348, 3.9839999675750732, 3.9679999351501465, 3.947999954223633, 3.9119999408721924, 3.875999927520752, 3.875999927520752, 3.8480000495910645, 3.8239998817443848, 3.7039999961853027, 3.5920000076293945, 3.259999990463257, 3.259999990463257, 3.240000009536743, 3.2160000801086426, 3.196000099182129, 3.171999931335449, 3.1519999504089355, 3.1519999504089355, 3.140000104904175, 3.124000072479248, 3.115999937057495, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.51200008392334, 2.51200008392334, 2.51200008392334, 2.51200008392334, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.6040000915527344, 2.6040000915527344, 2.6040000915527344, 2.6040000915527344, 250, 250, 250, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 250, 250, 250, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 2.635999917984009, 2.628000020980835, 2.619999885559082, 2.619999885559082, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.552000045776367, 2.5399999618530273, 2.5280001163482666, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.5, 2.48799991607666, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4560000896453857, 2.444000005722046, 2.436000108718872, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4159998893737793, 2.4200000762939453, 2.431999921798706, 2.444000005722046, 2.4600000381469727, 2.4600000381469727, 2.4760000705718994, 2.48799991607666, 2.5, 2.5160000324249268, 2.5360000133514404, 2.5360000133514404, 2.5480000972747803, 2.563999891281128, 2.5840001106262207, 2.7079999446868896, 2.684000015258789, 2.684000015258789, 2.5880000591278076, 2.5880000591278076, 2.5880000591278076, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5920000076293945, 2.5880000591278076, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.559999942779541, 2.559999942779541, 2.552000045776367, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5320000648498535, 2.5320000648498535, 2.5280001163482666, 2.5239999294281006, 2.5160000324249268, 2.507999897003174, 2.503999948501587, 2.503999948501587, 2.5, 2.492000102996826, 2.492000102996826, 2.4839999675750732, 2.4800000190734863, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4600000381469727, 2.4600000381469727, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.447999954223633, 2.440000057220459, 2.440000057220459, 2.431999921798706, 2.427999973297119, 2.427999973297119, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4119999408721924, 2.4079999923706055, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.371999979019165, 2.388000011444092, 2.388000011444092, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.002000093460083, 2.002000093460083, 2.002000093460083, 2.002000093460083, 2.002000093460083, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 2.0, 2.009999990463257, 2.009999990463257, 2.0320000648498535, 2.0480000972747803, 2.0320000648498535, 2.0179998874664307, 2.007999897003174, 2.007999897003174, 1.996000051498413, 1.9919999837875366, 1.9900000095367432, 1.9880000352859497, 1.99399995803833, 1.99399995803833, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.99399995803833, 1.9980000257492065, 1.9980000257492065, 2.0, 2.002000093460083, 2.0, 2.002000093460083, 1.9759999513626099, 1.9759999513626099, 1.9359999895095825, 2.0360000133514404, 2.0859999656677246, 2.0439999103546143, 1.9819999933242798, 1.9819999933242798, 1.9279999732971191, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9559999704360962, 1.9559999704360962, 1.9579999446868896, 1.9620000123977661, 1.9639999866485596, 1.968000054359436, 1.9700000286102295, 1.9700000286102295, 1.972000002861023, 1.9739999771118164, 1.9739999771118164, 1.9739999771118164, 1.9780000448226929, 1.9780000448226929, 1.9839999675750732, 1.9859999418258667, 1.9859999418258667, 1.9900000095367432, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.99399995803833, 1.9980000257492065, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.5999999046325684, 1.6720000505447388, 2.0480000972747803, 2.115999937057495, 2.115999937057495, 2.191999912261963, 2.196000099182129, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5360000133514404, 2.5360000133514404, 2.5360000133514404, 2.5399999618530273, 2.5439999103546143, 2.5439999103546143, 2.5439999103546143, 2.5439999103546143, 2.5439999103546143, 2.5480000972747803, 2.555999994277954, 2.559999942779541, 1.315999984741211, 1.315999984741211, 1.315999984741211, 1.315999984741211, 1.3040000200271606, 1.3040000200271606, 1.2979999780654907, 1.2920000553131104, 1.2920000553131104, 1.2860000133514404, 1.2899999618530273, 1.2999999523162842, 1.3020000457763672, 1.3020000457763672, 1.3020000457763672, 1.2999999523162842, 1.2960000038146973, 1.3020000457763672, 1.312000036239624, 1.315999984741211, 1.315999984741211, 1.3179999589920044, 1.3279999494552612, 1.3359999656677246, 1.3420000076293945, 1.2200000286102295, 1.2200000286102295, 250, 250, 250, 250, 250, 1.090000033378601, 1.090000033378601, 1.090000033378601, 1.090000033378601, 1.0880000591278076, 1.0880000591278076, 1.0880000591278076, 1.0920000076293945, 1.0959999561309814, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.0959999561309814, 1.0959999561309814, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.0920000076293945, 1.0920000076293945, 1.0920000076293945, 1.093999981880188, 1.1019999980926514, 1.1139999628067017, 1.1239999532699585, 1.1239999532699585, 1.1299999952316284, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.888000011444092, 5.888000011444092, 5.888000011444092, 5.888000011444092, 5.863999843597412, 5.863999843597412, 5.808000087738037, 5.751999855041504, 5.751999855041504, 5.74399995803833, 5.74399995803833, 5.656000137329102, 5.656000137329102, 5.656000137329102, 5.584000110626221, 5.584000110626221, 5.584000110626221, 5.584000110626221, 5.552000045776367, 5.559999942779541, 5.599999904632568, 5.599999904632568, 6.71999979019165, 6.783999919891357, 6.831999778747559, 6.815999984741211, 6.696000099182129, 6.552000045776367, 6.552000045776367, 6.423999786376953, 6.343999862670898, 6.328000068664551, 9.232000350952148, 9.192000389099121, 9.192000389099121, 9.175999641418457, 9.144000053405762, 9.039999961853027, 8.960000038146973, 7.943999767303467, 7.943999767303467, 7.863999843597412, 7.8480000495910645, 7.791999816894531, 7.751999855041504, 7.736000061035156, 7.736000061035156, 7.71999979019165, 7.728000164031982, 7.728000164031982, 7.711999893188477, 7.696000099182129, 7.696000099182129, 7.688000202178955, 7.696000099182129, 7.696000099182129, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.736000061035156, 7.800000190734863, 8.616000175476074, 8.600000381469727, 8.600000381469727, 8.576000213623047, 8.543999671936035, 8.520000457763672, 8.51200008392334, 8.496000289916992, 8.496000289916992, 8.472000122070312, 8.456000328063965, 8.4399995803833, 8.4399995803833, 8.423999786376953, 8.423999786376953, 8.383999824523926, 8.37600040435791, 8.37600040435791, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.35200023651123, 8.312000274658203, 8.248000144958496, 8.223999977111816, 8.288000106811523, 8.288000106811523, 8.4399995803833, 8.607999801635742, 8.776000022888184, 8.895999908447266, 9.064000129699707, 9.064000129699707, 9.168000221252441, 11.84000015258789, 12.175999641418457, 12.312000274658203, 12.656000137329102, 12.656000137329102, 12.767999649047852, 13.208000183105469, 13.35200023651123, 13.656000137329102, 13.64799976348877, 13.64799976348877, 13.527999877929688, 14.015999794006348, 13.960000038146973, 14.119999885559082, 14.255999565124512, 14.255999565124512, 13.784000396728516, 15.407999992370605, 15.496000289916992, 15.53600025177002, 15.423999786376953, 15.423999786376953, 15.37600040435791, 15.416000366210938, 15.423999786376953, 15.383999824523926, 15.343999862670898, 15.343999862670898, 15.368000030517578, 15.32800006866455, 15.255999565124512, 15.272000312805176, 15.272000312805176, 15.272000312805176, 15.215999603271484, 15.272000312805176, 15.295999526977539, 15.208000183105469, 15.215999603271484, 15.215999603271484, 15.208000183105469, 15.208000183105469, 15.192000389099121, 15.168000221252441, 15.168000221252441, 16.719999313354492, 16.719999313354492, 16.639999389648438, 16.6560001373291, 16.719999313354492, 16.719999313354492, 16.639999389648438, 16.672000885009766, 16.687999725341797, 16.54400062561035, 16.576000213623047, 16.6560001373291, 16.6560001373291, 16.27199935913086, 16.583999633789062, 16.079999923706055, 16.399999618530273, 16.736000061035156, 16.736000061035156, 16.143999099731445, 250, 20.304000854492188, 20.304000854492188, 20.304000854492188, 20.304000854492188, 250, 250, 250, 20.20800018310547, 20.20800018310547, 20.20800018310547, 20.20800018310547, 250, 250, 250, 250, 250, 250}; -- GitLab From f4f75253f1bf813e09cef749d2c817d6d17f0db1 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 22 Feb 2021 07:22:38 +0100 Subject: [PATCH 050/100] 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 keypointSet2; - extract(scanFALKO, keypointSet2); + falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams); - //Compute descriptors - std::vector descriptorSet2; - compute(scanFALKO, keypointSet2, descriptorSet2); + // Compute descriptors + std::vector keypointSet2; + extract(scanFALKO, keypointSet2); - //Matching - int rows = keypointSets.size(); + // Compute descriptors + std::vector descriptorSet2; + compute(scanFALKO, keypointSet2, descriptorSet2); - numberKeypointsMatch=0; - numberSceneMatch=0; - matchingPosition=-1; - for (int i=0; i > 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> 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 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 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 #include +#include /************************** * laser_scan_utils includes * @@ -24,86 +24,94 @@ /************************** * Falko includes * **************************/ -#include -#include #include +#include +#include #include #include #include -#include #include +#include - -namespace laserscanutils -{ +namespace laserscanutils { /** \brief A 2D corner extractor and loop closure computing class **/ -class CornerFalko2d: public falkolib::FALKOExtractor, public falkolib::BSCExtractor, public falkolib::NNMatcher -{ +class CornerFalko2d : public falkolib::FALKOExtractor, + public falkolib::BSCExtractor, + public falkolib::NNMatcher { 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> keypointSets; - std::vector lastKeypointSet; - std::vector> descriptorSets; - std::vectorlastDescriptorSet; - - // 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> keypointSets; + std::vector lastKeypointSet; + std::vector> descriptorSets; + std::vector 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 loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + std::shared_ptr loopClosureBase2d::extractScene(LaserScan + &scan,LaserScanParams &scanParams){ auto NewScene=std::make_shared(); @@ -20,8 +21,6 @@ namespace laserscanutils{ } */ - - //void findLoopClosure(std::list& scenes, const cornerScene newScene){} - - +// void findLoopClosure(std::list& 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 #include +#include #include /************************** @@ -22,43 +22,43 @@ /************************** * Falko includes * **************************/ -#include -#include #include +#include +#include #include #include #include -#include #include +#include -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& scenes, const cornerScene newScene){} - //virtual void findLoopClosure(){} + /** \brief Destructor + **/ + ~loopClosureBase2d(); - /** \brief update the scene struct with keypoints and descriptors - **/ - virtual std::shared_ptr> extractScene(LaserScan &scan,LaserScanParams &scanParams){} + /** \brief compare new scans against the trained set in order to find loop + *closures + **/ + // virtual void findLoopClosure(std::list& scenes, const + // cornerScene newScene){} + // virtual void findLoopClosure(){} + /** \brief update the scene struct with keypoints and descriptors + **/ + virtual std::shared_ptr> + 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 - loopClosureFalko::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 +loopClosureFalko::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 loopClosureFalko::~loopClosureFalko(){} */ - template - std::shared_ptr loopClosureFalko::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){ - auto scanFALKO=std::make_shared(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - std::vector doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); - ((*scanFALKO).fromRanges)(doubleRanges); - return scanFALKO; - } +template +std::shared_ptr +loopClosureFalko::convert2LaserScanFALKO( + LaserScan &scan, LaserScanParams &scanParams) { + auto scanFALKO = std::make_shared( + scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + std::vector doubleRanges(scan.ranges_raw_.begin(), + scan.ranges_raw_.end()); + ((*scanFALKO).fromRanges)(doubleRanges); + return scanFALKO; +} /* template - typename loopClosureFalko::sceneFalkoBSCPtr loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams &scanParams){ + typename loopClosureFalko::sceneFalkoBSCPtr + loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams + &scanParams){ auto newScene=std::make_shared>(); - auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); + auto scanFALKO =loopClosureFalko::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 loopClosureFalko::matchLoopClosurePtr matchScene(std::shared_ptr> scene1,std::shared_ptr> scene2){ + typename loopClosureFalko::matchLoopClosurePtr + matchScene(std::shared_ptr> + scene1,std::shared_ptr> scene2){ std::vector > 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>(); new_match-> keypointsNumberMatch = matching_number; int keypoints_number_th=5; @@ -74,10 +82,9 @@ namespace laserscanutils{ return new_match; } */ - //void findLoopClosure(std::list& scenes, const cornerScene newScene){} - - +// void findLoopClosure(std::list& scenes, const cornerScene +// newScene){} - //Explicitly compile all the templates - template class loopClosureFalko ; +// Explicitly compile all the templates +template class loopClosureFalko; } 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 #include -#include +#include #include #include +#include /************************** * 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 -#include #include +#include +#include #include #include #include -#include #include +#include -namespace laserscanutils -{ +namespace laserscanutils { typedef falkolib::BSC bsc; typedef falkolib::BSCExtractor bscExtractor; typedef falkolib::NNMatcher NNMatcher; - - /** \brief A base class for loop closure using falko library - **/ +/** \brief A base class for loop closure using falko library +**/ template -class loopClosureFalko : public loopClosureBase2d, public falkolib::FALKOExtractor{ +class loopClosureFalko : public loopClosureBase2d, + public falkolib::FALKOExtractor { private: - public: + typedef std::shared_ptr> sceneFalkoBSCPtr; + typedef std::shared_ptr> matchLoopClosurePtr; - typedef std::shared_ptr> sceneFalkoBSCPtr; - typedef std::shared_ptr> 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>(); - auto scanFALKO =loopClosureFalko::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 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 > assoNN; - int matching_number = matcher_.match(scene1->keypointsList,scene2->keypointsList, assoNN); - auto new_match=std::make_shared>(); - 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>(); + auto scanFALKO = + loopClosureFalko::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 + 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> assoNN; + int matching_number = + matcher_.match(scene1->keypointsList, scene2->keypointsList, assoNN); + auto new_match = std::make_shared>(); + 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 findLoopClosure(std::list &l_scenes, const sceneFalkoBSCPtr &new_scene){ - int number_ref_sc = l_scenes.size(); - std::list matchings; - for (int i=0; i + findLoopClosure(std::list &l_scenes, + const sceneFalkoBSCPtr &new_scene) { + int number_ref_sc = l_scenes.size(); + std::list 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 #include +#include #include #include @@ -18,23 +18,20 @@ **************************/ #include "scene_falko.h" -namespace laserscanutils -{ +namespace laserscanutils { -template -struct matchLoopClosure{ - //std::shared_ptr> scene1; - //std::shared_ptr> scene2; - - // tuple that stores the pointers of two matched scenes - std::tuple>, std::shared_ptr>> sceneTuple; - bool match; - int keypointsNumberMatch; - double score; +template struct matchLoopClosure { + // std::shared_ptr> scene1; + // std::shared_ptr> scene2; + // tuple that stores the pointers of two matched scenes + std::tuple>, std::shared_ptr>> + 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 #include +#include +namespace laserscanutils { -namespace laserscanutils -{ - -struct sceneBase{ - //std::vector keypointsList; - //std::vector descriptorsList; +struct sceneBase { + // std::vector keypointsList; + // std::vector 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 #include +#include /************************** * LaserScanUtils includes * **************************/ #include "scene_base.h" - /************************** * Falko includes * **************************/ -#include -#include #include +#include +#include #include #include #include -#include #include +#include -namespace laserscanutils -{ +namespace laserscanutils { -template -struct sceneFalko : public sceneBase{ - std::vector keypointsList; - std::vector descriptorsList; +template struct sceneFalko : public sceneBase { + std::vector keypointsList; + std::vector 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 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>> 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 LCFalko; - std::shared_ptr scanFALKO=LCFalko.convert2LaserScanFALKO(scan, laserParams); + std::shared_ptr 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 From b4f6adfad88575b8d0974a98db8bad24e48fae6a Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 22 Feb 2021 09:51:09 +0100 Subject: [PATCH 051/100] passed all functions to loop_closure_falko.h. cpp file empty --- src/loop_closure_falko.cpp | 7 ++++++- src/loop_closure_falko.h | 31 ++++++++++++++++++++++++++++--- src/match_loop_closure.h | 2 -- 3 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp index c07eb63..0a41ac0 100644 --- a/src/loop_closure_falko.cpp +++ b/src/loop_closure_falko.cpp @@ -10,6 +10,7 @@ namespace laserscanutils { // CONSTRUCTOR +/* template loopClosureFalko::loopClosureFalko(int _circularSectorNumber, int _radialRingNumber) @@ -27,11 +28,14 @@ loopClosureFalko::loopClosureFalko(int _circularSectorNumber, // Matcher Extractor Parameters matcher_.setDistanceThreshold(0.1); } +*/ /* // DESTRUCTOR template loopClosureFalko::~loopClosureFalko(){} */ + +/* template std::shared_ptr loopClosureFalko::convert2LaserScanFALKO( @@ -43,6 +47,7 @@ loopClosureFalko::convert2LaserScanFALKO( ((*scanFALKO).fromRanges)(doubleRanges); return scanFALKO; } +*/ /* template typename loopClosureFalko::sceneFalkoBSCPtr @@ -86,5 +91,5 @@ loopClosureFalko::convert2LaserScanFALKO( // newScene){} // Explicitly compile all the templates -template class loopClosureFalko; +//template class loopClosureFalko; } diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 0e28aee..80a2d49 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -42,6 +42,10 @@ typedef falkolib::BSC bsc; typedef falkolib::BSCExtractor bscExtractor; typedef falkolib::NNMatcher NNMatcher; +typedef falkolib::CGH cgh; +typedef falkolib::CGHExtractor cghExtractor; +typedef falkolib::AHTMatcher AHTatcher; + /** \brief A base class for loop closure using falko library **/ @@ -52,13 +56,28 @@ private: public: typedef std::shared_ptr> sceneFalkoBSCPtr; typedef std::shared_ptr> matchLoopClosurePtr; + typedef std::shared_ptr 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 - convert2LaserScanFALKO(LaserScan &scan, LaserScanParams &scanParams); + laserScanPtr convert2LaserScanFALKO(LaserScan &scan, LaserScanParams &scanParams) { + auto scanFALKO = std::make_shared( + scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); + std::vector 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 2753a4b..41d1009 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -21,8 +21,6 @@ namespace laserscanutils { template struct matchLoopClosure { - // std::shared_ptr> scene1; - // std::shared_ptr> scene2; // tuple that stores the pointers of two matched scenes std::tuple>, std::shared_ptr>> -- GitLab From 3f2caf0ecac53f933e2ea2a73b03ffb21f78c98b Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 22 Feb 2021 10:30:48 +0100 Subject: [PATCH 052/100] added struct loopClosurParams --- src/loop_closure_falko.h | 58 +++++++++++++++++++++---------- test/gtest_loop_closure_falko.cpp | 8 +++-- 2 files changed, 46 insertions(+), 20 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 80a2d49..2d1689e 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -46,6 +46,25 @@ typedef falkolib::CGH cgh; typedef falkolib::CGHExtractor cghExtractor; typedef falkolib::AHTMatcher 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( - scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - std::vector 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( + _scan_params.angle_min_, _scan_params.angle_max_, _scan.ranges_raw_.size()); + std::vector 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>(); 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 caa15c5..0373da0 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 LCFalko; + parameterLoopClosureFalko param; + loopClosureFalko 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 LCFalko; + parameterLoopClosureFalko param; + loopClosureFalko LCFalko(param); + std::shared_ptr scanFALKO = LCFalko.convert2LaserScanFALKO(scan, laserParams); -- GitLab From cdec0e4bab2082b7abeecb27cc586e60e0443830 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 22 Feb 2021 10:49:31 +0100 Subject: [PATCH 053/100] code guidelines applyied --- src/loop_closure_falko.h | 36 +++++++-------- test/gtest_loop_closure_falko.cpp | 76 ++++++++----------------------- 2 files changed, 37 insertions(+), 75 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 2d1689e..6089b8a 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -105,16 +105,16 @@ public: /** \brief Create and update the scene struct with keypoints and descriptors **/ - sceneFalkoBSCPtr extractScene(LaserScan &scan, LaserScanParams &scanParams) { - auto newScene = std::make_shared>(); - auto scanFALKO = - loopClosureFalko::convert2LaserScanFALKO(scan, scanParams); + sceneFalkoBSCPtr extractScene(LaserScan &_scan, LaserScanParams &_scan_params) { + auto new_scene = std::make_shared>(); + auto scan_falko = + convert2LaserScanFALKO(_scan, _scan_params); // Extract keypoints - extract((*scanFALKO), (newScene->keypointsList)); + extract(*scan_falko, (new_scene->keypointsList)); // Compute descriptors - extractor_.compute(*scanFALKO, newScene->keypointsList, - newScene->descriptorsList); - return newScene; + extractor_.compute(*scan_falko, new_scene->keypointsList, + new_scene->descriptorsList); + return new_scene; } /** \brief Convert scans from laserscanutils::LaserScan to @@ -132,11 +132,11 @@ public: /** \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> assoNN; + matchLoopClosurePtr matchScene(sceneFalkoBSCPtr _scene1, + sceneFalkoBSCPtr _scene2) { + std::vector> asso_nn; int matching_number = - matcher_.match(scene1->keypointsList, scene2->keypointsList, assoNN); + matcher_.match(_scene1->keypointsList, _scene2->keypointsList, asso_nn); auto new_match = std::make_shared>(); new_match->keypointsNumberMatch = matching_number; if (matching_number > keypoints_number_th) { @@ -144,7 +144,7 @@ public: } else { new_match->match = false; } - new_match->sceneTuple = std::make_tuple(scene1, scene2); + new_match->sceneTuple = std::make_tuple(_scene1, _scene2); return new_match; } @@ -152,14 +152,14 @@ public: *closures **/ std::list - findLoopClosure(std::list &l_scenes, - const sceneFalkoBSCPtr &new_scene) { - int number_ref_sc = l_scenes.size(); + findLoopClosure(std::list &_l_scenes, + const sceneFalkoBSCPtr &_new_scene) { + int number_ref_sc = _l_scenes.size(); std::list matchings; for (int i = 0; i < number_ref_sc; i++) { - auto l_front = l_scenes.begin(); + auto l_front = _l_scenes.begin(); std::advance(l_front, i); - auto new_match = matchScene(*l_front, new_scene); + auto new_match = matchScene(*l_front, _new_scene); matchings.push_back(new_match); } return matchings; diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 0373da0..ac80971 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -6,89 +6,51 @@ using namespace laserscanutils; TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { - int scanSize = 1440; + // Initialization + int scan_size = 1440; LaserScan scan; - LaserScanParams laserParams; - - laserParams.angle_min_ = 0; - laserParams.angle_max_ = 2.0 * M_PI; - - for (int i = 0; i < scanSize; i++) { + LaserScanParams laser_params; + laser_params.angle_min_ = 0; + laser_params.angle_max_ = 2.0 * M_PI; + for (int i = 0; i < scan_size; i++) { scan.ranges_raw_.push_back(testRanges1[i]); } - parameterLoopClosureFalko param; - loopClosureFalko LCFalko(param); - - - auto new_scene = LCFalko.extractScene(scan, laserParams); + loopClosureFalko loop_cl_falko(param); - int detectedKeypoints = ((*new_scene).keypointsList).size(); + // Test convert2LaserScanFALKO + std::shared_ptr scan_falko = + loop_cl_falko.convert2LaserScanFALKO(scan, laser_params); + int firstPoint = scan_falko->ranges[0]; + ASSERT_EQ(firstPoint, 250); + + // Test extractScene + auto new_scene = loop_cl_falko.extractScene(scan, laser_params); + int detectedKeypoints = new_scene->keypointsList.size(); int detectedDescriptors = new_scene->descriptorsList.size(); - ASSERT_EQ(detectedKeypoints, 18); - ASSERT_EQ(detectedDescriptors, 18); - // Test matcher - - auto new_match = LCFalko.matchScene(new_scene, new_scene); - + // Test matcheScene + auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); int keypoints_matched = new_match->keypointsNumberMatch; - ASSERT_EQ(keypoints_matched, 18); // TEST findLoopClosure - std::list>> ref_scenes; - ref_scenes.push_back(new_scene); - ref_scenes.push_back(new_scene); - - auto matchings = LCFalko.findLoopClosure(ref_scenes, new_scene); - + auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); int matchings_number = matchings.size(); - ASSERT_EQ(matchings_number, 2); - auto l_front = matchings.begin(); - auto there_is_match = l_front->get()->match; - ASSERT_EQ(there_is_match, true); // PRINTF("All good at TestTest::DummyTestExample !\n"); } -TEST(loop_closure_falko2, convert2laserScanFalko) { - int scanSize = 1440; - - LaserScan scan; - LaserScanParams laserParams; - - 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]); - } - - parameterLoopClosureFalko param; - loopClosureFalko LCFalko(param); - - - std::shared_ptr scanFALKO = - LCFalko.convert2LaserScanFALKO(scan, laserParams); - - int firstPoint = ((*scanFALKO).ranges)[0]; - - ASSERT_EQ(firstPoint, 250); - - // PRINTF("All good at TestTest::DummyTestExample !\n"); -} - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -- GitLab From 49ced181a0d39af914b9cff178febfed52bc625a Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 22 Feb 2021 10:56:36 +0100 Subject: [PATCH 054/100] code guidelines --- src/loop_closure_falko.h | 58 ++++++++++++++++++++-------------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 6089b8a..7102075 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -47,22 +47,22 @@ typedef falkolib::CGHExtractor cghExtractor; typedef falkolib::AHTMatcher 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; + // Keypoints extractor Default + 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 Default + int circularSectorNumber_ = 16; + int radialRingNumber_ = 8; + + //matcher threshold Default + double matcher_distance_th_=0.1; + int keypoints_number_th_ = 5; }; /** \brief A base class for loop closure using falko library @@ -82,21 +82,21 @@ public: /** \brief Constructor **/ - loopClosureFalko(parameterLoopClosureFalko param) + loopClosureFalko(parameterLoopClosureFalko _param) : loopClosureBase2d(), falkolib::FALKOExtractor(), - extractor_(param._circularSectorNumber, param._radialRingNumber), matcher_() { + extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), matcher_() { // FALKO Extractor Parameters - 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); + 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(param.matcher_distance_th); - keypoints_number_th = param.keypoints_number_th; + matcher_.setDistanceThreshold(_param.matcher_distance_th_); + keypoints_number_th_ = _param.keypoints_number_th_; }; /** \brief Destructor @@ -139,7 +139,7 @@ public: matcher_.match(_scene1->keypointsList, _scene2->keypointsList, asso_nn); auto new_match = std::make_shared>(); new_match->keypointsNumberMatch = matching_number; - if (matching_number > keypoints_number_th) { + if (matching_number > keypoints_number_th_) { new_match->match = true; } else { new_match->match = false; @@ -165,7 +165,7 @@ public: return matchings; } - int keypoints_number_th; + int keypoints_number_th_; }; } /* namespace laserscanutils */ -- GitLab From 8beaf6cfcd7eb40bfc7721f332581460d93c4d22 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 22 Feb 2021 11:04:22 +0100 Subject: [PATCH 055/100] code guidelines --- src/loop_closure_falko.h | 8 ++++---- src/scene_falko.h | 4 ++-- test/gtest_loop_closure_falko.cpp | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 7102075..992f28a 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -110,10 +110,10 @@ public: auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); // Extract keypoints - extract(*scan_falko, (new_scene->keypointsList)); + extract(*scan_falko, (new_scene->keypoints_list_)); // Compute descriptors - extractor_.compute(*scan_falko, new_scene->keypointsList, - new_scene->descriptorsList); + extractor_.compute(*scan_falko, new_scene->keypoints_list_, + new_scene->descriptors_list_); return new_scene; } @@ -136,7 +136,7 @@ public: sceneFalkoBSCPtr _scene2) { std::vector> asso_nn; int matching_number = - matcher_.match(_scene1->keypointsList, _scene2->keypointsList, asso_nn); + matcher_.match(_scene1->keypoints_list_, _scene2->keypoints_list_, asso_nn); auto new_match = std::make_shared>(); new_match->keypointsNumberMatch = matching_number; if (matching_number > keypoints_number_th_) { diff --git a/src/scene_falko.h b/src/scene_falko.h index ef096ae..1bbdc3e 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -33,8 +33,8 @@ namespace laserscanutils { template struct sceneFalko : public sceneBase { - std::vector keypointsList; - std::vector descriptorsList; + std::vector keypoints_list_; + std::vector descriptors_list_; }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index ac80971..2e89f6d 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -27,8 +27,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { // Test extractScene auto new_scene = loop_cl_falko.extractScene(scan, laser_params); - int detectedKeypoints = new_scene->keypointsList.size(); - int detectedDescriptors = new_scene->descriptorsList.size(); + int detectedKeypoints = new_scene->keypoints_list_.size(); + int detectedDescriptors = new_scene->descriptors_list_.size(); ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedDescriptors, 18); -- GitLab From ca41f142dc776cf8eff7729b3365bb5c4ebe6e7b Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 22 Feb 2021 16:32:12 +0100 Subject: [PATCH 056/100] refactoring loop_closure_falko functions --- src/CMakeLists.txt | 4 +- src/loop_closure_base.h | 42 ++++++-------- src/loop_closure_falko.cpp | 95 ------------------------------- src/loop_closure_falko.h | 42 ++++++++------ src/match_loop_closure.h | 11 ++-- src/scene_base.h | 4 +- test/gtest_loop_closure_falko.cpp | 30 ++++++---- 7 files changed, 70 insertions(+), 158 deletions(-) delete mode 100644 src/loop_closure_falko.cpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2ce8fcf..6c0814b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -90,7 +90,6 @@ SET(SRCS point_set.cpp polyline.cpp scan_segment.cpp - loop_closure_base.cpp ) IF(csm_FOUND) SET(SRCS ${SRCS} @@ -99,8 +98,7 @@ SET(SRCS IF(falkolib_FOUND) SET(SRCS ${SRCS} - corner_falko_2d.cpp - loop_closure_falko.cpp) + corner_falko_2d.cpp) ENDIF(falkolib_FOUND) diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index 81c538c..61fc619 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -17,21 +17,8 @@ **************************/ #include "laser_scan.h" #include "scene_base.h" -#include "scene_falko.h" +#include "match_loop_closure.h" -/************************** - * Falko includes * - **************************/ -#include -#include -#include -#include - -#include -#include - -#include -#include namespace laserscanutils { @@ -42,23 +29,30 @@ private: public: /** \brief Constructor **/ - loopClosureBase2d(); + loopClosureBase2d(){}; /** \brief Destructor **/ - ~loopClosureBase2d(); + ~loopClosureBase2d(){}; - /** \brief compare new scans against the trained set in order to find loop - *closures - **/ - // virtual void findLoopClosure(std::list& scenes, const - // cornerScene newScene){} - // virtual void findLoopClosure(){} /** \brief update the scene struct with keypoints and descriptors **/ - virtual std::shared_ptr> - extractScene(LaserScan &scan, LaserScanParams &scanParams) {} + virtual sceneBasePtr + extractScene(LaserScan &scan, LaserScanParams &scanParams) =0; + + /** \brief Create and update a matchLoopClosure struct with the info that is + *produced when matching two given scenes + **/ + virtual matchLoopClosurePtr matchScene(sceneBasePtr _scene1, + sceneBasePtr _scene2) =0; + + /** \brief compare new scans against the trained set in order to find loop + *closures + **/ + virtual std::map + findLoopClosure(std::list _l_scenes, + const sceneBasePtr _new_scene)=0; }; } /* namespace laserscanutils */ diff --git a/src/loop_closure_falko.cpp b/src/loop_closure_falko.cpp deleted file mode 100644 index 0a41ac0..0000000 --- a/src/loop_closure_falko.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/** - * \file loop_closure_base_2d.h - * - * Created on: Feb 9, 2021 - * \author: spujol - */ - -#include "loop_closure_falko.h" - -namespace laserscanutils { - -// CONSTRUCTOR -/* -template -loopClosureFalko::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 - loopClosureFalko::~loopClosureFalko(){} -*/ - -/* -template -std::shared_ptr -loopClosureFalko::convert2LaserScanFALKO( - LaserScan &scan, LaserScanParams &scanParams) { - auto scanFALKO = std::make_shared( - scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size()); - std::vector doubleRanges(scan.ranges_raw_.begin(), - scan.ranges_raw_.end()); - ((*scanFALKO).fromRanges)(doubleRanges); - return scanFALKO; -} -*/ -/* - template - typename loopClosureFalko::sceneFalkoBSCPtr - loopClosureFalko::extractScene(LaserScan &scan,LaserScanParams - &scanParams){ - auto newScene=std::make_shared>(); - auto scanFALKO =loopClosureFalko::convert2LaserScanFALKO(scan, - scanParams); - // Extract keypoints - extract((*scanFALKO), (newScene->keypointsList)); - - // Compute descriptors - extractor_.compute(*scanFALKO,newScene->keypointsList, - newScene->descriptorsList); - return newScene; - } - */ -/* - template - typename loopClosureFalko::matchLoopClosurePtr - matchScene(std::shared_ptr> - scene1,std::shared_ptr> scene2){ - std::vector > assoNN; - int matching_number = match(scene1->keypointsList,scene2->keypointsList, - assoNN); - auto new_match=std::make_shared>(); - 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[0] = scene1; - new_match -> sceneTuple[1] = scene2; - - return new_match; - } -*/ -// void findLoopClosure(std::list& scenes, const cornerScene -// newScene){} - -// Explicitly compile all the templates -//template class loopClosureFalko; -} diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 992f28a..a8b8dd1 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -74,7 +74,6 @@ class loopClosureFalko : public loopClosureBase2d, private: public: typedef std::shared_ptr> sceneFalkoBSCPtr; - typedef std::shared_ptr> matchLoopClosurePtr; typedef std::shared_ptr laserScanPtr; Extr extractor_; @@ -105,7 +104,7 @@ public: /** \brief Create and update the scene struct with keypoints and descriptors **/ - sceneFalkoBSCPtr extractScene(LaserScan &_scan, LaserScanParams &_scan_params) { + sceneBasePtr extractScene(LaserScan &_scan, LaserScanParams &_scan_params) override{ auto new_scene = std::make_shared>(); auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); @@ -132,35 +131,42 @@ public: /** \brief Create and update a matchLoopClosure struct with the info that is *produced when matching two given scenes **/ - matchLoopClosurePtr matchScene(sceneFalkoBSCPtr _scene1, - sceneFalkoBSCPtr _scene2) { + matchLoopClosurePtr matchScene(sceneBasePtr _scene1, + sceneBasePtr _scene2) override{ std::vector> asso_nn; + auto scene_1_falko =std::static_pointer_cast>(_scene1); + auto scene_2_falko =std::static_pointer_cast>(_scene2); int matching_number = - matcher_.match(_scene1->keypoints_list_, _scene2->keypoints_list_, asso_nn); - auto new_match = std::make_shared>(); - new_match->keypointsNumberMatch = matching_number; + matcher_.match(scene_1_falko->keypoints_list_, + scene_2_falko->keypoints_list_, asso_nn); + auto new_match = std::make_shared(); + new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { new_match->match = true; } else { new_match->match = false; } - new_match->sceneTuple = std::make_tuple(_scene1, _scene2); + new_match->scene_1 =_scene1; + new_match->scene_2 =_scene2; + + new_match->score = (double) matching_number / (double) std::min(scene_1_falko->keypoints_list_.size(), + scene_2_falko->keypoints_list_.size()); return new_match; } /** \brief compare new scans against the trained set in order to find loop *closures **/ - std::list - findLoopClosure(std::list &_l_scenes, - const sceneFalkoBSCPtr &_new_scene) { - int number_ref_sc = _l_scenes.size(); - std::list 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); + std::map + findLoopClosure(std::list _l_scenes, + const sceneBasePtr _new_scene) override{ + auto new_scene_falko = std::static_pointer_cast>(_new_scene); + std::map matchings; + for (auto scene : _l_scenes) + { + auto ref_scene = std::static_pointer_cast>(scene); + auto match = matchScene(ref_scene, new_scene_falko); + matchings.emplace(match->score, match); } return matchings; } diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index 41d1009..7f6e4b2 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -20,16 +20,17 @@ namespace laserscanutils { -template struct matchLoopClosure { +struct matchLoopClosure { - // tuple that stores the pointers of two matched scenes - std::tuple>, std::shared_ptr>> - sceneTuple; + sceneBasePtr scene_1; + sceneBasePtr scene_2; bool match; - int keypointsNumberMatch; + int keypoints_number_match; double score; }; +typedef std::shared_ptr matchLoopClosurePtr; + } /* namespace laserscanutils */ #endif /* MATCH_LOOP_CLOSURE_H_ */ diff --git a/src/scene_base.h b/src/scene_base.h index 3511177..f27a92e 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -13,10 +13,10 @@ namespace laserscanutils { + struct sceneBase { - // std::vector keypointsList; - // std::vector descriptorsList; }; +typedef std::shared_ptr sceneBasePtr; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 2e89f6d..0e9012e 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -8,13 +8,15 @@ using namespace laserscanutils; TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { // Initialization int scan_size = 1440; - LaserScan scan; + LaserScan scan, scan2; LaserScanParams laser_params; laser_params.angle_min_ = 0; laser_params.angle_max_ = 2.0 * M_PI; for (int i = 0; i < scan_size; i++) { scan.ranges_raw_.push_back(testRanges1[i]); + scan2.ranges_raw_.push_back(testRanges2[i]); } + parameterLoopClosureFalko param; loopClosureFalko loop_cl_falko(param); @@ -26,7 +28,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { ASSERT_EQ(firstPoint, 250); // Test extractScene - auto new_scene = loop_cl_falko.extractScene(scan, laser_params); + auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, laser_params)); + auto new_scene2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); int detectedKeypoints = new_scene->keypoints_list_.size(); int detectedDescriptors = new_scene->descriptors_list_.size(); ASSERT_EQ(detectedKeypoints, 18); @@ -34,20 +37,25 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { // Test matcheScene auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); - int keypoints_matched = new_match->keypointsNumberMatch; - ASSERT_EQ(keypoints_matched, 18); + ASSERT_EQ(new_match->keypoints_number_match, 18); // TEST findLoopClosure std::list>> ref_scenes; + ref_scenes.push_back(new_scene); - ref_scenes.push_back(new_scene); - auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); - int matchings_number = matchings.size(); - ASSERT_EQ(matchings_number, 2); - auto l_front = matchings.begin(); - auto there_is_match = l_front->get()->match; - ASSERT_EQ(there_is_match, true); + ref_scenes.push_back(new_scene2); + + //auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); + /* + ASSERT_EQ(matchings.size(), 2); + auto best_match = matchings.rbegin()->second; + ASSERT_EQ(best_match->match, true); + + ASSERT_EQ(best_match->scene_1, new_scene); + ASSERT_EQ(best_match->scene_2, new_scene); + ASSERT_EQ(best_match->score, 1); +*/ // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From bdd5ae6e1c7b5dd2f38b3c798e56dbf5f8e59339 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 25 Feb 2021 11:19:56 +0100 Subject: [PATCH 057/100] scaneFalkoList created, functions adapted with static_pointer_cast --- src/loop_closure_base.h | 2 +- src/loop_closure_falko.h | 17 +++++++++-------- src/scene_base.h | 7 +++++-- src/scene_falko.h | 10 +++++++++- test/gtest_loop_closure_falko.cpp | 19 +++++++++---------- 5 files changed, 33 insertions(+), 22 deletions(-) diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index 61fc619..775b6f6 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -51,7 +51,7 @@ public: *closures **/ virtual std::map - findLoopClosure(std::list _l_scenes, + findLoopClosure(sceneBaseListPtr _l_scenes, const sceneBasePtr _new_scene)=0; }; } /* namespace laserscanutils */ diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index a8b8dd1..32674fa 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -73,7 +73,7 @@ class loopClosureFalko : public loopClosureBase2d, public falkolib::FALKOExtractor { private: public: - typedef std::shared_ptr> sceneFalkoBSCPtr; + typedef std::shared_ptr> sceneFalkoBSCPtr; typedef std::shared_ptr laserScanPtr; Extr extractor_; @@ -105,7 +105,7 @@ public: /** \brief Create and update the scene struct with keypoints and descriptors **/ sceneBasePtr extractScene(LaserScan &_scan, LaserScanParams &_scan_params) override{ - auto new_scene = std::make_shared>(); + auto new_scene = std::make_shared>(); auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); // Extract keypoints @@ -134,8 +134,8 @@ public: matchLoopClosurePtr matchScene(sceneBasePtr _scene1, sceneBasePtr _scene2) override{ std::vector> asso_nn; - auto scene_1_falko =std::static_pointer_cast>(_scene1); - auto scene_2_falko =std::static_pointer_cast>(_scene2); + auto scene_1_falko =std::static_pointer_cast>(_scene1); + auto scene_2_falko =std::static_pointer_cast>(_scene2); int matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); @@ -158,13 +158,14 @@ public: *closures **/ std::map - findLoopClosure(std::list _l_scenes, + findLoopClosure(sceneBaseListPtr _l_scenes, const sceneBasePtr _new_scene) override{ - auto new_scene_falko = std::static_pointer_cast>(_new_scene); + auto _l_scenes_falko = std::static_pointer_cast>(_l_scenes); + auto new_scene_falko = std::static_pointer_cast>(_new_scene); std::map matchings; - for (auto scene : _l_scenes) + for (auto scene : _l_scenes_falko->scenes_) { - auto ref_scene = std::static_pointer_cast>(scene); + auto ref_scene = std::static_pointer_cast>(scene); auto match = matchScene(ref_scene, new_scene_falko); matchings.emplace(match->score, match); } diff --git a/src/scene_base.h b/src/scene_base.h index f27a92e..98b38ce 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -13,10 +13,13 @@ namespace laserscanutils { +struct SceneBase { +}; +typedef std::shared_ptr sceneBasePtr; -struct sceneBase { +struct SceneBaseList { }; -typedef std::shared_ptr sceneBasePtr; +typedef std::shared_ptr sceneBaseListPtr; } /* namespace laserscanutils */ diff --git a/src/scene_falko.h b/src/scene_falko.h index 1bbdc3e..01c2fe1 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -10,6 +10,9 @@ #include #include +#include +#include +#include /************************** * LaserScanUtils includes * @@ -32,11 +35,16 @@ namespace laserscanutils { -template struct sceneFalko : public sceneBase { +template struct SceneFalko : public SceneBase { std::vector keypoints_list_; std::vector descriptors_list_; }; +template struct SceneFalkoList : public SceneBaseList { +public: + std::list>> scenes_; +}; + } /* namespace laserscanutils */ #endif /* SCENE_FALKO_H_ */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 0e9012e..a8ab3ea 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -28,8 +28,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { ASSERT_EQ(firstPoint, 250); // Test extractScene - auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, laser_params)); - auto new_scene2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); + auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, laser_params)); + auto new_scene2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); int detectedKeypoints = new_scene->keypoints_list_.size(); int detectedDescriptors = new_scene->descriptors_list_.size(); ASSERT_EQ(detectedKeypoints, 18); @@ -40,13 +40,12 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { ASSERT_EQ(new_match->keypoints_number_match, 18); // TEST findLoopClosure - std::list>> ref_scenes; - - ref_scenes.push_back(new_scene); - ref_scenes.push_back(new_scene2); - - //auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); - /* + auto ref_scenes= std::make_shared>(); + ref_scenes->scenes_.emplace_back(new_scene); + ref_scenes->scenes_.emplace_back(new_scene2); + + auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); + ASSERT_EQ(matchings.size(), 2); auto best_match = matchings.rbegin()->second; ASSERT_EQ(best_match->match, true); @@ -55,7 +54,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { ASSERT_EQ(best_match->scene_2, new_scene); ASSERT_EQ(best_match->score, 1); -*/ + // PRINTF("All good at TestTest::DummyTestExample !\n"); } -- GitLab From 2213dd700c237764d68f79383341031d3adb2ca6 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 25 Feb 2021 11:28:49 +0100 Subject: [PATCH 058/100] code guide lines --- src/loop_closure_base.h | 6 +++--- src/loop_closure_falko.h | 12 ++++++------ src/match_loop_closure.h | 4 ++-- test/gtest_loop_closure_falko.cpp | 6 ++---- 4 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index 775b6f6..983ae1e 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -24,16 +24,16 @@ namespace laserscanutils { /** \brief A 2base class for loop closure using falko library */ -class loopClosureBase2d { +class LoopClosureBase2d { private: public: /** \brief Constructor **/ - loopClosureBase2d(){}; + LoopClosureBase2d(){}; /** \brief Destructor **/ - ~loopClosureBase2d(){}; + ~LoopClosureBase2d(){}; /** \brief update the scene struct with keypoints and descriptors diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 32674fa..e9def7e 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -46,7 +46,7 @@ typedef falkolib::CGH cgh; typedef falkolib::CGHExtractor cghExtractor; typedef falkolib::AHTMatcher AHTatcher; -struct parameterLoopClosureFalko { +struct ParameterLoopClosureFalko { // Keypoints extractor Default double min_extraction_range_=0.1; double max_extraction_range_=25; @@ -69,7 +69,7 @@ struct parameterLoopClosureFalko { **/ template -class loopClosureFalko : public loopClosureBase2d, +class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor { private: public: @@ -81,8 +81,8 @@ public: /** \brief Constructor **/ - loopClosureFalko(parameterLoopClosureFalko _param) - : loopClosureBase2d(), falkolib::FALKOExtractor(), + LoopClosureFalko(ParameterLoopClosureFalko _param) + : LoopClosureBase2d(), falkolib::FALKOExtractor(), extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), matcher_() { // FALKO Extractor Parameters setMinExtractionRange(_param.min_extraction_range_); @@ -100,7 +100,7 @@ public: /** \brief Destructor **/ - ~loopClosureFalko() {} + ~LoopClosureFalko() {} /** \brief Create and update the scene struct with keypoints and descriptors **/ @@ -139,7 +139,7 @@ public: int matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); - auto new_match = std::make_shared(); + auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { new_match->match = true; diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index 7f6e4b2..b51d850 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -20,7 +20,7 @@ namespace laserscanutils { -struct matchLoopClosure { +struct MatchLoopClosure { sceneBasePtr scene_1; sceneBasePtr scene_2; @@ -29,7 +29,7 @@ struct matchLoopClosure { double score; }; -typedef std::shared_ptr matchLoopClosurePtr; +typedef std::shared_ptr matchLoopClosurePtr; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index a8ab3ea..04c70b7 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -17,8 +17,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { scan2.ranges_raw_.push_back(testRanges2[i]); } - parameterLoopClosureFalko param; - loopClosureFalko loop_cl_falko(param); + ParameterLoopClosureFalko param; + LoopClosureFalko loop_cl_falko(param); // Test convert2LaserScanFALKO std::shared_ptr scan_falko = @@ -49,9 +49,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { ASSERT_EQ(matchings.size(), 2); auto best_match = matchings.rbegin()->second; ASSERT_EQ(best_match->match, true); - ASSERT_EQ(best_match->scene_1, new_scene); - ASSERT_EQ(best_match->scene_2, new_scene); ASSERT_EQ(best_match->score, 1); -- GitLab From bf3c8291677d91bc52a2ffd040c221d7ce31e42c Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 25 Feb 2021 12:48:10 +0100 Subject: [PATCH 059/100] gtest modified --- test/gtest_loop_closure_falko.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 04c70b7..14e0f96 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -16,7 +16,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { scan.ranges_raw_.push_back(testRanges1[i]); scan2.ranges_raw_.push_back(testRanges2[i]); } - + ParameterLoopClosureFalko param; LoopClosureFalko loop_cl_falko(param); @@ -26,10 +26,12 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { int firstPoint = scan_falko->ranges[0]; ASSERT_EQ(firstPoint, 250); - + // Test extractScene - auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, laser_params)); - auto new_scene2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); + auto new_scene = std::static_pointer_cast>( + loop_cl_falko.extractScene(scan, laser_params)); + auto new_scene2 = std::static_pointer_cast>( + loop_cl_falko.extractScene(scan2, laser_params)); int detectedKeypoints = new_scene->keypoints_list_.size(); int detectedDescriptors = new_scene->descriptors_list_.size(); ASSERT_EQ(detectedKeypoints, 18); @@ -39,8 +41,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); ASSERT_EQ(new_match->keypoints_number_match, 18); - // TEST findLoopClosure - auto ref_scenes= std::make_shared>(); + // TesT findLoopClosure + auto ref_scenes = std::make_shared>(); ref_scenes->scenes_.emplace_back(new_scene); ref_scenes->scenes_.emplace_back(new_scene2); -- GitLab From 084e3ae5ef86326e5f2cc0579098232bd712c338 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Fri, 26 Feb 2021 11:44:55 +0100 Subject: [PATCH 060/100] added id to MatchLoopClosure --- src/loop_closure_falko.h | 12 ++++++------ src/match_loop_closure.h | 1 + 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index e9def7e..ccca82a 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -131,11 +131,11 @@ public: /** \brief Create and update a matchLoopClosure struct with the info that is *produced when matching two given scenes **/ - matchLoopClosurePtr matchScene(sceneBasePtr _scene1, - sceneBasePtr _scene2) override{ + matchLoopClosurePtr matchScene(sceneBasePtr _scene_1, + sceneBasePtr _scene_2) override{ std::vector> asso_nn; - auto scene_1_falko =std::static_pointer_cast>(_scene1); - auto scene_2_falko =std::static_pointer_cast>(_scene2); + auto scene_1_falko =std::static_pointer_cast>(_scene_1); + auto scene_2_falko =std::static_pointer_cast>(_scene_2); int matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); @@ -146,8 +146,8 @@ public: } else { new_match->match = false; } - new_match->scene_1 =_scene1; - new_match->scene_2 =_scene2; + new_match->scene_1 =_scene_1; + new_match->scene_2 =_scene_2; new_match->score = (double) matching_number / (double) std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index b51d850..ab359c4 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -27,6 +27,7 @@ struct MatchLoopClosure { bool match; int keypoints_number_match; double score; + double id; }; typedef std::shared_ptr matchLoopClosurePtr; -- GitLab From fad8484791eb7fdc8f7664f55f9380a17b3fd404 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Fri, 26 Feb 2021 11:48:28 +0100 Subject: [PATCH 061/100] added id to sceneFalko --- src/match_loop_closure.h | 1 - src/scene_falko.h | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index ab359c4..b51d850 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -27,7 +27,6 @@ struct MatchLoopClosure { bool match; int keypoints_number_match; double score; - double id; }; typedef std::shared_ptr matchLoopClosurePtr; diff --git a/src/scene_falko.h b/src/scene_falko.h index 01c2fe1..ce8e999 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -38,6 +38,7 @@ namespace laserscanutils { template struct SceneFalko : public SceneBase { std::vector keypoints_list_; std::vector descriptors_list_; + int id; }; template struct SceneFalkoList : public SceneBaseList { -- GitLab From fdee62089bab3774d5624c4eefb96d5cc73f57bb Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Fri, 26 Feb 2021 12:59:13 +0100 Subject: [PATCH 062/100] match_loop_closure id --- src/match_loop_closure.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index b51d850..93a2c41 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -22,8 +22,8 @@ namespace laserscanutils { struct MatchLoopClosure { - sceneBasePtr scene_1; - sceneBasePtr scene_2; + sceneBasePtr scene_1; //ref_scene + sceneBasePtr scene_2; //new_scene bool match; int keypoints_number_match; double score; -- GitLab From e2d0413cb2817d9ab9e283ca5e284e664059eb2f Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 1 Mar 2021 10:16:28 +0100 Subject: [PATCH 063/100] added options to test matcher with or without decriptors --- src/loop_closure_falko.h | 141 +++++++++++++++++++++++---------------- 1 file changed, 82 insertions(+), 59 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index ccca82a..bb5443e 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -40,35 +40,38 @@ namespace laserscanutils { typedef falkolib::BSC bsc; typedef falkolib::BSCExtractor bscExtractor; -typedef falkolib::NNMatcher NNMatcher; - typedef falkolib::CGH cgh; typedef falkolib::CGHExtractor cghExtractor; -typedef falkolib::AHTMatcher AHTatcher; + +template using NNMatcher = falkolib::NNMatcher; +template using AHTmatcher = falkolib::AHTMatcher; struct ParameterLoopClosureFalko { // Keypoints extractor Default - 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 Default + 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 Default int circularSectorNumber_ = 16; int radialRingNumber_ = 8; - //matcher threshold Default - double matcher_distance_th_=0.1; + // matcher threshold Default + double matcher_distance_th_ = 0.2; int keypoints_number_th_ = 5; + int match_type = 1; // match_type=1-> uses keypoints and descriptors for + // matching. match_type=2-> uses only keypoints for + // matching }; /** \brief A base class for loop closure using falko library **/ -template +template typename M> class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor { private: @@ -77,26 +80,29 @@ public: typedef std::shared_ptr laserScanPtr; Extr extractor_; - M matcher_; + M matcher_; + M matcher_desc_; /** \brief Constructor **/ LoopClosureFalko(ParameterLoopClosureFalko _param) - : LoopClosureBase2d(), falkolib::FALKOExtractor(), - extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), matcher_() { - // FALKO Extractor Parameters - 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(_param.matcher_distance_th_); - keypoints_number_th_ = _param.keypoints_number_th_; -}; + : LoopClosureBase2d(), falkolib::FALKOExtractor(), + extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), + matcher_(), matcher_desc_() { + // FALKO Extractor Parameters + 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(_param.matcher_distance_th_); + matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_); + keypoints_number_th_ = _param.keypoints_number_th_; + }; /** \brief Destructor **/ @@ -104,10 +110,10 @@ public: /** \brief Create and update the scene struct with keypoints and descriptors **/ - sceneBasePtr extractScene(LaserScan &_scan, LaserScanParams &_scan_params) override{ + sceneBasePtr extractScene(LaserScan &_scan, + LaserScanParams &_scan_params) override { auto new_scene = std::make_shared>(); - auto scan_falko = - convert2LaserScanFALKO(_scan, _scan_params); + auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); // Extract keypoints extract(*scan_falko, (new_scene->keypoints_list_)); // Compute descriptors @@ -119,26 +125,43 @@ public: /** \brief Convert scans from laserscanutils::LaserScan to *falkolib::LaserScan object **/ - laserScanPtr convert2LaserScanFALKO(LaserScan &_scan, LaserScanParams &_scan_params) { - auto scan_falko = std::make_shared( - _scan_params.angle_min_, _scan_params.angle_max_, _scan.ranges_raw_.size()); - std::vector double_ranges(_scan.ranges_raw_.begin(), - _scan.ranges_raw_.end()); - scan_falko->fromRanges(double_ranges); - return scan_falko; -}; + laserScanPtr convert2LaserScanFALKO(LaserScan &_scan, + LaserScanParams &_scan_params) { + auto scan_falko = std::make_shared( + _scan_params.angle_min_, _scan_params.angle_max_, + _scan.ranges_raw_.size()); + std::vector 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 *produced when matching two given scenes **/ matchLoopClosurePtr matchScene(sceneBasePtr _scene_1, - sceneBasePtr _scene_2) override{ + sceneBasePtr _scene_2) override { std::vector> asso_nn; - auto scene_1_falko =std::static_pointer_cast>(_scene_1); - auto scene_2_falko =std::static_pointer_cast>(_scene_2); - int matching_number = - matcher_.match(scene_1_falko->keypoints_list_, - scene_2_falko->keypoints_list_, asso_nn); + auto scene_1_falko = std::static_pointer_cast>(_scene_1); + auto scene_2_falko = std::static_pointer_cast>(_scene_2); + int match_type = 1; + int matching_number; + if (match_type = 1) { + matching_number = matcher_.match(scene_1_falko->keypoints_list_, + scene_2_falko->keypoints_list_, asso_nn); + // std::cout << "scene_1_falko->descriptors_list_ : " + // << scene_1_falko->descriptors_list_[0] << std::endl; + + } else if (match_type = 2) { + matching_number = matcher_.match( + scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, + scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, + asso_nn); + } else if (match_type = 3) { + matching_number = matcher_desc_.match(scene_1_falko->descriptors_list_, + scene_2_falko->descriptors_list_, asso_nn); + } + auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { @@ -146,25 +169,26 @@ public: } else { new_match->match = false; } - new_match->scene_1 =_scene_1; - new_match->scene_2 =_scene_2; + new_match->scene_1 = _scene_1; + new_match->scene_2 = _scene_2; - new_match->score = (double) matching_number / (double) std::min(scene_1_falko->keypoints_list_.size(), - scene_2_falko->keypoints_list_.size()); + new_match->score = (double)matching_number / + (double)std::min(scene_1_falko->keypoints_list_.size(), + scene_2_falko->keypoints_list_.size()); return new_match; } /** \brief compare new scans against the trained set in order to find loop *closures **/ - std::map + std::map findLoopClosure(sceneBaseListPtr _l_scenes, - const sceneBasePtr _new_scene) override{ - auto _l_scenes_falko = std::static_pointer_cast>(_l_scenes); + const sceneBasePtr _new_scene) override { + auto _l_scenes_falko = + std::static_pointer_cast>(_l_scenes); auto new_scene_falko = std::static_pointer_cast>(_new_scene); - std::map matchings; - for (auto scene : _l_scenes_falko->scenes_) - { + std::map matchings; + for (auto scene : _l_scenes_falko->scenes_) { auto ref_scene = std::static_pointer_cast>(scene); auto match = matchScene(ref_scene, new_scene_falko); matchings.emplace(match->score, match); @@ -173,7 +197,6 @@ public: } int keypoints_number_th_; - }; } /* namespace laserscanutils */ -- GitLab From c82419ded06675a8d38ab60e9a973299cc4fff1a Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Mar 2021 10:46:41 +0100 Subject: [PATCH 064/100] added computation of transform in match_loop_closure struct --- src/loop_closure_falko.h | 7 +++++-- src/match_loop_closure.h | 1 + test/gtest_loop_closure_falko.cpp | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index bb5443e..6b08cfb 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -43,8 +43,8 @@ typedef falkolib::BSCExtractor bscExtractor; typedef falkolib::CGH cgh; typedef falkolib::CGHExtractor cghExtractor; -template using NNMatcher = falkolib::NNMatcher; -template using AHTmatcher = falkolib::AHTMatcher; +template using nn_matcher = falkolib::NNMatcher; +template using aht_matcher = falkolib::AHTMatcher; struct ParameterLoopClosureFalko { // Keypoints extractor Default @@ -175,6 +175,9 @@ public: new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); + + computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform); + return new_match; } diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index 93a2c41..ce87de4 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -27,6 +27,7 @@ struct MatchLoopClosure { bool match; int keypoints_number_match; double score; + Eigen::Affine2d transform; }; typedef std::shared_ptr matchLoopClosurePtr; diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 14e0f96..d0bc343 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -18,7 +18,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { } ParameterLoopClosureFalko param; - LoopClosureFalko loop_cl_falko(param); + LoopClosureFalko loop_cl_falko(param); // Test convert2LaserScanFALKO std::shared_ptr scan_falko = -- GitLab From a8d6cbb5e16bb432246bf2db6b2211eee70b1fab Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Mar 2021 12:33:43 +0100 Subject: [PATCH 065/100] adapted to add matcher aht --- src/loop_closure_falko.h | 29 ++++++++++++++++++----------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 6b08cfb..e1252dd 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -36,6 +36,8 @@ #include #include +using namespace falkolib; + namespace laserscanutils { typedef falkolib::BSC bsc; @@ -66,6 +68,14 @@ struct ParameterLoopClosureFalko { int match_type = 1; // match_type=1-> uses keypoints and descriptors for // matching. match_type=2-> uses only keypoints for // matching + + // aht matcher + double xRes_=0.1; + double yRes_=0.1; + double thetaRes_=0.04; + double xAbsMax_=5; + double yAbsMax_=5; + double thetaAbsMax_=1.57; }; /** \brief A base class for loop closure using falko library @@ -81,14 +91,14 @@ public: Extr extractor_; M matcher_; - M matcher_desc_; + //M matcher_desc_; - /** \brief Constructor + /** \brief Constructor for nn_matcher **/ LoopClosureFalko(ParameterLoopClosureFalko _param) : LoopClosureBase2d(), falkolib::FALKOExtractor(), extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), - matcher_(), matcher_desc_() { + matcher_() { // FALKO Extractor Parameters setMinExtractionRange(_param.min_extraction_range_); setMaxExtractionRange(_param.max_extraction_range_); @@ -100,7 +110,7 @@ public: // Matcher Extractor Parameters matcher_.setDistanceThreshold(_param.matcher_distance_th_); - matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_); + //matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_); keypoints_number_th_ = _param.keypoints_number_th_; }; @@ -149,23 +159,22 @@ public: if (match_type = 1) { matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); - // std::cout << "scene_1_falko->descriptors_list_ : " - // << scene_1_falko->descriptors_list_[0] << std::endl; } else if (match_type = 2) { matching_number = matcher_.match( scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn); - } else if (match_type = 3) { + + } /*else if (match_type = 3) { matching_number = matcher_desc_.match(scene_1_falko->descriptors_list_, scene_2_falko->descriptors_list_, asso_nn); } - +*/ auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { - new_match->match = true; + new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform); } else { new_match->match = false; } @@ -176,8 +185,6 @@ public: (double)std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); - computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform); - return new_match; } -- GitLab From 87c05d1567cc133f07e85d1fcb3cc472e1d3bb6b Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Mar 2021 12:37:07 +0100 Subject: [PATCH 066/100] namespace error corrected --- src/loop_closure_falko.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index e1252dd..6878405 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -36,8 +36,6 @@ #include #include -using namespace falkolib; - namespace laserscanutils { typedef falkolib::BSC bsc; -- GitLab From 523ce247e82c5dbc49744666e48623e4a08af720 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Mar 2021 13:09:19 +0100 Subject: [PATCH 067/100] findLoopClosure passed to loop_closure_base.h --- src/loop_closure_base.h | 11 +++++++++-- src/loop_closure_falko.h | 5 ++++- src/match_loop_closure.h | 4 ---- src/scene_base.h | 1 + test/gtest_loop_closure_falko.cpp | 10 +++++++--- 5 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index 983ae1e..d8b64e1 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -51,8 +51,15 @@ public: *closures **/ virtual std::map - findLoopClosure(sceneBaseListPtr _l_scenes, - const sceneBasePtr _new_scene)=0; + findLoopClosure(std::list> _l_scenes,const sceneBasePtr _new_scene){ + std::map matchings; + for (auto ref_scene : _l_scenes) { + auto match = matchScene(ref_scene, _new_scene); + matchings.emplace(match->score, match); + } + return matchings; + } + }; } /* namespace laserscanutils */ diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 6878405..a8a0779 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -189,6 +189,7 @@ public: /** \brief compare new scans against the trained set in order to find loop *closures **/ +/* std::map findLoopClosure(sceneBaseListPtr _l_scenes, const sceneBasePtr _new_scene) override { @@ -203,9 +204,11 @@ public: } return matchings; } - +*/ int keypoints_number_th_; }; + + } /* namespace laserscanutils */ #endif /* LOOP_CLOSURE_FALKO_H_ */ diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index ce87de4..fab431d 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -13,10 +13,6 @@ #include #include -/************************** - * laser_scan_utils includes * - **************************/ -#include "scene_falko.h" namespace laserscanutils { diff --git a/src/scene_base.h b/src/scene_base.h index 98b38ce..5e5a8d1 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -18,6 +18,7 @@ struct SceneBase { typedef std::shared_ptr sceneBasePtr; struct SceneBaseList { + std::list> scenes_; }; typedef std::shared_ptr sceneBaseListPtr; diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index d0bc343..e88305c 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -42,9 +42,13 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { ASSERT_EQ(new_match->keypoints_number_match, 18); // TesT findLoopClosure - auto ref_scenes = std::make_shared>(); - ref_scenes->scenes_.emplace_back(new_scene); - ref_scenes->scenes_.emplace_back(new_scene2); + //auto ref_scenes = std::make_shared>(); + //ref_scenes->scenes_.emplace_back(new_scene); + //ref_scenes->scenes_.emplace_back(new_scene2); + + std::list> ref_scenes; + ref_scenes.emplace_back(new_scene); + ref_scenes.emplace_back(new_scene2); auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); -- GitLab From c0889682b5e7106b25cf7921214abbac4a801b6c Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Mar 2021 15:07:49 +0100 Subject: [PATCH 068/100] corrected error in MatchLoopClosure --- src/loop_closure_base.h | 30 ++++++++++++++---------------- src/match_loop_closure.h | 2 ++ 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index d8b64e1..1140c3b 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -16,42 +16,41 @@ * laser_scan_utils includes * **************************/ #include "laser_scan.h" -#include "scene_base.h" #include "match_loop_closure.h" - +#include "scene_base.h" namespace laserscanutils { /** \brief A 2base class for loop closure using falko library -*/ + */ class LoopClosureBase2d { private: public: /** \brief Constructor - **/ + **/ LoopClosureBase2d(){}; /** \brief Destructor - **/ + **/ ~LoopClosureBase2d(){}; - /** \brief update the scene struct with keypoints and descriptors **/ - virtual sceneBasePtr - extractScene(LaserScan &scan, LaserScanParams &scanParams) =0; - + virtual sceneBasePtr extractScene(LaserScan &scan, + LaserScanParams &scanParams) = 0; + /** \brief Create and update a matchLoopClosure struct with the info that is - *produced when matching two given scenes + *produced when matching two given scenes **/ virtual matchLoopClosurePtr matchScene(sceneBasePtr _scene1, - sceneBasePtr _scene2) =0; - + sceneBasePtr _scene2) = 0; + /** \brief compare new scans against the trained set in order to find loop - *closures + *closures **/ - virtual std::map - findLoopClosure(std::list> _l_scenes,const sceneBasePtr _new_scene){ + virtual std::map + findLoopClosure(std::list> _l_scenes, + const sceneBasePtr _new_scene) { std::map matchings; for (auto ref_scene : _l_scenes) { auto match = matchScene(ref_scene, _new_scene); @@ -59,7 +58,6 @@ public: } return matchings; } - }; } /* namespace laserscanutils */ diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index fab431d..145db4e 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -14,6 +14,8 @@ #include +#include "scene_base.h" + namespace laserscanutils { struct MatchLoopClosure { -- GitLab From 9bb1be542622feef385c8e455dd0b57a39803414 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 2 Mar 2021 16:52:07 +0100 Subject: [PATCH 069/100] work in progress --- src/loop_closure_falko.h | 20 +++++++++++++------- src/match_loop_closure.h | 1 - 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index a8a0779..ca6095d 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -63,7 +63,7 @@ struct ParameterLoopClosureFalko { // matcher threshold Default double matcher_distance_th_ = 0.2; int keypoints_number_th_ = 5; - int match_type = 1; // match_type=1-> uses keypoints and descriptors for + int match_type_ = 1; // match_type=1-> uses keypoints and descriptors for // matching. match_type=2-> uses only keypoints for // matching @@ -110,6 +110,7 @@ public: matcher_.setDistanceThreshold(_param.matcher_distance_th_); //matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_); keypoints_number_th_ = _param.keypoints_number_th_; + match_type_=_param.match_type_; }; /** \brief Destructor @@ -152,23 +153,24 @@ public: std::vector> asso_nn; auto scene_1_falko = std::static_pointer_cast>(_scene_1); auto scene_2_falko = std::static_pointer_cast>(_scene_2); - int match_type = 1; int matching_number; - if (match_type = 1) { + if (match_type_ == 1) { matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); - - } else if (match_type = 2) { + std::cout << "AAAA" << std::endl; + } else if (match_type_ == 2) { matching_number = matcher_.match( scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn); + std::cout << "BBB" << std::endl; - } /*else if (match_type = 3) { + } /*else if (match_type_ = 3) { matching_number = matcher_desc_.match(scene_1_falko->descriptors_list_, scene_2_falko->descriptors_list_, asso_nn); } */ +std::cout << "matching_number : " << matching_number << std::endl; auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { @@ -182,7 +184,10 @@ public: new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); - + std::cout << "score : " << new_match->score << std::endl; + std::cout << "matching_number : " << matching_number << std::endl; + std::cout << "kp1 : " << scene_1_falko->keypoints_list_.size() << std::endl; + std::cout << "kp2 : " << scene_2_falko->keypoints_list_.size() << std::endl; return new_match; } @@ -206,6 +211,7 @@ public: } */ int keypoints_number_th_; + int match_type_; }; diff --git a/src/match_loop_closure.h b/src/match_loop_closure.h index 145db4e..b6880ed 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure.h @@ -13,7 +13,6 @@ #include #include - #include "scene_base.h" namespace laserscanutils { -- GitLab From d3ba4a537d259bba81ee832b7fbefbddd9b644e8 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 4 Mar 2021 09:33:34 +0100 Subject: [PATCH 070/100] findLoopClosure function in LoopClosureBase class --- src/loop_closure_falko.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index ca6095d..e107659 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -108,6 +108,7 @@ public: // Matcher Extractor Parameters matcher_.setDistanceThreshold(_param.matcher_distance_th_); + matcher_.setDescriptorThreshold(0.3); //matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_); keypoints_number_th_ = _param.keypoints_number_th_; match_type_=_param.match_type_; @@ -153,24 +154,22 @@ public: std::vector> asso_nn; auto scene_1_falko = std::static_pointer_cast>(_scene_1); auto scene_2_falko = std::static_pointer_cast>(_scene_2); - int matching_number; + int matching_number =0; + if (match_type_ == 1) { matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); - std::cout << "AAAA" << std::endl; } else if (match_type_ == 2) { matching_number = matcher_.match( scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn); - std::cout << "BBB" << std::endl; - + } /*else if (match_type_ = 3) { matching_number = matcher_desc_.match(scene_1_falko->descriptors_list_, scene_2_falko->descriptors_list_, asso_nn); } */ -std::cout << "matching_number : " << matching_number << std::endl; auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { @@ -184,10 +183,12 @@ std::cout << "matching_number : " << matching_number << std::endl; new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); + /* std::cout << "score : " << new_match->score << std::endl; std::cout << "matching_number : " << matching_number << std::endl; std::cout << "kp1 : " << scene_1_falko->keypoints_list_.size() << std::endl; std::cout << "kp2 : " << scene_2_falko->keypoints_list_.size() << std::endl; + */ return new_match; } -- GitLab From 62dbfe70c30e4ff07ba542153f3fd06315124609 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 4 Mar 2021 11:41:57 +0100 Subject: [PATCH 071/100] some comments deleted --- src/loop_closure_falko.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index e107659..34a35be 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -183,12 +183,6 @@ public: new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); - /* - std::cout << "score : " << new_match->score << std::endl; - std::cout << "matching_number : " << matching_number << std::endl; - std::cout << "kp1 : " << scene_1_falko->keypoints_list_.size() << std::endl; - std::cout << "kp2 : " << scene_2_falko->keypoints_list_.size() << std::endl; - */ return new_match; } -- GitLab From 12e27911905b8b254a9769ffe8495d1f658b76f8 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 9 Mar 2021 11:19:24 +0100 Subject: [PATCH 072/100] added parameter match_ddesc_th --- src/loop_closure_falko.h | 83 +++++++++++++++++++++------------------- 1 file changed, 43 insertions(+), 40 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 34a35be..2f2ddb6 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -44,7 +44,8 @@ typedef falkolib::CGH cgh; typedef falkolib::CGHExtractor cghExtractor; template using nn_matcher = falkolib::NNMatcher; -template using aht_matcher = falkolib::AHTMatcher; +template +using aht_matcher = falkolib::AHTMatcher; struct ParameterLoopClosureFalko { // Keypoints extractor Default @@ -64,20 +65,21 @@ struct ParameterLoopClosureFalko { double matcher_distance_th_ = 0.2; int keypoints_number_th_ = 5; int match_type_ = 1; // match_type=1-> uses keypoints and descriptors for - // matching. match_type=2-> uses only keypoints for - // matching - - // aht matcher - double xRes_=0.1; - double yRes_=0.1; - double thetaRes_=0.04; - double xAbsMax_=5; - double yAbsMax_=5; - double thetaAbsMax_=1.57; + // matching. match_type=2-> uses only keypoints for + // matching + double matcher_ddesc_th_=0.2; + + // aht matcher + double xRes_ = 0.1; + double yRes_ = 0.1; + double thetaRes_ = 0.04; + double xAbsMax_ = 5; + double yAbsMax_ = 5; + double thetaAbsMax_ = 1.57; }; /** \brief A base class for loop closure using falko library -**/ + **/ template typename M> class LoopClosureFalko : public LoopClosureBase2d, @@ -89,10 +91,10 @@ public: Extr extractor_; M matcher_; - //M matcher_desc_; + // M matcher_desc_; /** \brief Constructor for nn_matcher - **/ + **/ LoopClosureFalko(ParameterLoopClosureFalko _param) : LoopClosureBase2d(), falkolib::FALKOExtractor(), extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), @@ -108,14 +110,13 @@ public: // Matcher Extractor Parameters matcher_.setDistanceThreshold(_param.matcher_distance_th_); - matcher_.setDescriptorThreshold(0.3); - //matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_); + matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_); keypoints_number_th_ = _param.keypoints_number_th_; - match_type_=_param.match_type_; + match_type_ = _param.match_type_; }; /** \brief Destructor - **/ + **/ ~LoopClosureFalko() {} /** \brief Create and update the scene struct with keypoints and descriptors @@ -133,7 +134,7 @@ public: } /** \brief Convert scans from laserscanutils::LaserScan to - *falkolib::LaserScan object + *falkolib::LaserScan object **/ laserScanPtr convert2LaserScanFALKO(LaserScan &_scan, LaserScanParams &_scan_params) { @@ -147,14 +148,14 @@ public: } /** \brief Create and update a matchLoopClosure struct with the info that is - *produced when matching two given scenes + *produced when matching two given scenes **/ matchLoopClosurePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override { std::vector> asso_nn; auto scene_1_falko = std::static_pointer_cast>(_scene_1); auto scene_2_falko = std::static_pointer_cast>(_scene_2); - int matching_number =0; + int matching_number = 0; if (match_type_ == 1) { matching_number = matcher_.match(scene_1_falko->keypoints_list_, @@ -166,14 +167,17 @@ public: asso_nn); } /*else if (match_type_ = 3) { - matching_number = matcher_desc_.match(scene_1_falko->descriptors_list_, + matching_number = + matcher_desc_.match(scene_1_falko->descriptors_list_, scene_2_falko->descriptors_list_, asso_nn); } */ auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { - new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform); + new_match->match = computeTransform(scene_1_falko->keypoints_list_, + scene_2_falko->keypoints_list_, + asso_nn, new_match->transform); } else { new_match->match = false; } @@ -187,29 +191,28 @@ public: } /** \brief compare new scans against the trained set in order to find loop - *closures + *closures **/ -/* - std::map - findLoopClosure(sceneBaseListPtr _l_scenes, - const sceneBasePtr _new_scene) override { - auto _l_scenes_falko = - std::static_pointer_cast>(_l_scenes); - auto new_scene_falko = std::static_pointer_cast>(_new_scene); - std::map matchings; - for (auto scene : _l_scenes_falko->scenes_) { - auto ref_scene = std::static_pointer_cast>(scene); - auto match = matchScene(ref_scene, new_scene_falko); - matchings.emplace(match->score, match); + /* + std::map + findLoopClosure(sceneBaseListPtr _l_scenes, + const sceneBasePtr _new_scene) override { + auto _l_scenes_falko = + std::static_pointer_cast>(_l_scenes); + auto new_scene_falko = + std::static_pointer_cast>(_new_scene); std::map matchings; for (auto scene : _l_scenes_falko->scenes_) + { auto ref_scene = std::static_pointer_cast>(scene); auto + match = matchScene(ref_scene, new_scene_falko); + matchings.emplace(match->score, match); + } + return matchings; } - return matchings; - } -*/ + */ int keypoints_number_th_; int match_type_; }; - } /* namespace laserscanutils */ #endif /* LOOP_CLOSURE_FALKO_H_ */ -- GitLab From c874d6a45cbd7e7c529942c8f11e3a9b33771701 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 9 Mar 2021 15:00:35 +0100 Subject: [PATCH 073/100] refactor --- src/loop_closure_falko.h | 40 ++++++++++++---------------------------- 1 file changed, 12 insertions(+), 28 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 2f2ddb6..f2e9b2d 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -12,6 +12,7 @@ #include #include #include +#include #include /************************** @@ -67,10 +68,10 @@ struct ParameterLoopClosureFalko { int match_type_ = 1; // match_type=1-> uses keypoints and descriptors for // matching. match_type=2-> uses only keypoints for // matching - double matcher_ddesc_th_=0.2; + double matcher_ddesc_th_ = 0.2; - // aht matcher - double xRes_ = 0.1; + // aht matcher + double xRes_ = 0.1; double yRes_ = 0.1; double thetaRes_ = 0.04; double xAbsMax_ = 5; @@ -152,9 +153,16 @@ public: **/ matchLoopClosurePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override { + + // int n_rotations = 8; + // for (i = 0; i < n_rotations; i++) { + // double rotation = (2 * M_PI / n_rotations) * i; std::vector> asso_nn; auto scene_1_falko = std::static_pointer_cast>(_scene_1); auto scene_2_falko = std::static_pointer_cast>(_scene_2); + // //Rotate all descriptors of the list + // for (j=0; j<) + // scene_2_falko->descriptors_list_[j] int matching_number = 0; if (match_type_ == 1) { @@ -165,13 +173,7 @@ public: scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn); - - } /*else if (match_type_ = 3) { - matching_number = - matcher_desc_.match(scene_1_falko->descriptors_list_, - scene_2_falko->descriptors_list_, asso_nn); } -*/ auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { @@ -187,28 +189,10 @@ public: new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); + //} return new_match; } - /** \brief compare new scans against the trained set in order to find loop - *closures - **/ - /* - std::map - findLoopClosure(sceneBaseListPtr _l_scenes, - const sceneBasePtr _new_scene) override { - auto _l_scenes_falko = - std::static_pointer_cast>(_l_scenes); - auto new_scene_falko = - std::static_pointer_cast>(_new_scene); std::map matchings; for (auto scene : _l_scenes_falko->scenes_) - { auto ref_scene = std::static_pointer_cast>(scene); auto - match = matchScene(ref_scene, new_scene_falko); - matchings.emplace(match->score, match); - } - return matchings; - } - */ int keypoints_number_th_; int match_type_; }; -- GitLab From 43282a1cc731056ef2c2ce03ea3be6d271423660 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 18 Mar 2021 09:18:01 +0100 Subject: [PATCH 074/100] comments deleted --- src/loop_closure_falko.h | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index f2e9b2d..59de67a 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -154,15 +154,10 @@ public: matchLoopClosurePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override { - // int n_rotations = 8; - // for (i = 0; i < n_rotations; i++) { - // double rotation = (2 * M_PI / n_rotations) * i; std::vector> asso_nn; auto scene_1_falko = std::static_pointer_cast>(_scene_1); auto scene_2_falko = std::static_pointer_cast>(_scene_2); - // //Rotate all descriptors of the list - // for (j=0; j<) - // scene_2_falko->descriptors_list_[j] + int matching_number = 0; if (match_type_ == 1) { -- GitLab From a4d5c236cb5f818aadf97089b538c70670905811 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 30 Mar 2021 14:55:27 +0200 Subject: [PATCH 075/100] falkolib commit updated, some refactor --- deps/falkolib | 2 +- src/loop_closure_base.h | 4 ++-- src/loop_closure_falko.h | 10 ++++------ src/scene_falko.h | 3 +++ 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/deps/falkolib b/deps/falkolib index 327bafa..0017c6c 160000 --- a/deps/falkolib +++ b/deps/falkolib @@ -1 +1 @@ -Subproject commit 327bafa8ee1d9ea545d9766415986ea13a1064d1 +Subproject commit 0017c6c5be963b923684bfd8eb6bd8b7641ba1b6 diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index 1140c3b..1109b2d 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -36,8 +36,8 @@ public: /** \brief update the scene struct with keypoints and descriptors **/ - virtual sceneBasePtr extractScene(LaserScan &scan, - LaserScanParams &scanParams) = 0; + virtual sceneBasePtr extractScene(const LaserScan &scan, + const LaserScanParams &scanParams) = 0; /** \brief Create and update a matchLoopClosure struct with the info that is *produced when matching two given scenes diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 59de67a..6d8b763 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -39,9 +39,7 @@ namespace laserscanutils { -typedef falkolib::BSC bsc; typedef falkolib::BSCExtractor bscExtractor; -typedef falkolib::CGH cgh; typedef falkolib::CGHExtractor cghExtractor; template using nn_matcher = falkolib::NNMatcher; @@ -122,8 +120,8 @@ public: /** \brief Create and update the scene struct with keypoints and descriptors **/ - sceneBasePtr extractScene(LaserScan &_scan, - LaserScanParams &_scan_params) override { + sceneBasePtr extractScene(const LaserScan &_scan, + const LaserScanParams &_scan_params) override { auto new_scene = std::make_shared>(); auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); // Extract keypoints @@ -137,8 +135,8 @@ public: /** \brief Convert scans from laserscanutils::LaserScan to *falkolib::LaserScan object **/ - laserScanPtr convert2LaserScanFALKO(LaserScan &_scan, - LaserScanParams &_scan_params) { + laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, + const LaserScanParams &_scan_params) { auto scan_falko = std::make_shared( _scan_params.angle_min_, _scan_params.angle_max_, _scan.ranges_raw_.size()); diff --git a/src/scene_falko.h b/src/scene_falko.h index ce8e999..b74aa91 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -35,6 +35,9 @@ namespace laserscanutils { +typedef falkolib::BSC bsc; +typedef falkolib::CGH cgh; + template struct SceneFalko : public SceneBase { std::vector keypoints_list_; std::vector descriptors_list_; -- GitLab From 71c3fef2902b4bd1cf4dff13f777de932f680da1 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 6 Apr 2021 11:16:16 +0200 Subject: [PATCH 076/100] scene base id added --- src/scene_base.h | 9 +++------ src/scene_falko.h | 7 +------ 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/scene_base.h b/src/scene_base.h index 5e5a8d1..a753162 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -13,15 +13,12 @@ namespace laserscanutils { -struct SceneBase { +struct SceneBase +{ + int id; }; typedef std::shared_ptr sceneBasePtr; -struct SceneBaseList { - std::list> scenes_; -}; -typedef std::shared_ptr sceneBaseListPtr; - } /* namespace laserscanutils */ #endif /* SCENE_BASE_H_ */ diff --git a/src/scene_falko.h b/src/scene_falko.h index b74aa91..cb14ddf 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -40,14 +40,9 @@ typedef falkolib::CGH cgh; template struct SceneFalko : public SceneBase { std::vector keypoints_list_; - std::vector descriptors_list_; - int id; + std::vector descriptors_list_; }; -template struct SceneFalkoList : public SceneBaseList { -public: - std::list>> scenes_; -}; } /* namespace laserscanutils */ -- GitLab From 20a993d6cb4186ef43b147870f812be1a3868362 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 6 Apr 2021 17:19:09 +0200 Subject: [PATCH 077/100] renaming to MatchLoopClosureScene --- src/CMakeLists.txt | 2 +- src/loop_closure_base.h | 8 ++++---- src/loop_closure_falko.h | 6 +++--- ...match_loop_closure.h => match_loop_closure_scene.h} | 10 +++++----- 4 files changed, 13 insertions(+), 13 deletions(-) rename src/{match_loop_closure.h => match_loop_closure_scene.h} (66%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6c0814b..fee4770 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -70,7 +70,7 @@ SET(HDRS corner_falko_2d.h loop_closure_falko.h scene_falko.h - match_loop_closure.h) + match_loop_closure_scene.h) ENDIF(falkolib_FOUND) #sources diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index 1109b2d..fe790cf 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -16,7 +16,7 @@ * laser_scan_utils includes * **************************/ #include "laser_scan.h" -#include "match_loop_closure.h" +#include "match_loop_closure_scene.h" #include "scene_base.h" namespace laserscanutils { @@ -42,16 +42,16 @@ public: /** \brief Create and update a matchLoopClosure struct with the info that is *produced when matching two given scenes **/ - virtual matchLoopClosurePtr matchScene(sceneBasePtr _scene1, + virtual MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene1, sceneBasePtr _scene2) = 0; /** \brief compare new scans against the trained set in order to find loop *closures **/ - virtual std::map + virtual std::map findLoopClosure(std::list> _l_scenes, const sceneBasePtr _new_scene) { - std::map matchings; + std::map matchings; for (auto ref_scene : _l_scenes) { auto match = matchScene(ref_scene, _new_scene); matchings.emplace(match->score, match); diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 6d8b763..75a194b 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -20,7 +20,7 @@ **************************/ #include "laser_scan.h" #include "loop_closure_base.h" -#include "match_loop_closure.h" +#include "match_loop_closure_scene.h" #include "scene_falko.h" /************************** @@ -149,7 +149,7 @@ public: /** \brief Create and update a matchLoopClosure struct with the info that is *produced when matching two given scenes **/ - matchLoopClosurePtr matchScene(sceneBasePtr _scene_1, + MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override { std::vector> asso_nn; @@ -167,7 +167,7 @@ public: scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn); } - auto new_match = std::make_shared(); + auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; if (matching_number > keypoints_number_th_) { new_match->match = computeTransform(scene_1_falko->keypoints_list_, diff --git a/src/match_loop_closure.h b/src/match_loop_closure_scene.h similarity index 66% rename from src/match_loop_closure.h rename to src/match_loop_closure_scene.h index b6880ed..47bfa22 100644 --- a/src/match_loop_closure.h +++ b/src/match_loop_closure_scene.h @@ -5,8 +5,8 @@ * \author: spujol */ -#ifndef MATCH_LOOP_CLOSURE_H_ -#define MATCH_LOOP_CLOSURE_H_ +#ifndef MATCH_LOOP_CLOSURE_SCENE_H_ +#define MATCH_LOOP_CLOSURE_SCENE_H_ #include #include @@ -17,7 +17,7 @@ namespace laserscanutils { -struct MatchLoopClosure { +struct MatchLoopClosureScene { sceneBasePtr scene_1; //ref_scene sceneBasePtr scene_2; //new_scene @@ -27,8 +27,8 @@ struct MatchLoopClosure { Eigen::Affine2d transform; }; -typedef std::shared_ptr matchLoopClosurePtr; +typedef std::shared_ptr MatchLoopClosureScenePtr; } /* namespace laserscanutils */ -#endif /* MATCH_LOOP_CLOSURE_H_ */ +#endif /* MATCH_LOOP_CLOSURE_SCENE_H_ */ -- GitLab From 6f7311ce00d2ff96779430f4940590f063533ee6 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 6 Apr 2021 21:45:35 +0200 Subject: [PATCH 078/100] renaming use_descriptors --- src/loop_closure_falko.h | 279 +++++++++++++++--------------- test/gtest_loop_closure_falko.cpp | 109 ++++++------ 2 files changed, 192 insertions(+), 196 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 75a194b..ecfda98 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -42,152 +42,155 @@ namespace laserscanutils { typedef falkolib::BSCExtractor bscExtractor; typedef falkolib::CGHExtractor cghExtractor; -template using nn_matcher = falkolib::NNMatcher; -template -using aht_matcher = falkolib::AHTMatcher; - -struct ParameterLoopClosureFalko { - // Keypoints extractor Default - 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 Default - int circularSectorNumber_ = 16; - int radialRingNumber_ = 8; - - // matcher threshold Default - double matcher_distance_th_ = 0.2; - int keypoints_number_th_ = 5; - int match_type_ = 1; // match_type=1-> uses keypoints and descriptors for - // matching. match_type=2-> uses only keypoints for - // matching - double matcher_ddesc_th_ = 0.2; - - // aht matcher - double xRes_ = 0.1; - double yRes_ = 0.1; - double thetaRes_ = 0.04; - double xAbsMax_ = 5; - double yAbsMax_ = 5; - double thetaAbsMax_ = 1.57; +template using nn_matcher = falkolib::NNMatcher; +template using aht_matcher = falkolib::AHTMatcher; + +struct ParameterLoopClosureFalko +{ + // Keypoints extractor Default + 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 Default + int circularSectorNumber_ = 16; + int radialRingNumber_ = 8; + + // matcher threshold Default + double matcher_distance_th_ = 0.2; + int keypoints_number_th_ = 5; + bool use_descriptors_ = 1; // match_type=1-> uses keypoints and descriptors + // match_type=0-> uses only keypoints + + // matching + double matcher_ddesc_th_ = 0.2; + + // aht matcher + double xRes_ = 0.1; + double yRes_ = 0.1; + double thetaRes_ = 0.04; + double xAbsMax_ = 5; + double yAbsMax_ = 5; + double thetaAbsMax_ = 1.57; }; /** \brief A base class for loop closure using falko library **/ template typename M> -class LoopClosureFalko : public LoopClosureBase2d, - public falkolib::FALKOExtractor { -private: -public: - typedef std::shared_ptr> sceneFalkoBSCPtr; - typedef std::shared_ptr laserScanPtr; - - Extr extractor_; - M matcher_; - // M matcher_desc_; - - /** \brief Constructor for nn_matcher - **/ - LoopClosureFalko(ParameterLoopClosureFalko _param) - : LoopClosureBase2d(), falkolib::FALKOExtractor(), - extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), - matcher_() { - // FALKO Extractor Parameters - 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(_param.matcher_distance_th_); - matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_); - keypoints_number_th_ = _param.keypoints_number_th_; - match_type_ = _param.match_type_; - }; - - /** \brief Destructor - **/ - ~LoopClosureFalko() {} - - /** \brief Create and update the scene struct with keypoints and descriptors - **/ - sceneBasePtr extractScene(const LaserScan &_scan, - const LaserScanParams &_scan_params) override { - auto new_scene = std::make_shared>(); - auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); - // Extract keypoints - extract(*scan_falko, (new_scene->keypoints_list_)); - // Compute descriptors - extractor_.compute(*scan_falko, new_scene->keypoints_list_, - new_scene->descriptors_list_); - return new_scene; - } - - /** \brief Convert scans from laserscanutils::LaserScan to - *falkolib::LaserScan object - **/ - laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, - const LaserScanParams &_scan_params) { - auto scan_falko = std::make_shared( - _scan_params.angle_min_, _scan_params.angle_max_, - _scan.ranges_raw_.size()); - std::vector 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 - *produced when matching two given scenes - **/ - MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, - sceneBasePtr _scene_2) override { - - std::vector> asso_nn; - auto scene_1_falko = std::static_pointer_cast>(_scene_1); - auto scene_2_falko = std::static_pointer_cast>(_scene_2); - - int matching_number = 0; - - if (match_type_ == 1) { - matching_number = matcher_.match(scene_1_falko->keypoints_list_, - scene_2_falko->keypoints_list_, asso_nn); - } else if (match_type_ == 2) { - matching_number = matcher_.match( - scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, - scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, - asso_nn); +class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor +{ + private: + public: + typedef std::shared_ptr> sceneFalkoBSCPtr; + typedef std::shared_ptr laserScanPtr; + + Extr extractor_; + M matcher_; + // M matcher_desc_; + + /** \brief Constructor for nn_matcher + **/ + LoopClosureFalko(ParameterLoopClosureFalko _param) + : LoopClosureBase2d() + , falkolib::FALKOExtractor() + , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_) + , matcher_() + { + // FALKO Extractor Parameters + 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(_param.matcher_distance_th_); + matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_); + keypoints_number_th_ = _param.keypoints_number_th_; + use_descriptors_ = _param.use_descriptors_; + }; + + /** \brief Destructor + **/ + ~LoopClosureFalko() {} + + /** \brief Create and update the scene struct with keypoints and descriptors + **/ + sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override + { + auto new_scene = std::make_shared>(); + auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); + // Extract keypoints + extract(*scan_falko, (new_scene->keypoints_list_)); + // Compute descriptors + extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); + return new_scene; } - auto new_match = std::make_shared(); - new_match->keypoints_number_match = matching_number; - if (matching_number > keypoints_number_th_) { - new_match->match = computeTransform(scene_1_falko->keypoints_list_, - scene_2_falko->keypoints_list_, - asso_nn, new_match->transform); - } else { - new_match->match = false; + + /** \brief Convert scans from laserscanutils::LaserScan to + *falkolib::LaserScan object + **/ + laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params) + { + auto scan_falko = std::make_shared(_scan_params.angle_min_, _scan_params.angle_max_, + _scan.ranges_raw_.size()); + std::vector 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 + *produced when matching two given scenes + **/ + MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override + { + + std::vector> asso_nn; + auto scene_1_falko = std::static_pointer_cast>(_scene_1); + auto scene_2_falko = std::static_pointer_cast>(_scene_2); + + int matching_number = 0; + + if (use_descriptors_ == 1) + { + matching_number = + matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); + } + else if (use_descriptors_ == 0) + { + matching_number = + matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, + scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn); + } + auto new_match = std::make_shared(); + new_match->keypoints_number_match = matching_number; + if (matching_number > keypoints_number_th_) + { + new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, + asso_nn, new_match->transform); + } + else + { + new_match->match = false; + } + new_match->scene_1 = _scene_1; + new_match->scene_2 = _scene_2; + + new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), + scene_2_falko->keypoints_list_.size()); + //} + return new_match; } - new_match->scene_1 = _scene_1; - new_match->scene_2 = _scene_2; - - new_match->score = (double)matching_number / - (double)std::min(scene_1_falko->keypoints_list_.size(), - scene_2_falko->keypoints_list_.size()); - //} - return new_match; - } - - int keypoints_number_th_; - int match_type_; + + int keypoints_number_th_; + bool use_descriptors_; }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index e88305c..f01b38b 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -5,64 +5,57 @@ using namespace laserscanutils; -TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { - // Initialization - int scan_size = 1440; - LaserScan scan, scan2; - LaserScanParams laser_params; - laser_params.angle_min_ = 0; - laser_params.angle_max_ = 2.0 * M_PI; - for (int i = 0; i < scan_size; i++) { - scan.ranges_raw_.push_back(testRanges1[i]); - scan2.ranges_raw_.push_back(testRanges2[i]); - } - - ParameterLoopClosureFalko param; - LoopClosureFalko loop_cl_falko(param); - - // Test convert2LaserScanFALKO - std::shared_ptr scan_falko = - loop_cl_falko.convert2LaserScanFALKO(scan, laser_params); - int firstPoint = scan_falko->ranges[0]; - - ASSERT_EQ(firstPoint, 250); - - // Test extractScene - auto new_scene = std::static_pointer_cast>( - loop_cl_falko.extractScene(scan, laser_params)); - auto new_scene2 = std::static_pointer_cast>( - loop_cl_falko.extractScene(scan2, laser_params)); - int detectedKeypoints = new_scene->keypoints_list_.size(); - int detectedDescriptors = new_scene->descriptors_list_.size(); - ASSERT_EQ(detectedKeypoints, 18); - ASSERT_EQ(detectedDescriptors, 18); - - // Test matcheScene - auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); - ASSERT_EQ(new_match->keypoints_number_match, 18); - - // TesT findLoopClosure - //auto ref_scenes = std::make_shared>(); - //ref_scenes->scenes_.emplace_back(new_scene); - //ref_scenes->scenes_.emplace_back(new_scene2); - - std::list> ref_scenes; - ref_scenes.emplace_back(new_scene); - ref_scenes.emplace_back(new_scene2); - - auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); - - ASSERT_EQ(matchings.size(), 2); - auto best_match = matchings.rbegin()->second; - ASSERT_EQ(best_match->match, true); - ASSERT_EQ(best_match->scene_1, new_scene); - ASSERT_EQ(best_match->scene_2, new_scene); - ASSERT_EQ(best_match->score, 1); - - // PRINTF("All good at TestTest::DummyTestExample !\n"); +TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) +{ + // Initialization + int scan_size = 1440; + LaserScan scan, scan2; + LaserScanParams laser_params; + laser_params.angle_min_ = 0; + laser_params.angle_max_ = 2.0 * M_PI; + for (int i = 0; i < scan_size; i++) + { + scan.ranges_raw_.push_back(testRanges1[i]); + scan2.ranges_raw_.push_back(testRanges2[i]); + } + + ParameterLoopClosureFalko param; + LoopClosureFalko loop_cl_falko(param); + + // Test convert2LaserScanFALKO + std::shared_ptr scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params); + int firstPoint = scan_falko->ranges[0]; + + ASSERT_EQ(firstPoint, 250); + + // Test extractScene + auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, laser_params)); + auto new_scene2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); + int detectedKeypoints = new_scene->keypoints_list_.size(); + int detectedDescriptors = new_scene->descriptors_list_.size(); + ASSERT_EQ(detectedKeypoints, 18); + ASSERT_EQ(detectedDescriptors, 18); + + // Test matcheScene + auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); + ASSERT_EQ(new_match->keypoints_number_match, 18); + + std::list> ref_scenes; + ref_scenes.emplace_back(new_scene); + ref_scenes.emplace_back(new_scene2); + + auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); + + ASSERT_EQ(matchings.size(), 2); + auto best_match = matchings.rbegin()->second; + ASSERT_EQ(best_match->match, true); + ASSERT_EQ(best_match->scene_1, new_scene); + ASSERT_EQ(best_match->scene_2, new_scene); + ASSERT_EQ(best_match->score, 1); } -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } -- GitLab From 0d816fbd50fc215e01226b4b3f6ac9e6c8e50693 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Wed, 14 Apr 2021 16:06:59 +0200 Subject: [PATCH 079/100] unused files deleted --- src/CMakeLists.txt | 5 -- src/corner_falko_2d.cpp | 121 ----------------------------- src/corner_falko_2d.h | 119 ---------------------------- src/examples/CMakeLists.txt | 4 - src/examples/corner_falko_demo.cpp | 49 ------------ src/examples/testData.cpp | 23 ------ src/examples/testData.h | 28 ------- 7 files changed, 349 deletions(-) delete mode 100644 src/corner_falko_2d.cpp delete mode 100644 src/corner_falko_2d.h delete mode 100644 src/examples/corner_falko_demo.cpp delete mode 100644 src/examples/testData.cpp delete mode 100644 src/examples/testData.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fee4770..71ab644 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -67,7 +67,6 @@ SET(HDRS IF(falkolib_FOUND) SET(HDRS ${HDRS} - corner_falko_2d.h loop_closure_falko.h scene_falko.h match_loop_closure_scene.h) @@ -96,10 +95,6 @@ SET(SRCS icp.cpp) ENDIF(csm_FOUND) - IF(falkolib_FOUND) - SET(SRCS ${SRCS} - corner_falko_2d.cpp) - ENDIF(falkolib_FOUND) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp deleted file mode 100644 index edce4b9..0000000 --- a/src/corner_falko_2d.cpp +++ /dev/null @@ -1,121 +0,0 @@ -/** - * \file corner_falko_2d.cpp - * - * Created on: Jan 26, 2021 - * \author: spujol - */ - -#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() {} - -void CornerFalko2d::AddNewReferenceScene(falkolib::LaserScan scanFALKO) { - - // Extract keypoints - lastKeypointSet.clear(); - extract(scanFALKO, lastKeypointSet); - - // Compute descriptors - lastDescriptorSet.clear(); - compute(scanFALKO, lastKeypointSet, lastDescriptorSet); - - keypointSets.push_back(lastKeypointSet); - descriptorSets.push_back(lastDescriptorSet); - - scansExtracted = scansExtracted + 1; -} - -void CornerFalko2d::findLoopClosure(LaserScan scan, - LaserScanParams scanParams) { - - falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams); - - // Compute descriptors - std::vector keypointSet2; - extract(scanFALKO, keypointSet2); - - // Compute descriptors - std::vector descriptorSet2; - compute(scanFALKO, keypointSet2, descriptorSet2); - - // Matching - int rows = keypointSets.size(); - - numberKeypointsMatch = 0; - numberSceneMatch = 0; - matchingPosition = -1; - for (int i = 0; i < rows; i++) { - std::vector> assoNN; - - 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; - } -} - -falkolib::LaserScan -CornerFalko2d::convert2LaserScanFALKO(LaserScan scan, - LaserScanParams scanParams) { - - falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, - scan.ranges_raw_.size()); - std::vector doubleRanges(scan.ranges_raw_.begin(), - scan.ranges_raw_.end()); - scanFALKO.fromRanges(doubleRanges); - return scanFALKO; -} - -int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan, - LaserScanParams scanParams, - int scanInterval) { - - scanNumber = scanNumber + 1; - - int NewSceneAdded = 0; - - if (scanNumber % scanInterval == 0 || scanNumber == 1) { - - findLoopClosure(scan, scanParams); - - if (numberKeypointsMatch < refSceneAddingTh) { - - falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams); - - AddNewReferenceScene(scanFALKO); - - NewSceneAdded = 1; - } - } - - return NewSceneAdded; -} - -} // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h deleted file mode 100644 index 89513e6..0000000 --- a/src/corner_falko_2d.h +++ /dev/null @@ -1,119 +0,0 @@ -/** - * \file corner_falko_2d.h - * - * Created on: Jan 26, 2021 - * \author: spujol - */ - -#ifndef CORNER_FALKO_2D_H_ -#define CORNER_FALKO_2D_H_ - -#include -#include - -/************************** - * laser_scan_utils includes * - **************************/ -#include "laser_scan.h" - -/************************** - * WOLF includes * - **************************/ -//#include "core/landmark/landmark_base.h" - -/************************** - * Falko includes * - **************************/ -#include -#include -#include -#include - -#include -#include - -#include -#include - -namespace laserscanutils { - -/** \brief A 2D corner extractor and loop closure computing class -**/ - -class CornerFalko2d : public falkolib::FALKOExtractor, - public falkolib::BSCExtractor, - public falkolib::NNMatcher { -private: - /** \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> keypointSets; - std::vector lastKeypointSet; - std::vector> descriptorSets; - std::vector 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 */ - -#endif /* LANDMARK_POLYLINE_2d_H_ */ diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 40bd1f1..63e4efb 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -3,7 +3,3 @@ INCLUDE_DIRECTORIES(../src) # polylines demo ADD_EXECUTABLE(demo_polylines polyline_demo.cpp) TARGET_LINK_LIBRARIES(demo_polylines ${PROJECT_NAME}) - -# falkocorners demo -ADD_EXECUTABLE(demo_corner_falko corner_falko_demo.cpp testData.cpp) -TARGET_LINK_LIBRARIES(demo_corner_falko ${PROJECT_NAME}) diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp deleted file mode 100644 index 8baa356..0000000 --- a/src/examples/corner_falko_demo.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/** - * \file corner_falko_2d_test.cpp - * - * Created on: Jan 26, 2021 - * \author: spujol - */ - -//LaserScanUtils includes -#include "../line_finder_iterative.h" -#include "../corner_falko_2d.h" -#include "../laser_scan.h" -#include "testData.h" - - -//std includes -#include -#include -using namespace laserscanutils; - -int main(int argc, char** argv) -{ - - int scanSize = 1440; - int scanInterval =1; - - LaserScan scan; - LaserScanParams laserParams; - - laserParams.angle_min_=0; - laserParams.angle_max_=2.0 * M_PI; - - for (int i=0; i. - */ - - double testRanges1[]= {250, 250, 250, 250, 250, 17.263999938964844, 17.263999938964844, 17.263999938964844, 17.263999938964844, 13.552000045776367, 13.552000045776367, 13.552000045776367, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.51200008392334, 11.567999839782715, 11.567999839782715, 10.720000267028809, 10.720000267028809, 10.720000267028809, 10.359999656677246, 10.336000442504883, 10.343999862670898, 10.35200023651123, 10.35200023651123, 10.32800006866455, 10.319999694824219, 10.383999824523926, 10.407999992370605, 10.432000160217285, 10.432000160217285, 10.447999954223633, 10.46399974822998, 10.46399974822998, 10.503999710083008, 10.503999710083008, 10.527999877929688, 10.592000007629395, 10.62399959564209, 8.368000030517578, 8.343999862670898, 8.343999862670898, 8.031999588012695, 7.831999778747559, 7.6479997634887695, 7.552000045776367, 7.415999889373779, 7.415999889373779, 7.263999938964844, 7.144000053405762, 7.0320000648498535, 6.920000076293945, 6.776000022888184, 6.776000022888184, 6.671999931335449, 6.576000213623047, 6.440000057220459, 6.328000068664551, 6.271999835968018, 6.271999835968018, 6.263999938964844, 6.271999835968018, 6.296000003814697, 6.320000171661377, 6.328000068664551, 6.328000068664551, 6.335999965667725, 6.320000171661377, 3.187999963760376, 3.187999963760376, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.504000186920166, 4.504000186920166, 4.504000186920166, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.599999904632568, 4.576000213623047, 4.495999813079834, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.4079999923706055, 4.368000030517578, 4.320000171661377, 4.271999835968018, 4.271999835968018, 4.223999977111816, 4.184000015258789, 4.168000221252441, 4.159999847412109, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.184000015258789, 4.199999809265137, 4.199999809265137, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.223999977111816, 4.23199987411499, 4.23199987411499, 4.176000118255615, 250, 250, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.099999904632568, 4.099999904632568, 4.099999904632568, 4.079999923706055, 4.0320000648498535, 3.98799991607666, 3.9719998836517334, 3.9719998836517334, 3.9560000896453857, 3.9240000247955322, 3.888000011444092, 3.859999895095825, 3.8320000171661377, 3.8320000171661377, 3.803999900817871, 3.7880001068115234, 3.259999990463257, 3.240000009536743, 3.2200000286102295, 3.2200000286102295, 3.200000047683716, 3.1760001182556152, 3.1559998989105225, 3.1440000534057617, 3.128000020980835, 3.128000020980835, 3.119999885559082, 2.5959999561309814, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.496000051498413, 2.5320000648498535, 250, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 250, 250, 250, 250, 250, 250, 250, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.640000104904175, 2.640000104904175, 2.640000104904175, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.555999994277954, 2.5439999103546143, 2.5320000648498535, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.503999948501587, 2.492000102996826, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4600000381469727, 2.4519999027252197, 2.440000057220459, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4079999923706055, 2.427999973297119, 2.444000005722046, 2.444000005722046, 2.4600000381469727, 2.4719998836517334, 2.48799991607666, 2.5, 2.5160000324249268, 2.5160000324249268, 2.5280001163482666, 2.5399999618530273, 2.559999942779541, 2.5799999237060547, 2.5840001106262207, 2.5840001106262207, 3.0480000972747803, 3.0480000972747803, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5959999561309814, 2.5920000076293945, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.572000026702881, 2.563999891281128, 2.555999994277954, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5399999618530273, 2.5360000133514404, 2.5320000648498535, 2.5239999294281006, 2.5199999809265137, 2.507999897003174, 2.507999897003174, 2.503999948501587, 2.5, 2.496000051498413, 2.492000102996826, 2.48799991607666, 2.48799991607666, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4639999866485596, 2.4560000896453857, 2.4560000896453857, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.444000005722046, 2.436000108718872, 2.436000108718872, 2.431999921798706, 2.427999973297119, 2.4240000247955322, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4040000438690186, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.3559999465942383, 2.3559999465942383, 2.3559999465942383, 2.6600000858306885, 2.6600000858306885, 2.6600000858306885, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.0, 2.0, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 2.0, 2.0, 2.009999990463257, 2.0220000743865967, 2.0460000038146973, 2.0460000038146973, 2.0260000228881836, 2.0260000228881836, 2.00600004196167, 1.996000051498413, 1.99399995803833, 1.9900000095367432, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.9980000257492065, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9919999837875366, 1.9520000219345093, 2.0320000648498535, 2.0999999046325684, 2.1040000915527344, 2.1040000915527344, 2.0280001163482666, 1.9040000438690186, 1.9279999732971191, 1.9520000219345093, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9600000381469727, 1.9620000123977661, 1.9639999866485596, 1.9639999866485596, 1.965999960899353, 1.965999960899353, 1.968000054359436, 1.972000002861023, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9800000190734863, 1.9839999675750732, 1.9800000190734863, 1.9800000190734863, 1.9819999933242798, 1.9880000352859497, 1.996000051498413, 2.009999990463257, 2.00600004196167, 2.00600004196167, 1.99399995803833, 2.687999963760376, 2.687999963760376, 2.687999963760376, 1.74399995803833, 1.74399995803833, 2.063999891281128, 2.115999937057495, 2.196000099182129, 2.191999912261963, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5199999809265137, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5399999618530273, 2.5480000972747803, 2.5480000972747803, 2.5480000972747803, 2.5399999618530273, 2.5439999103546143, 2.552000045776367, 2.555999994277954, 2.555999994277954, 2.552000045776367, 2.5480000972747803, 250, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3079999685287476, 1.2999999523162842, 1.2940000295639038, 1.2940000295639038, 1.2879999876022339, 1.2860000133514404, 1.2940000295639038, 1.3040000200271606, 1.3040000200271606, 1.3040000200271606, 1.2999999523162842, 1.2979999780654907, 1.3020000457763672, 1.309999942779541, 1.3140000104904175, 1.3140000104904175, 1.3140000104904175, 1.3240000009536743, 1.3359999656677246, 1.3420000076293945, 1.343999981880188, 1.343999981880188, 1.2200000286102295, 250, 250, 250, 250, 250, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.090000033378601, 1.090000033378601, 1.0920000076293945, 1.0980000495910645, 1.0980000495910645, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.1019999980926514, 1.1119999885559082, 1.121999979019165, 1.1260000467300415, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.776000022888184, 5.776000022888184, 5.776000022888184, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.736000061035156, 5.679999828338623, 5.671999931335449, 5.671999931335449, 5.631999969482422, 5.631999969482422, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.559999942779541, 5.584000110626221, 6.104000091552734, 6.783999919891357, 6.831999778747559, 6.831999778747559, 6.831999778747559, 6.760000228881836, 6.616000175476074, 6.455999851226807, 6.343999862670898, 6.343999862670898, 6.320000171661377, 6.311999797821045, 9.192000389099121, 9.175999641418457, 9.15999984741211, 9.15999984741211, 9.128000259399414, 9.119999885559082, 7.992000102996826, 7.9120001792907715, 7.8480000495910645, 7.8480000495910645, 7.800000190734863, 7.74399995803833, 7.71999979019165, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.71999979019165, 7.71999979019165, 7.711999893188477, 7.703999996185303, 7.703999996185303, 7.703999996185303, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.671999931335449, 8.67199993133545, 8.640000343322754, 8.607999801635742, 8.583999633789062, 8.583999633789062, 8.5600004196167, 8.552000045776367, 8.53600025177002, 8.520000457763672, 8.503999710083008, 8.503999710083008, 8.472000122070312, 8.447999954223633, 8.416000366210938, 8.383999824523926, 8.383999824523926, 8.383999824523926, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.336000442504883, 8.303999900817871, 8.303999900817871, 8.263999938964844, 8.239999771118164, 8.279999732971191, 8.392000198364258, 8.496000289916992, 8.496000289916992, 8.656000137329102, 8.807999610900879, 8.928000450134277, 9.015999794006348, 11.767999649047852, 11.767999649047852, 12.071999549865723, 12.175999641418457, 12.567999839782715, 12.663999557495117, 13.064000129699707, 13.064000129699707, 13.168000221252441, 13.607999801635742, 13.640000343322754, 13.53600025177002, 13.968000411987305, 13.968000411987305, 14.008000373840332, 14.104000091552734, 14.175999641418457, 14.104000091552734, 15.4399995803833, 15.4399995803833, 15.543999671936035, 15.48799991607666, 15.48799991607666, 15.503999710083008, 15.479999542236328, 15.479999542236328, 15.447999954223633, 15.399999618530273, 15.37600040435791, 15.35200023651123, 15.319999694824219, 15.319999694824219, 15.272000312805176, 15.272000312805176, 15.336000442504883, 15.303999900817871, 15.248000144958496, 15.248000144958496, 14.744000434875488, 15.303999900817871, 15.272000312805176, 15.248000144958496, 15.215999603271484, 15.223999977111816, 15.223999977111816, 15.248000144958496, 16.784000396728516, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.672000885009766, 16.687999725341797, 16.719999313354492, 16.70400047302246, 16.607999801635742, 16.607999801635742, 16.624000549316406, 16.6560001373291, 16.23200035095215, 16.143999099731445, 16.06399917602539, 16.06399917602539, 16.016000747680664, 17.472000122070312, 19.007999420166016, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250}; - - diff --git a/src/examples/testData.h b/src/examples/testData.h deleted file mode 100644 index d0e07f0..0000000 --- a/src/examples/testData.h +++ /dev/null @@ -1,28 +0,0 @@ -/** - * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant - * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. - * - * This file is part of FALKOLib. - * - * FALKOLib is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * FALKOLib is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with FALKOLib. If not, see . - */ -#pragma once - -extern double testRanges1[]; - -extern double testRanges2[]; - -extern double testRangesOrtho1[]; - -extern double testRangesOrtho2[]; -- GitLab From 1ef5238c37afdcdfc594d0fc173efacaad4a4b37 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 15 Apr 2021 15:33:57 +0200 Subject: [PATCH 080/100] added vector3d transform to matchClosureScene --- src/CMakeLists.txt | 5 ++ src/corner_falko_2d.cpp | 121 +++++++++++++++++++++++++++++ src/corner_falko_2d.h | 119 ++++++++++++++++++++++++++++ src/examples/CMakeLists.txt | 4 + src/examples/corner_falko_demo.cpp | 49 ++++++++++++ src/examples/testData.cpp | 23 ++++++ src/examples/testData.h | 28 +++++++ src/loop_closure_falko.h | 8 +- src/match_loop_closure_scene.h | 1 + 9 files changed, 356 insertions(+), 2 deletions(-) create mode 100644 src/corner_falko_2d.cpp create mode 100644 src/corner_falko_2d.h create mode 100644 src/examples/corner_falko_demo.cpp create mode 100644 src/examples/testData.cpp create mode 100644 src/examples/testData.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 71ab644..fee4770 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -67,6 +67,7 @@ SET(HDRS IF(falkolib_FOUND) SET(HDRS ${HDRS} + corner_falko_2d.h loop_closure_falko.h scene_falko.h match_loop_closure_scene.h) @@ -95,6 +96,10 @@ SET(SRCS icp.cpp) ENDIF(csm_FOUND) + IF(falkolib_FOUND) + SET(SRCS ${SRCS} + corner_falko_2d.cpp) + ENDIF(falkolib_FOUND) diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp new file mode 100644 index 0000000..edce4b9 --- /dev/null +++ b/src/corner_falko_2d.cpp @@ -0,0 +1,121 @@ +/** + * \file corner_falko_2d.cpp + * + * Created on: Jan 26, 2021 + * \author: spujol + */ + +#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() {} + +void CornerFalko2d::AddNewReferenceScene(falkolib::LaserScan scanFALKO) { + + // Extract keypoints + lastKeypointSet.clear(); + extract(scanFALKO, lastKeypointSet); + + // Compute descriptors + lastDescriptorSet.clear(); + compute(scanFALKO, lastKeypointSet, lastDescriptorSet); + + keypointSets.push_back(lastKeypointSet); + descriptorSets.push_back(lastDescriptorSet); + + scansExtracted = scansExtracted + 1; +} + +void CornerFalko2d::findLoopClosure(LaserScan scan, + LaserScanParams scanParams) { + + falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams); + + // Compute descriptors + std::vector keypointSet2; + extract(scanFALKO, keypointSet2); + + // Compute descriptors + std::vector descriptorSet2; + compute(scanFALKO, keypointSet2, descriptorSet2); + + // Matching + int rows = keypointSets.size(); + + numberKeypointsMatch = 0; + numberSceneMatch = 0; + matchingPosition = -1; + for (int i = 0; i < rows; i++) { + std::vector> assoNN; + + 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; + } +} + +falkolib::LaserScan +CornerFalko2d::convert2LaserScanFALKO(LaserScan scan, + LaserScanParams scanParams) { + + falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, + scan.ranges_raw_.size()); + std::vector doubleRanges(scan.ranges_raw_.begin(), + scan.ranges_raw_.end()); + scanFALKO.fromRanges(doubleRanges); + return scanFALKO; +} + +int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan, + LaserScanParams scanParams, + int scanInterval) { + + scanNumber = scanNumber + 1; + + int NewSceneAdded = 0; + + if (scanNumber % scanInterval == 0 || scanNumber == 1) { + + findLoopClosure(scan, scanParams); + + if (numberKeypointsMatch < refSceneAddingTh) { + + falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams); + + AddNewReferenceScene(scanFALKO); + + NewSceneAdded = 1; + } + } + + return NewSceneAdded; +} + +} // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h new file mode 100644 index 0000000..89513e6 --- /dev/null +++ b/src/corner_falko_2d.h @@ -0,0 +1,119 @@ +/** + * \file corner_falko_2d.h + * + * Created on: Jan 26, 2021 + * \author: spujol + */ + +#ifndef CORNER_FALKO_2D_H_ +#define CORNER_FALKO_2D_H_ + +#include +#include + +/************************** + * laser_scan_utils includes * + **************************/ +#include "laser_scan.h" + +/************************** + * WOLF includes * + **************************/ +//#include "core/landmark/landmark_base.h" + +/************************** + * Falko includes * + **************************/ +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace laserscanutils { + +/** \brief A 2D corner extractor and loop closure computing class +**/ + +class CornerFalko2d : public falkolib::FALKOExtractor, + public falkolib::BSCExtractor, + public falkolib::NNMatcher { +private: + /** \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> keypointSets; + std::vector lastKeypointSet; + std::vector> descriptorSets; + std::vector 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 */ + +#endif /* LANDMARK_POLYLINE_2d_H_ */ diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 63e4efb..40bd1f1 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -3,3 +3,7 @@ INCLUDE_DIRECTORIES(../src) # polylines demo ADD_EXECUTABLE(demo_polylines polyline_demo.cpp) TARGET_LINK_LIBRARIES(demo_polylines ${PROJECT_NAME}) + +# falkocorners demo +ADD_EXECUTABLE(demo_corner_falko corner_falko_demo.cpp testData.cpp) +TARGET_LINK_LIBRARIES(demo_corner_falko ${PROJECT_NAME}) diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp new file mode 100644 index 0000000..8baa356 --- /dev/null +++ b/src/examples/corner_falko_demo.cpp @@ -0,0 +1,49 @@ +/** + * \file corner_falko_2d_test.cpp + * + * Created on: Jan 26, 2021 + * \author: spujol + */ + +//LaserScanUtils includes +#include "../line_finder_iterative.h" +#include "../corner_falko_2d.h" +#include "../laser_scan.h" +#include "testData.h" + + +//std includes +#include +#include +using namespace laserscanutils; + +int main(int argc, char** argv) +{ + + int scanSize = 1440; + int scanInterval =1; + + LaserScan scan; + LaserScanParams laserParams; + + laserParams.angle_min_=0; + laserParams.angle_max_=2.0 * M_PI; + + for (int i=0; i. + */ + + double testRanges1[]= {250, 250, 250, 250, 250, 17.263999938964844, 17.263999938964844, 17.263999938964844, 17.263999938964844, 13.552000045776367, 13.552000045776367, 13.552000045776367, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.51200008392334, 11.567999839782715, 11.567999839782715, 10.720000267028809, 10.720000267028809, 10.720000267028809, 10.359999656677246, 10.336000442504883, 10.343999862670898, 10.35200023651123, 10.35200023651123, 10.32800006866455, 10.319999694824219, 10.383999824523926, 10.407999992370605, 10.432000160217285, 10.432000160217285, 10.447999954223633, 10.46399974822998, 10.46399974822998, 10.503999710083008, 10.503999710083008, 10.527999877929688, 10.592000007629395, 10.62399959564209, 8.368000030517578, 8.343999862670898, 8.343999862670898, 8.031999588012695, 7.831999778747559, 7.6479997634887695, 7.552000045776367, 7.415999889373779, 7.415999889373779, 7.263999938964844, 7.144000053405762, 7.0320000648498535, 6.920000076293945, 6.776000022888184, 6.776000022888184, 6.671999931335449, 6.576000213623047, 6.440000057220459, 6.328000068664551, 6.271999835968018, 6.271999835968018, 6.263999938964844, 6.271999835968018, 6.296000003814697, 6.320000171661377, 6.328000068664551, 6.328000068664551, 6.335999965667725, 6.320000171661377, 3.187999963760376, 3.187999963760376, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.504000186920166, 4.504000186920166, 4.504000186920166, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.599999904632568, 4.576000213623047, 4.495999813079834, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.4079999923706055, 4.368000030517578, 4.320000171661377, 4.271999835968018, 4.271999835968018, 4.223999977111816, 4.184000015258789, 4.168000221252441, 4.159999847412109, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.184000015258789, 4.199999809265137, 4.199999809265137, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.223999977111816, 4.23199987411499, 4.23199987411499, 4.176000118255615, 250, 250, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.099999904632568, 4.099999904632568, 4.099999904632568, 4.079999923706055, 4.0320000648498535, 3.98799991607666, 3.9719998836517334, 3.9719998836517334, 3.9560000896453857, 3.9240000247955322, 3.888000011444092, 3.859999895095825, 3.8320000171661377, 3.8320000171661377, 3.803999900817871, 3.7880001068115234, 3.259999990463257, 3.240000009536743, 3.2200000286102295, 3.2200000286102295, 3.200000047683716, 3.1760001182556152, 3.1559998989105225, 3.1440000534057617, 3.128000020980835, 3.128000020980835, 3.119999885559082, 2.5959999561309814, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.496000051498413, 2.5320000648498535, 250, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 250, 250, 250, 250, 250, 250, 250, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.640000104904175, 2.640000104904175, 2.640000104904175, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.555999994277954, 2.5439999103546143, 2.5320000648498535, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.503999948501587, 2.492000102996826, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4600000381469727, 2.4519999027252197, 2.440000057220459, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4079999923706055, 2.427999973297119, 2.444000005722046, 2.444000005722046, 2.4600000381469727, 2.4719998836517334, 2.48799991607666, 2.5, 2.5160000324249268, 2.5160000324249268, 2.5280001163482666, 2.5399999618530273, 2.559999942779541, 2.5799999237060547, 2.5840001106262207, 2.5840001106262207, 3.0480000972747803, 3.0480000972747803, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5959999561309814, 2.5920000076293945, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.572000026702881, 2.563999891281128, 2.555999994277954, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5399999618530273, 2.5360000133514404, 2.5320000648498535, 2.5239999294281006, 2.5199999809265137, 2.507999897003174, 2.507999897003174, 2.503999948501587, 2.5, 2.496000051498413, 2.492000102996826, 2.48799991607666, 2.48799991607666, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4639999866485596, 2.4560000896453857, 2.4560000896453857, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.444000005722046, 2.436000108718872, 2.436000108718872, 2.431999921798706, 2.427999973297119, 2.4240000247955322, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4040000438690186, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.3559999465942383, 2.3559999465942383, 2.3559999465942383, 2.6600000858306885, 2.6600000858306885, 2.6600000858306885, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.0, 2.0, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 2.0, 2.0, 2.009999990463257, 2.0220000743865967, 2.0460000038146973, 2.0460000038146973, 2.0260000228881836, 2.0260000228881836, 2.00600004196167, 1.996000051498413, 1.99399995803833, 1.9900000095367432, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.9980000257492065, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9919999837875366, 1.9520000219345093, 2.0320000648498535, 2.0999999046325684, 2.1040000915527344, 2.1040000915527344, 2.0280001163482666, 1.9040000438690186, 1.9279999732971191, 1.9520000219345093, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9600000381469727, 1.9620000123977661, 1.9639999866485596, 1.9639999866485596, 1.965999960899353, 1.965999960899353, 1.968000054359436, 1.972000002861023, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9800000190734863, 1.9839999675750732, 1.9800000190734863, 1.9800000190734863, 1.9819999933242798, 1.9880000352859497, 1.996000051498413, 2.009999990463257, 2.00600004196167, 2.00600004196167, 1.99399995803833, 2.687999963760376, 2.687999963760376, 2.687999963760376, 1.74399995803833, 1.74399995803833, 2.063999891281128, 2.115999937057495, 2.196000099182129, 2.191999912261963, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5199999809265137, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5399999618530273, 2.5480000972747803, 2.5480000972747803, 2.5480000972747803, 2.5399999618530273, 2.5439999103546143, 2.552000045776367, 2.555999994277954, 2.555999994277954, 2.552000045776367, 2.5480000972747803, 250, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3079999685287476, 1.2999999523162842, 1.2940000295639038, 1.2940000295639038, 1.2879999876022339, 1.2860000133514404, 1.2940000295639038, 1.3040000200271606, 1.3040000200271606, 1.3040000200271606, 1.2999999523162842, 1.2979999780654907, 1.3020000457763672, 1.309999942779541, 1.3140000104904175, 1.3140000104904175, 1.3140000104904175, 1.3240000009536743, 1.3359999656677246, 1.3420000076293945, 1.343999981880188, 1.343999981880188, 1.2200000286102295, 250, 250, 250, 250, 250, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.090000033378601, 1.090000033378601, 1.0920000076293945, 1.0980000495910645, 1.0980000495910645, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.1019999980926514, 1.1119999885559082, 1.121999979019165, 1.1260000467300415, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.776000022888184, 5.776000022888184, 5.776000022888184, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.736000061035156, 5.679999828338623, 5.671999931335449, 5.671999931335449, 5.631999969482422, 5.631999969482422, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.559999942779541, 5.584000110626221, 6.104000091552734, 6.783999919891357, 6.831999778747559, 6.831999778747559, 6.831999778747559, 6.760000228881836, 6.616000175476074, 6.455999851226807, 6.343999862670898, 6.343999862670898, 6.320000171661377, 6.311999797821045, 9.192000389099121, 9.175999641418457, 9.15999984741211, 9.15999984741211, 9.128000259399414, 9.119999885559082, 7.992000102996826, 7.9120001792907715, 7.8480000495910645, 7.8480000495910645, 7.800000190734863, 7.74399995803833, 7.71999979019165, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.71999979019165, 7.71999979019165, 7.711999893188477, 7.703999996185303, 7.703999996185303, 7.703999996185303, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.671999931335449, 8.67199993133545, 8.640000343322754, 8.607999801635742, 8.583999633789062, 8.583999633789062, 8.5600004196167, 8.552000045776367, 8.53600025177002, 8.520000457763672, 8.503999710083008, 8.503999710083008, 8.472000122070312, 8.447999954223633, 8.416000366210938, 8.383999824523926, 8.383999824523926, 8.383999824523926, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.336000442504883, 8.303999900817871, 8.303999900817871, 8.263999938964844, 8.239999771118164, 8.279999732971191, 8.392000198364258, 8.496000289916992, 8.496000289916992, 8.656000137329102, 8.807999610900879, 8.928000450134277, 9.015999794006348, 11.767999649047852, 11.767999649047852, 12.071999549865723, 12.175999641418457, 12.567999839782715, 12.663999557495117, 13.064000129699707, 13.064000129699707, 13.168000221252441, 13.607999801635742, 13.640000343322754, 13.53600025177002, 13.968000411987305, 13.968000411987305, 14.008000373840332, 14.104000091552734, 14.175999641418457, 14.104000091552734, 15.4399995803833, 15.4399995803833, 15.543999671936035, 15.48799991607666, 15.48799991607666, 15.503999710083008, 15.479999542236328, 15.479999542236328, 15.447999954223633, 15.399999618530273, 15.37600040435791, 15.35200023651123, 15.319999694824219, 15.319999694824219, 15.272000312805176, 15.272000312805176, 15.336000442504883, 15.303999900817871, 15.248000144958496, 15.248000144958496, 14.744000434875488, 15.303999900817871, 15.272000312805176, 15.248000144958496, 15.215999603271484, 15.223999977111816, 15.223999977111816, 15.248000144958496, 16.784000396728516, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.672000885009766, 16.687999725341797, 16.719999313354492, 16.70400047302246, 16.607999801635742, 16.607999801635742, 16.624000549316406, 16.6560001373291, 16.23200035095215, 16.143999099731445, 16.06399917602539, 16.06399917602539, 16.016000747680664, 17.472000122070312, 19.007999420166016, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250}; + + diff --git a/src/examples/testData.h b/src/examples/testData.h new file mode 100644 index 0000000..d0e07f0 --- /dev/null +++ b/src/examples/testData.h @@ -0,0 +1,28 @@ +/** + * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant + * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. + * + * This file is part of FALKOLib. + * + * FALKOLib is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * FALKOLib is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with FALKOLib. If not, see . + */ +#pragma once + +extern double testRanges1[]; + +extern double testRanges2[]; + +extern double testRangesOrtho1[]; + +extern double testRangesOrtho2[]; diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index ecfda98..3aa5421 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -84,8 +84,8 @@ struct ParameterLoopClosureFalko template typename M> class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor { - private: - public: + private: + public: typedef std::shared_ptr> sceneFalkoBSCPtr; typedef std::shared_ptr laserScanPtr; @@ -185,6 +185,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), scene_2_falko->keypoints_list_.size()); + + new_match->transform_vector.head(2) = new_match->transform.translation(); + new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); + //} return new_match; } diff --git a/src/match_loop_closure_scene.h b/src/match_loop_closure_scene.h index 47bfa22..423398f 100644 --- a/src/match_loop_closure_scene.h +++ b/src/match_loop_closure_scene.h @@ -25,6 +25,7 @@ struct MatchLoopClosureScene { int keypoints_number_match; double score; Eigen::Affine2d transform; + Eigen::Vector3d transform_vector; }; typedef std::shared_ptr MatchLoopClosureScenePtr; -- GitLab From 4fbc6472b67bd87e2c66c77a0db55d953772daae Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 15 Apr 2021 16:01:17 +0200 Subject: [PATCH 081/100] deleted old falko demo --- src/examples/CMakeLists.txt | 4 --- src/examples/corner_falko_demo.cpp | 49 ------------------------------ src/examples/testData.cpp | 23 -------------- src/examples/testData.h | 28 ----------------- 4 files changed, 104 deletions(-) delete mode 100644 src/examples/corner_falko_demo.cpp delete mode 100644 src/examples/testData.cpp delete mode 100644 src/examples/testData.h diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 40bd1f1..63e4efb 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -3,7 +3,3 @@ INCLUDE_DIRECTORIES(../src) # polylines demo ADD_EXECUTABLE(demo_polylines polyline_demo.cpp) TARGET_LINK_LIBRARIES(demo_polylines ${PROJECT_NAME}) - -# falkocorners demo -ADD_EXECUTABLE(demo_corner_falko corner_falko_demo.cpp testData.cpp) -TARGET_LINK_LIBRARIES(demo_corner_falko ${PROJECT_NAME}) diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp deleted file mode 100644 index 8baa356..0000000 --- a/src/examples/corner_falko_demo.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/** - * \file corner_falko_2d_test.cpp - * - * Created on: Jan 26, 2021 - * \author: spujol - */ - -//LaserScanUtils includes -#include "../line_finder_iterative.h" -#include "../corner_falko_2d.h" -#include "../laser_scan.h" -#include "testData.h" - - -//std includes -#include -#include -using namespace laserscanutils; - -int main(int argc, char** argv) -{ - - int scanSize = 1440; - int scanInterval =1; - - LaserScan scan; - LaserScanParams laserParams; - - laserParams.angle_min_=0; - laserParams.angle_max_=2.0 * M_PI; - - for (int i=0; i. - */ - - double testRanges1[]= {250, 250, 250, 250, 250, 17.263999938964844, 17.263999938964844, 17.263999938964844, 17.263999938964844, 13.552000045776367, 13.552000045776367, 13.552000045776367, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.51200008392334, 11.567999839782715, 11.567999839782715, 10.720000267028809, 10.720000267028809, 10.720000267028809, 10.359999656677246, 10.336000442504883, 10.343999862670898, 10.35200023651123, 10.35200023651123, 10.32800006866455, 10.319999694824219, 10.383999824523926, 10.407999992370605, 10.432000160217285, 10.432000160217285, 10.447999954223633, 10.46399974822998, 10.46399974822998, 10.503999710083008, 10.503999710083008, 10.527999877929688, 10.592000007629395, 10.62399959564209, 8.368000030517578, 8.343999862670898, 8.343999862670898, 8.031999588012695, 7.831999778747559, 7.6479997634887695, 7.552000045776367, 7.415999889373779, 7.415999889373779, 7.263999938964844, 7.144000053405762, 7.0320000648498535, 6.920000076293945, 6.776000022888184, 6.776000022888184, 6.671999931335449, 6.576000213623047, 6.440000057220459, 6.328000068664551, 6.271999835968018, 6.271999835968018, 6.263999938964844, 6.271999835968018, 6.296000003814697, 6.320000171661377, 6.328000068664551, 6.328000068664551, 6.335999965667725, 6.320000171661377, 3.187999963760376, 3.187999963760376, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.504000186920166, 4.504000186920166, 4.504000186920166, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.599999904632568, 4.576000213623047, 4.495999813079834, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.4079999923706055, 4.368000030517578, 4.320000171661377, 4.271999835968018, 4.271999835968018, 4.223999977111816, 4.184000015258789, 4.168000221252441, 4.159999847412109, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.184000015258789, 4.199999809265137, 4.199999809265137, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.223999977111816, 4.23199987411499, 4.23199987411499, 4.176000118255615, 250, 250, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.099999904632568, 4.099999904632568, 4.099999904632568, 4.079999923706055, 4.0320000648498535, 3.98799991607666, 3.9719998836517334, 3.9719998836517334, 3.9560000896453857, 3.9240000247955322, 3.888000011444092, 3.859999895095825, 3.8320000171661377, 3.8320000171661377, 3.803999900817871, 3.7880001068115234, 3.259999990463257, 3.240000009536743, 3.2200000286102295, 3.2200000286102295, 3.200000047683716, 3.1760001182556152, 3.1559998989105225, 3.1440000534057617, 3.128000020980835, 3.128000020980835, 3.119999885559082, 2.5959999561309814, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.496000051498413, 2.5320000648498535, 250, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 250, 250, 250, 250, 250, 250, 250, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.640000104904175, 2.640000104904175, 2.640000104904175, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.555999994277954, 2.5439999103546143, 2.5320000648498535, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.503999948501587, 2.492000102996826, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4600000381469727, 2.4519999027252197, 2.440000057220459, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4079999923706055, 2.427999973297119, 2.444000005722046, 2.444000005722046, 2.4600000381469727, 2.4719998836517334, 2.48799991607666, 2.5, 2.5160000324249268, 2.5160000324249268, 2.5280001163482666, 2.5399999618530273, 2.559999942779541, 2.5799999237060547, 2.5840001106262207, 2.5840001106262207, 3.0480000972747803, 3.0480000972747803, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5959999561309814, 2.5920000076293945, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.572000026702881, 2.563999891281128, 2.555999994277954, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5399999618530273, 2.5360000133514404, 2.5320000648498535, 2.5239999294281006, 2.5199999809265137, 2.507999897003174, 2.507999897003174, 2.503999948501587, 2.5, 2.496000051498413, 2.492000102996826, 2.48799991607666, 2.48799991607666, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4639999866485596, 2.4560000896453857, 2.4560000896453857, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.444000005722046, 2.436000108718872, 2.436000108718872, 2.431999921798706, 2.427999973297119, 2.4240000247955322, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4040000438690186, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.3559999465942383, 2.3559999465942383, 2.3559999465942383, 2.6600000858306885, 2.6600000858306885, 2.6600000858306885, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.0, 2.0, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 2.0, 2.0, 2.009999990463257, 2.0220000743865967, 2.0460000038146973, 2.0460000038146973, 2.0260000228881836, 2.0260000228881836, 2.00600004196167, 1.996000051498413, 1.99399995803833, 1.9900000095367432, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.9980000257492065, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9919999837875366, 1.9520000219345093, 2.0320000648498535, 2.0999999046325684, 2.1040000915527344, 2.1040000915527344, 2.0280001163482666, 1.9040000438690186, 1.9279999732971191, 1.9520000219345093, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9600000381469727, 1.9620000123977661, 1.9639999866485596, 1.9639999866485596, 1.965999960899353, 1.965999960899353, 1.968000054359436, 1.972000002861023, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9800000190734863, 1.9839999675750732, 1.9800000190734863, 1.9800000190734863, 1.9819999933242798, 1.9880000352859497, 1.996000051498413, 2.009999990463257, 2.00600004196167, 2.00600004196167, 1.99399995803833, 2.687999963760376, 2.687999963760376, 2.687999963760376, 1.74399995803833, 1.74399995803833, 2.063999891281128, 2.115999937057495, 2.196000099182129, 2.191999912261963, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5199999809265137, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5399999618530273, 2.5480000972747803, 2.5480000972747803, 2.5480000972747803, 2.5399999618530273, 2.5439999103546143, 2.552000045776367, 2.555999994277954, 2.555999994277954, 2.552000045776367, 2.5480000972747803, 250, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3079999685287476, 1.2999999523162842, 1.2940000295639038, 1.2940000295639038, 1.2879999876022339, 1.2860000133514404, 1.2940000295639038, 1.3040000200271606, 1.3040000200271606, 1.3040000200271606, 1.2999999523162842, 1.2979999780654907, 1.3020000457763672, 1.309999942779541, 1.3140000104904175, 1.3140000104904175, 1.3140000104904175, 1.3240000009536743, 1.3359999656677246, 1.3420000076293945, 1.343999981880188, 1.343999981880188, 1.2200000286102295, 250, 250, 250, 250, 250, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.090000033378601, 1.090000033378601, 1.0920000076293945, 1.0980000495910645, 1.0980000495910645, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.1019999980926514, 1.1119999885559082, 1.121999979019165, 1.1260000467300415, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.776000022888184, 5.776000022888184, 5.776000022888184, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.736000061035156, 5.679999828338623, 5.671999931335449, 5.671999931335449, 5.631999969482422, 5.631999969482422, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.559999942779541, 5.584000110626221, 6.104000091552734, 6.783999919891357, 6.831999778747559, 6.831999778747559, 6.831999778747559, 6.760000228881836, 6.616000175476074, 6.455999851226807, 6.343999862670898, 6.343999862670898, 6.320000171661377, 6.311999797821045, 9.192000389099121, 9.175999641418457, 9.15999984741211, 9.15999984741211, 9.128000259399414, 9.119999885559082, 7.992000102996826, 7.9120001792907715, 7.8480000495910645, 7.8480000495910645, 7.800000190734863, 7.74399995803833, 7.71999979019165, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.71999979019165, 7.71999979019165, 7.711999893188477, 7.703999996185303, 7.703999996185303, 7.703999996185303, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.671999931335449, 8.67199993133545, 8.640000343322754, 8.607999801635742, 8.583999633789062, 8.583999633789062, 8.5600004196167, 8.552000045776367, 8.53600025177002, 8.520000457763672, 8.503999710083008, 8.503999710083008, 8.472000122070312, 8.447999954223633, 8.416000366210938, 8.383999824523926, 8.383999824523926, 8.383999824523926, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.336000442504883, 8.303999900817871, 8.303999900817871, 8.263999938964844, 8.239999771118164, 8.279999732971191, 8.392000198364258, 8.496000289916992, 8.496000289916992, 8.656000137329102, 8.807999610900879, 8.928000450134277, 9.015999794006348, 11.767999649047852, 11.767999649047852, 12.071999549865723, 12.175999641418457, 12.567999839782715, 12.663999557495117, 13.064000129699707, 13.064000129699707, 13.168000221252441, 13.607999801635742, 13.640000343322754, 13.53600025177002, 13.968000411987305, 13.968000411987305, 14.008000373840332, 14.104000091552734, 14.175999641418457, 14.104000091552734, 15.4399995803833, 15.4399995803833, 15.543999671936035, 15.48799991607666, 15.48799991607666, 15.503999710083008, 15.479999542236328, 15.479999542236328, 15.447999954223633, 15.399999618530273, 15.37600040435791, 15.35200023651123, 15.319999694824219, 15.319999694824219, 15.272000312805176, 15.272000312805176, 15.336000442504883, 15.303999900817871, 15.248000144958496, 15.248000144958496, 14.744000434875488, 15.303999900817871, 15.272000312805176, 15.248000144958496, 15.215999603271484, 15.223999977111816, 15.223999977111816, 15.248000144958496, 16.784000396728516, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.672000885009766, 16.687999725341797, 16.719999313354492, 16.70400047302246, 16.607999801635742, 16.607999801635742, 16.624000549316406, 16.6560001373291, 16.23200035095215, 16.143999099731445, 16.06399917602539, 16.06399917602539, 16.016000747680664, 17.472000122070312, 19.007999420166016, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250}; - - diff --git a/src/examples/testData.h b/src/examples/testData.h deleted file mode 100644 index d0e07f0..0000000 --- a/src/examples/testData.h +++ /dev/null @@ -1,28 +0,0 @@ -/** - * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant - * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. - * - * This file is part of FALKOLib. - * - * FALKOLib is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * FALKOLib is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with FALKOLib. If not, see . - */ -#pragma once - -extern double testRanges1[]; - -extern double testRanges2[]; - -extern double testRangesOrtho1[]; - -extern double testRangesOrtho2[]; -- GitLab From 24049139039820c6f9d162f4ac789c573fa6db16 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 20 Apr 2021 11:53:56 +0200 Subject: [PATCH 082/100] doxygen comments added --- src/loop_closure_falko.h | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 3aa5421..bb0c1cc 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -45,6 +45,8 @@ typedef falkolib::CGHExtractor cghExtractor; template using nn_matcher = falkolib::NNMatcher; template using aht_matcher = falkolib::AHTMatcher; +/** \brief Struct class that store falkolib parameters + **/ struct ParameterLoopClosureFalko { // Keypoints extractor Default @@ -79,6 +81,12 @@ struct ParameterLoopClosureFalko }; /** \brief A base class for loop closure using falko library + * The class is a wrapper of the falkolib that is designed to be used for loop closures in the wolf problem + * Diferent types of descriptors can be used, and are specified as template parameters. + * \tparam D Descriptor type. or + * \tparam Extr descriptor extractor type or + * \tparam M Matcher type or + * \param _param parameter struct with falko lib parameters **/ template typename M> @@ -91,9 +99,9 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract Extr extractor_; M matcher_; - // M matcher_desc_; - /** \brief Constructor for nn_matcher + /** \brief Constructor + * \param _param parameter struct with falko lib parameters **/ LoopClosureFalko(ParameterLoopClosureFalko _param) : LoopClosureBase2d() @@ -146,8 +154,9 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract return scan_falko; } - /** \brief Create and update a matchLoopClosure struct with the info that is - *produced when matching two given scenes + /** \brief Create and update a matchLoopClosure struct with the info that is produced when matching two given scenes + * \param _scene_1 reference scene struct + * \param _scene_2 target scene struct **/ MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override { -- GitLab From d754a078b01020a476514a43d76a08c74ee9fe4e Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 20 Apr 2021 13:51:22 +0200 Subject: [PATCH 083/100] doxygen comments added --- src/loop_closure_base.h | 60 ++++++++++++++++++++-------------------- src/loop_closure_falko.h | 12 ++++++-- 2 files changed, 40 insertions(+), 32 deletions(-) diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index fe790cf..b6a815f 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -23,41 +23,41 @@ namespace laserscanutils { /** \brief A 2base class for loop closure using falko library */ -class LoopClosureBase2d { -private: -public: - /** \brief Constructor - **/ - LoopClosureBase2d(){}; +class LoopClosureBase2d +{ + private: + public: + /** \brief Constructor + **/ + LoopClosureBase2d(){}; - /** \brief Destructor - **/ - ~LoopClosureBase2d(){}; + /** \brief Destructor + **/ + ~LoopClosureBase2d(){}; - /** \brief update the scene struct with keypoints and descriptors - **/ - virtual sceneBasePtr extractScene(const LaserScan &scan, - const LaserScanParams &scanParams) = 0; + /** \brief update the scene struct with keypoints and descriptors + **/ + virtual sceneBasePtr extractScene(const LaserScan &scan, const LaserScanParams &scanParams) = 0; - /** \brief Create and update a matchLoopClosure struct with the info that is - *produced when matching two given scenes - **/ - virtual MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene1, - sceneBasePtr _scene2) = 0; + /** \brief Create and update a matchLoopClosure struct with the info that is + *produced when matching two given scenes + **/ + virtual MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene1, sceneBasePtr _scene2) = 0; - /** \brief compare new scans against the trained set in order to find loop - *closures - **/ - virtual std::map - findLoopClosure(std::list> _l_scenes, - const sceneBasePtr _new_scene) { - std::map matchings; - for (auto ref_scene : _l_scenes) { - auto match = matchScene(ref_scene, _new_scene); - matchings.emplace(match->score, match); + /** \brief It matches a target scene against a list of references scenes in order to find loop + * closures + **/ + virtual std::map findLoopClosure(std::list> _l_scenes, + const sceneBasePtr _new_scene) + { + std::map matchings; + for (auto ref_scene : _l_scenes) + { + auto match = matchScene(ref_scene, _new_scene); + matchings.emplace(match->score, match); + } + return matchings; } - return matchings; - } }; } /* namespace laserscanutils */ diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index bb0c1cc..5b177d4 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -80,9 +80,18 @@ struct ParameterLoopClosureFalko double thetaAbsMax_ = 1.57; }; -/** \brief A base class for loop closure using falko library +/** \brief A class for loop closure using falko library + * * The class is a wrapper of the falkolib that is designed to be used for loop closures in the wolf problem + * + * It extracts scenes from a laserscanutils::LaserScan. The scenes contain keypoints and descriptors + * + * It matches a target scene against a list of reference scenes. + * + * The reference scenes are found from a search of the previous captures + * * Diferent types of descriptors can be used, and are specified as template parameters. + * * \tparam D Descriptor type. or * \tparam Extr descriptor extractor type or * \tparam M Matcher type or @@ -198,7 +207,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract new_match->transform_vector.head(2) = new_match->transform.translation(); new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); - //} return new_match; } -- GitLab From c5e90bda982e42121f39ef07141a339cf8418c12 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 27 Apr 2021 09:41:00 +0200 Subject: [PATCH 084/100] use_descriptors if rectified --- src/loop_closure_falko.h | 15 ++++----------- src/scene_falko.h | 16 +++++----------- 2 files changed, 9 insertions(+), 22 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 5b177d4..1e862fd 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -26,13 +26,6 @@ /************************** * Falko includes * **************************/ -#include -#include -#include -#include - -#include -#include #include #include @@ -50,8 +43,8 @@ template using aht_matcher = falkolib::AHTMatcher struct ParameterLoopClosureFalko { // Keypoints extractor Default - double min_extraction_range_ = 0.1; - double max_extraction_range_ = 25; + double min_extraction_range_ = 0; + double max_extraction_range_ = 30; bool enable_subbeam_ = true; double nms_radius_ = 0.1; double neigh_b_ = 0.01; @@ -176,12 +169,12 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract int matching_number = 0; - if (use_descriptors_ == 1) + if (use_descriptors_ == 0) { matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); } - else if (use_descriptors_ == 0) + else if (use_descriptors_ == 1) { matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, diff --git a/src/scene_falko.h b/src/scene_falko.h index cb14ddf..18529b1 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -22,28 +22,22 @@ /************************** * Falko includes * **************************/ -#include -#include -#include -#include +#include #include #include -#include -#include - namespace laserscanutils { typedef falkolib::BSC bsc; typedef falkolib::CGH cgh; -template struct SceneFalko : public SceneBase { - std::vector keypoints_list_; - std::vector descriptors_list_; +template struct SceneFalko : public SceneBase +{ + std::vector keypoints_list_; + std::vector descriptors_list_; }; - } /* namespace laserscanutils */ #endif /* SCENE_FALKO_H_ */ -- GitLab From 5c1195bcd4bce8301948f7d8fd6a52962534cde7 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 10 May 2021 00:28:14 +0200 Subject: [PATCH 085/100] computed max_dist between keypoints of an scene --- src/loop_closure_falko.h | 111 ++++++++++++++++++++++++++++++++++++--- src/scene_base.h | 6 +++ 2 files changed, 111 insertions(+), 6 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 1e862fd..9911045 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -74,17 +74,17 @@ struct ParameterLoopClosureFalko }; /** \brief A class for loop closure using falko library - * + * * The class is a wrapper of the falkolib that is designed to be used for loop closures in the wolf problem - * + * * It extracts scenes from a laserscanutils::LaserScan. The scenes contain keypoints and descriptors - * + * * It matches a target scene against a list of reference scenes. - * + * * The reference scenes are found from a search of the previous captures - * + * * Diferent types of descriptors can be used, and are specified as template parameters. - * + * * \tparam D Descriptor type. or * \tparam Extr descriptor extractor type or * \tparam M Matcher type or @@ -141,6 +141,87 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract extract(*scan_falko, (new_scene->keypoints_list_)); // Compute descriptors extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); + + // Compute Scene mid point + Eigen::Vector2d mid_point(0, 0); + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) + { + mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; + mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; + } + new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); + + // Compute max_dist + new_scene->max_distance_ = 0; + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) + for (int j = 0; j < new_scene->keypoints_list_.size(); j++) + { + double X_dist = + fabs(new_scene->keypoints_list_[i].point[0] - new_scene->keypoints_list_[j].point[0]); + double Y_dist = + fabs(new_scene->keypoints_list_[i].point[1] - new_scene->keypoints_list_[j].point[1]); + double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); + std::cout << "distance : " << distance << std::endl; + if (distance > new_scene->max_distance_) + new_scene->max_distance_ = distance; + } + + // Compute Scene Area and Perimeter + int n = 3; + double X[n]; + double Y[n]; + new_scene->perimeter_ = 0.0; + new_scene->area_ = 0.0; + Eigen::Vector2d dist_between_two_kp; + + if (new_scene->keypoints_list_.size() < 3) + return new_scene; + + if (new_scene->keypoints_list_.size() < 4) + { + X[0] = new_scene->keypoints_list_[0].point[0]; + Y[0] = new_scene->keypoints_list_[0].point[1]; + + X[1] = new_scene->keypoints_list_[1].point[0]; + Y[1] = new_scene->keypoints_list_[1].point[1]; + + X[2] = new_scene->keypoints_list_[2].point[0]; + Y[2] = new_scene->keypoints_list_[2].point[1]; + + new_scene->area_ = new_scene->area_ + triangleArea(X, Y, n); + return new_scene; + } + + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) + { + X[0] = new_scene->mid_point_[0]; + Y[0] = new_scene->mid_point_[1]; + + // X[0] = 0.0; + // Y[0] = 0.0; + + X[1] = new_scene->keypoints_list_[i].point[0]; + Y[1] = new_scene->keypoints_list_[i].point[1]; + + if (i < new_scene->keypoints_list_.size() - 1) // Proceed until final keypoint + { + X[2] = new_scene->keypoints_list_[i + 1].point[0]; + Y[2] = new_scene->keypoints_list_[i + 1].point[1]; + + dist_between_two_kp = + new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point; + } + else // then use inital keypoint + { + X[2] = new_scene->keypoints_list_[0].point[0]; + Y[2] = new_scene->keypoints_list_[0].point[1]; + + dist_between_two_kp = new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[0].point; + } + new_scene->area_ = new_scene->area_ + (double)triangleArea(X, Y, n); + new_scene->perimeter_ = new_scene->perimeter_ + hypot(dist_between_two_kp[0], dist_between_two_kp[1]); + } + return new_scene; } @@ -205,6 +286,24 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract int keypoints_number_th_; bool use_descriptors_; + + // (X[i], Y[i]) are coordinates of i'th point. + double triangleArea(double X[], double Y[], int n) + { + // Initialize area + double area = 0.0; + + // Calculate value of shoelace formula + int j = n - 1; + for (int i = 0; i < n; i++) + { + area += (X[j] + X[i]) * (Y[j] - Y[i]); + j = i; // j is previous vertex to i + } + + // Return absolute value + return fabs((double)area / 2.0); + } }; } /* namespace laserscanutils */ diff --git a/src/scene_base.h b/src/scene_base.h index a753162..76cfa61 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -16,6 +16,12 @@ namespace laserscanutils { struct SceneBase { int id; + double area_; + double perimeter_; + double max_distance_; + double mean_distance_; + Eigen::Vector2d mid_point_; + }; typedef std::shared_ptr sceneBasePtr; -- GitLab From 659213efa80e3c0467a10d82b837ffe76de06ad5 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 18 May 2021 12:39:03 +0200 Subject: [PATCH 086/100] wip --- src/loop_closure_falko.h | 140 ++++++++++++++++++++++++------ src/match_loop_closure_scene.h | 1 + src/scene_base.h | 1 + src/scene_falko.h | 6 ++ test/gtest_loop_closure_falko.cpp | 3 +- 5 files changed, 122 insertions(+), 29 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 9911045..588e531 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -127,6 +127,8 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract use_descriptors_ = _param.use_descriptors_; }; + // Template specialization + /** \brief Destructor **/ ~LoopClosureFalko() {} @@ -138,34 +140,61 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract auto new_scene = std::make_shared>(); auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); // Extract keypoints - extract(*scan_falko, (new_scene->keypoints_list_)); + std::vector keypoints_list; + extract(*scan_falko, keypoints_list); + + // Compute angle vector for keypoints + double angle_min = _scan_params.angle_min_; + double angle_step = _scan_params.angle_step_; + + // Compute max_dist + new_scene->max_distance_ = 0; + for (int i = 0; i < keypoints_list.size(); i++) + for (int j = 0; j < keypoints_list.size(); j++) + { + double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]); + double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]); + double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); + if (distance > new_scene->max_distance_) + new_scene->max_distance_ = distance; + } + + // discard too close by kp + for (int i = 0; i < keypoints_list.size(); i++) + { + int repeated = 0; + for (int j = i + 1; j < keypoints_list.size(); j++) + { + double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]); + double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]); + double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); + if (distance < 0.05) + { + repeated = 1; + } + } + if (repeated == 0) + { + new_scene->keypoints_list_.push_back(keypoints_list[i]); + } + } + // Compute descriptors extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); - // Compute Scene mid point + // Compute Scene mid point, angle for each keypoint and rotate descriptors Eigen::Vector2d mid_point(0, 0); + new_scene->descriptors_list_rotated = new_scene->descriptors_list_; + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) { mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; + new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); + new_scene->descriptors_list_rotated[i].rotate(-new_scene->angle_rotation_[i]); } new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); - // Compute max_dist - new_scene->max_distance_ = 0; - for (int i = 0; i < new_scene->keypoints_list_.size(); i++) - for (int j = 0; j < new_scene->keypoints_list_.size(); j++) - { - double X_dist = - fabs(new_scene->keypoints_list_[i].point[0] - new_scene->keypoints_list_[j].point[0]); - double Y_dist = - fabs(new_scene->keypoints_list_[i].point[1] - new_scene->keypoints_list_[j].point[1]); - double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); - std::cout << "distance : " << distance << std::endl; - if (distance > new_scene->max_distance_) - new_scene->max_distance_ = distance; - } - // Compute Scene Area and Perimeter int n = 3; double X[n]; @@ -211,7 +240,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract dist_between_two_kp = new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point; } - else // then use inital keypoint + else // if you arrived to the final keypoint then use inital keypoint { X[2] = new_scene->keypoints_list_[0].point[0]; Y[2] = new_scene->keypoints_list_[0].point[1]; @@ -222,6 +251,58 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract new_scene->perimeter_ = new_scene->perimeter_ + hypot(dist_between_two_kp[0], dist_between_two_kp[1]); } + // Compue Scene linear regresion and initial angle + + double ss_xy = 0; + double ss_xx = 0; + for (int i = 0; i <= new_scene->keypoints_list_.size(); i++) + { + ss_xy += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * + (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]); + ss_xx += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * + (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]); + } + double b_1 = ss_xy / ss_xx; + double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0]; + + new_scene->regressor_coefs.push_back(b_0); + new_scene->regressor_coefs.push_back(b_1); + + double initial_angle = -atan(b_1); + + // double inital_angle_inv = initial_angle - M_PI; + + // rotate keypoints + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) + { + falkolib::FALKO keypoint_mid; + keypoint_mid = new_scene->keypoints_list_[i]; + + keypoint_mid.point[0] = new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]; + keypoint_mid.point[1] = new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]; + new_scene->keipoints_list_mid_point_.push_back(keypoint_mid); + + falkolib::FALKO keypoint_rotated; + keypoint_rotated = new_scene->keypoints_list_[i]; + keypoint_rotated.point[0] = new_scene->keypoints_list_[i].point[0] * cos(initial_angle) - + new_scene->keypoints_list_[i].point[1] * sin(initial_angle); + keypoint_rotated.point[1] = new_scene->keypoints_list_[i].point[0] * sin(initial_angle) + + new_scene->keypoints_list_[i].point[1] * cos(initial_angle); + new_scene->keipoints_list_rotated_.push_back(keypoint_rotated); + + falkolib::FALKO keypoint_rot_trans; + keypoint_rot_trans = new_scene->keypoints_list_[i]; + keypoint_rot_trans.point[0] = + (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * cos(initial_angle) - + (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * sin(initial_angle); + + keypoint_rot_trans.point[1] = + (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * sin(initial_angle) + + (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * cos(initial_angle); + + new_scene->keipoints_list_transl_rot_.push_back(keypoint_rot_trans); + } + return new_scene; } @@ -243,7 +324,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract **/ MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override { - std::vector> asso_nn; auto scene_1_falko = std::static_pointer_cast>(_scene_1); auto scene_2_falko = std::static_pointer_cast>(_scene_2); @@ -252,14 +332,14 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract if (use_descriptors_ == 0) { - matching_number = - matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); + matching_number = matcher_.match(scene_1_falko->keipoints_list_transl_rot_, + scene_2_falko->keipoints_list_transl_rot_, asso_nn); } else if (use_descriptors_ == 1) { - matching_number = - matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_, - scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn); + matching_number = matcher_.match( + scene_1_falko->keipoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, + scene_2_falko->keipoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn); } auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; @@ -272,11 +352,15 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract { new_match->match = false; } - new_match->scene_1 = _scene_1; - new_match->scene_2 = _scene_2; - new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), - scene_2_falko->keypoints_list_.size()); + new_match->associations = asso_nn; + new_match->scene_1 = _scene_1; + new_match->scene_2 = _scene_2; + + // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), + // scene_2_falko->keypoints_list_.size()); + + new_match->score = (double)matching_number / 10.0; new_match->transform_vector.head(2) = new_match->transform.translation(); new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); diff --git a/src/match_loop_closure_scene.h b/src/match_loop_closure_scene.h index 423398f..a99dc00 100644 --- a/src/match_loop_closure_scene.h +++ b/src/match_loop_closure_scene.h @@ -24,6 +24,7 @@ struct MatchLoopClosureScene { bool match; int keypoints_number_match; double score; + std::vector> associations; Eigen::Affine2d transform; Eigen::Vector3d transform_vector; }; diff --git a/src/scene_base.h b/src/scene_base.h index 76cfa61..740abf3 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -21,6 +21,7 @@ struct SceneBase double max_distance_; double mean_distance_; Eigen::Vector2d mid_point_; + std::vector regressor_coefs; }; typedef std::shared_ptr sceneBasePtr; diff --git a/src/scene_falko.h b/src/scene_falko.h index 18529b1..d8b4795 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -35,7 +35,13 @@ typedef falkolib::CGH cgh; template struct SceneFalko : public SceneBase { std::vector keypoints_list_; + std::vector keipoints_list_mid_point_; + std::vector keipoints_list_transl_rot_; + std::vector keipoints_list_rotated_; + std::vector keipoints_list_rotated_reverse_; std::vector descriptors_list_; + std::vector descriptors_list_rotated; + std::vector angle_rotation_; }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index f01b38b..378ec13 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -20,7 +20,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) } ParameterLoopClosureFalko param; - LoopClosureFalko loop_cl_falko(param); + param.matcher_distance_th_= 0.3; + LoopClosureFalko loop_cl_falko(param); // Test convert2LaserScanFALKO std::shared_ptr scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params); -- GitLab From b731ea4f2245cd1261221a682d0a64016ef77cae Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 24 May 2021 12:53:31 +0200 Subject: [PATCH 087/100] added gtest for descriptors rotation --- src/loop_closure_falko.h | 49 +- src/scene_falko.h | 8 +- test/CMakeLists.txt | 8 +- test/data/scan_data.h | 2903 +++++++++++++++++++++++++++++ test/gtest_loop_closure_falko.cpp | 218 ++- test/matplotlibcpp.h | 2555 +++++++++++++++++++++++++ 6 files changed, 5710 insertions(+), 31 deletions(-) create mode 100644 test/data/scan_data.h create mode 100644 test/matplotlibcpp.h diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 588e531..453cdbe 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -143,7 +143,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract std::vector keypoints_list; extract(*scan_falko, keypoints_list); - // Compute angle vector for keypoints double angle_min = _scan_params.angle_min_; double angle_step = _scan_params.angle_step_; @@ -181,20 +180,26 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract // Compute descriptors extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); + std::vector descriptors_list_rotated; + extractor_.compute(*scan_falko, new_scene->keypoints_list_, descriptors_list_rotated); // Compute Scene mid point, angle for each keypoint and rotate descriptors - Eigen::Vector2d mid_point(0, 0); - new_scene->descriptors_list_rotated = new_scene->descriptors_list_; - + Eigen::Vector2d mid_point(0, 0); + std::vector angle_rotation; for (int i = 0; i < new_scene->keypoints_list_.size(); i++) { mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; - new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); - new_scene->descriptors_list_rotated[i].rotate(-new_scene->angle_rotation_[i]); + // new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); + angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); + // std::cout << "angle_min :" << angle_min << std::endl; + // std::cout << "angle_min :" << angle_step << std::endl; + // std::cout << "new_scene->angle_rotation_[i] :" << angle_rotation[i] << std::endl; + descriptors_list_rotated[i].rotate(angle_rotation[i]); + new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]); } + new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); - // Compute Scene Area and Perimeter int n = 3; double X[n]; @@ -252,7 +257,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract } // Compue Scene linear regresion and initial angle - double ss_xy = 0; double ss_xx = 0; for (int i = 0; i <= new_scene->keypoints_list_.size(); i++) @@ -263,10 +267,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]); } double b_1 = ss_xy / ss_xx; - double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0]; + // double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0]; - new_scene->regressor_coefs.push_back(b_0); - new_scene->regressor_coefs.push_back(b_1); + // new_scene->regressor_coefs.push_back(b_0); + // new_scene->regressor_coefs.push_back(b_1); double initial_angle = -atan(b_1); @@ -280,7 +284,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract keypoint_mid.point[0] = new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]; keypoint_mid.point[1] = new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]; - new_scene->keipoints_list_mid_point_.push_back(keypoint_mid); + new_scene->keypoints_list_mid_point_.push_back(keypoint_mid); falkolib::FALKO keypoint_rotated; keypoint_rotated = new_scene->keypoints_list_[i]; @@ -288,7 +292,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract new_scene->keypoints_list_[i].point[1] * sin(initial_angle); keypoint_rotated.point[1] = new_scene->keypoints_list_[i].point[0] * sin(initial_angle) + new_scene->keypoints_list_[i].point[1] * cos(initial_angle); - new_scene->keipoints_list_rotated_.push_back(keypoint_rotated); + new_scene->keypoints_list_rotated_.push_back(keypoint_rotated); falkolib::FALKO keypoint_rot_trans; keypoint_rot_trans = new_scene->keypoints_list_[i]; @@ -300,7 +304,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * sin(initial_angle) + (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * cos(initial_angle); - new_scene->keipoints_list_transl_rot_.push_back(keypoint_rot_trans); + new_scene->keypoints_list_transl_rot_.push_back(keypoint_rot_trans); } return new_scene; @@ -332,14 +336,21 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract if (use_descriptors_ == 0) { - matching_number = matcher_.match(scene_1_falko->keipoints_list_transl_rot_, - scene_2_falko->keipoints_list_transl_rot_, asso_nn); + // matching_number = matcher_.match(scene_1_falko->keypoints_list_transl_rot_, + // scene_2_falko->keypoints_list_transl_rot_, asso_nn); + + matching_number = + matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); } else if (use_descriptors_ == 1) { - matching_number = matcher_.match( - scene_1_falko->keipoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, - scene_2_falko->keipoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn); + // matching_number = matcher_.match( + // scene_1_falko->keypoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, + // scene_2_falko->keypoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn); + + matching_number = + matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated, + scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn); } auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; diff --git a/src/scene_falko.h b/src/scene_falko.h index d8b4795..9dbd1f8 100644 --- a/src/scene_falko.h +++ b/src/scene_falko.h @@ -35,10 +35,10 @@ typedef falkolib::CGH cgh; template struct SceneFalko : public SceneBase { std::vector keypoints_list_; - std::vector keipoints_list_mid_point_; - std::vector keipoints_list_transl_rot_; - std::vector keipoints_list_rotated_; - std::vector keipoints_list_rotated_reverse_; + std::vector keypoints_list_mid_point_; + std::vector keypoints_list_transl_rot_; + std::vector keypoints_list_rotated_; + std::vector keypoints_list_rotated_reverse_; std::vector descriptors_list_; std::vector descriptors_list_rotated; std::vector angle_rotation_; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 89062a0..aa60e80 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -6,8 +6,9 @@ include_directories(${GTEST_INCLUDE_DIRS}) #Include directories INCLUDE_DIRECTORIES(../src) -INCLUDE_DIRECTORIES(../test) +INCLUDE_DIRECTORIES(/data) FIND_PACKAGE(Eigen3 3.3 REQUIRED) +find_package(PythonLibs 2.7) INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) ############# USE THIS TEST AS AN EXAMPLE #################### @@ -22,5 +23,6 @@ INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) gnss_utils_add_gtest(gtest_example gtest_example.cpp) target_link_libraries(gtest_example ${PROJECT_NAME}) -gnss_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/testData2.cpp) -target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME}) +gnss_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/data) +target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME} ${PYTHON_LIBRARIES}) +target_include_directories(gtest_loop_closure_falko PRIVATE ${PYTHON_INCLUDE_DIRS}) diff --git a/test/data/scan_data.h b/test/data/scan_data.h new file mode 100644 index 0000000..a98a244 --- /dev/null +++ b/test/data/scan_data.h @@ -0,0 +1,2903 @@ +/** + * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant + * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. + * + * This file is part of FALKOLib. + * + * FALKOLib is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * FALKOLib is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with FALKOLib. If not, see . + */ +std::vector testRanges1 = {250, + 250, + 250, + 250, + 250, + 17.263999938964844, + 17.263999938964844, + 17.263999938964844, + 17.263999938964844, + 13.552000045776367, + 13.552000045776367, + 13.552000045776367, + 12.95199966430664, + 12.95199966430664, + 12.95199966430664, + 12.95199966430664, + 12.51200008392334, + 11.567999839782715, + 11.567999839782715, + 10.720000267028809, + 10.720000267028809, + 10.720000267028809, + 10.359999656677246, + 10.336000442504883, + 10.343999862670898, + 10.35200023651123, + 10.35200023651123, + 10.32800006866455, + 10.319999694824219, + 10.383999824523926, + 10.407999992370605, + 10.432000160217285, + 10.432000160217285, + 10.447999954223633, + 10.46399974822998, + 10.46399974822998, + 10.503999710083008, + 10.503999710083008, + 10.527999877929688, + 10.592000007629395, + 10.62399959564209, + 8.368000030517578, + 8.343999862670898, + 8.343999862670898, + 8.031999588012695, + 7.831999778747559, + 7.6479997634887695, + 7.552000045776367, + 7.415999889373779, + 7.415999889373779, + 7.263999938964844, + 7.144000053405762, + 7.0320000648498535, + 6.920000076293945, + 6.776000022888184, + 6.776000022888184, + 6.671999931335449, + 6.576000213623047, + 6.440000057220459, + 6.328000068664551, + 6.271999835968018, + 6.271999835968018, + 6.263999938964844, + 6.271999835968018, + 6.296000003814697, + 6.320000171661377, + 6.328000068664551, + 6.328000068664551, + 6.335999965667725, + 6.320000171661377, + 3.187999963760376, + 3.187999963760376, + 4.495999813079834, + 4.495999813079834, + 4.495999813079834, + 4.495999813079834, + 4.504000186920166, + 4.504000186920166, + 4.504000186920166, + 4.544000148773193, + 4.544000148773193, + 4.544000148773193, + 4.544000148773193, + 4.51200008392334, + 4.51200008392334, + 4.51200008392334, + 4.51200008392334, + 4.599999904632568, + 4.576000213623047, + 4.495999813079834, + 4.455999851226807, + 4.455999851226807, + 4.455999851226807, + 4.455999851226807, + 4.4079999923706055, + 4.368000030517578, + 4.320000171661377, + 4.271999835968018, + 4.271999835968018, + 4.223999977111816, + 4.184000015258789, + 4.168000221252441, + 4.159999847412109, + 4.168000221252441, + 4.176000118255615, + 4.176000118255615, + 4.184000015258789, + 4.199999809265137, + 4.199999809265137, + 4.208000183105469, + 4.216000080108643, + 4.216000080108643, + 4.223999977111816, + 4.23199987411499, + 4.23199987411499, + 4.176000118255615, + 250, + 250, + 4.111999988555908, + 4.111999988555908, + 4.111999988555908, + 4.111999988555908, + 4.099999904632568, + 4.099999904632568, + 4.099999904632568, + 4.079999923706055, + 4.0320000648498535, + 3.98799991607666, + 3.9719998836517334, + 3.9719998836517334, + 3.9560000896453857, + 3.9240000247955322, + 3.888000011444092, + 3.859999895095825, + 3.8320000171661377, + 3.8320000171661377, + 3.803999900817871, + 3.7880001068115234, + 3.259999990463257, + 3.240000009536743, + 3.2200000286102295, + 3.2200000286102295, + 3.200000047683716, + 3.1760001182556152, + 3.1559998989105225, + 3.1440000534057617, + 3.128000020980835, + 3.128000020980835, + 3.119999885559082, + 2.5959999561309814, + 2.5959999561309814, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 2.4839999675750732, + 2.4839999675750732, + 2.4839999675750732, + 2.4839999675750732, + 2.496000051498413, + 2.5320000648498535, + 250, + 2.624000072479248, + 2.624000072479248, + 2.624000072479248, + 2.624000072479248, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 2.6679999828338623, + 2.6679999828338623, + 2.6679999828338623, + 2.6679999828338623, + 2.640000104904175, + 2.640000104904175, + 2.640000104904175, + 2.628000020980835, + 2.615999937057495, + 2.615999937057495, + 2.6080000400543213, + 2.5959999561309814, + 2.5880000591278076, + 2.5759999752044678, + 2.563999891281128, + 2.563999891281128, + 2.555999994277954, + 2.5439999103546143, + 2.5320000648498535, + 2.5199999809265137, + 2.51200008392334, + 2.51200008392334, + 2.503999948501587, + 2.492000102996826, + 2.4839999675750732, + 2.4719998836517334, + 2.4639999866485596, + 2.4639999866485596, + 2.4600000381469727, + 2.4519999027252197, + 2.440000057220459, + 2.431999921798706, + 2.4200000762939453, + 2.4200000762939453, + 2.4159998893737793, + 2.4079999923706055, + 2.4079999923706055, + 2.427999973297119, + 2.444000005722046, + 2.444000005722046, + 2.4600000381469727, + 2.4719998836517334, + 2.48799991607666, + 2.5, + 2.5160000324249268, + 2.5160000324249268, + 2.5280001163482666, + 2.5399999618530273, + 2.559999942779541, + 2.5799999237060547, + 2.5840001106262207, + 2.5840001106262207, + 3.0480000972747803, + 3.0480000972747803, + 2.631999969482422, + 2.631999969482422, + 2.631999969482422, + 2.624000072479248, + 2.624000072479248, + 2.624000072479248, + 2.624000072479248, + 2.615999937057495, + 2.611999988555908, + 2.6040000915527344, + 2.6040000915527344, + 2.5959999561309814, + 2.5920000076293945, + 2.5840001106262207, + 2.5759999752044678, + 2.572000026702881, + 2.572000026702881, + 2.563999891281128, + 2.555999994277954, + 2.5480000972747803, + 2.5439999103546143, + 2.5399999618530273, + 2.5399999618530273, + 2.5360000133514404, + 2.5320000648498535, + 2.5239999294281006, + 2.5199999809265137, + 2.507999897003174, + 2.507999897003174, + 2.503999948501587, + 2.5, + 2.496000051498413, + 2.492000102996826, + 2.48799991607666, + 2.48799991607666, + 2.4800000190734863, + 2.4760000705718994, + 2.4719998836517334, + 2.4679999351501465, + 2.4639999866485596, + 2.4560000896453857, + 2.4560000896453857, + 2.4519999027252197, + 2.447999954223633, + 2.447999954223633, + 2.444000005722046, + 2.436000108718872, + 2.436000108718872, + 2.431999921798706, + 2.427999973297119, + 2.4240000247955322, + 2.4240000247955322, + 2.4200000762939453, + 2.4200000762939453, + 2.4159998893737793, + 2.4079999923706055, + 2.4040000438690186, + 2.4040000438690186, + 2.4000000953674316, + 2.4000000953674316, + 2.3959999084472656, + 2.3919999599456787, + 2.3919999599456787, + 2.388000011444092, + 2.384000062942505, + 2.384000062942505, + 2.380000114440918, + 2.375999927520752, + 2.371999979019165, + 2.371999979019165, + 2.368000030517578, + 2.364000082015991, + 2.364000082015991, + 2.364000082015991, + 2.364000082015991, + 2.359999895095825, + 2.3559999465942383, + 2.3559999465942383, + 2.3559999465942383, + 2.6600000858306885, + 2.6600000858306885, + 2.6600000858306885, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.0, + 2.0, + 2.0, + 2.0, + 1.9980000257492065, + 1.9980000257492065, + 1.9980000257492065, + 1.9980000257492065, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.99399995803833, + 1.9919999837875366, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.99399995803833, + 2.0, + 2.0, + 2.009999990463257, + 2.0220000743865967, + 2.0460000038146973, + 2.0460000038146973, + 2.0260000228881836, + 2.0260000228881836, + 2.00600004196167, + 1.996000051498413, + 1.99399995803833, + 1.9900000095367432, + 1.9919999837875366, + 1.9919999837875366, + 1.996000051498413, + 1.9980000257492065, + 1.99399995803833, + 1.9919999837875366, + 1.9919999837875366, + 1.9919999837875366, + 1.99399995803833, + 1.99399995803833, + 1.996000051498413, + 1.9980000257492065, + 1.9980000257492065, + 1.9980000257492065, + 1.9980000257492065, + 2.0, + 2.0, + 1.9980000257492065, + 1.9980000257492065, + 1.9919999837875366, + 1.9520000219345093, + 2.0320000648498535, + 2.0999999046325684, + 2.1040000915527344, + 2.1040000915527344, + 2.0280001163482666, + 1.9040000438690186, + 1.9279999732971191, + 1.9520000219345093, + 1.9559999704360962, + 1.9559999704360962, + 1.9600000381469727, + 1.9579999446868896, + 1.9600000381469727, + 1.9620000123977661, + 1.9639999866485596, + 1.9639999866485596, + 1.965999960899353, + 1.965999960899353, + 1.968000054359436, + 1.972000002861023, + 1.9759999513626099, + 1.9759999513626099, + 1.9759999513626099, + 1.9759999513626099, + 1.9800000190734863, + 1.9839999675750732, + 1.9800000190734863, + 1.9800000190734863, + 1.9819999933242798, + 1.9880000352859497, + 1.996000051498413, + 2.009999990463257, + 2.00600004196167, + 2.00600004196167, + 1.99399995803833, + 2.687999963760376, + 2.687999963760376, + 2.687999963760376, + 1.74399995803833, + 1.74399995803833, + 2.063999891281128, + 2.115999937057495, + 2.196000099182129, + 2.191999912261963, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5199999809265137, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5280001163482666, + 2.5280001163482666, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5280001163482666, + 2.5239999294281006, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.5360000133514404, + 2.5399999618530273, + 2.5480000972747803, + 2.5480000972747803, + 2.5480000972747803, + 2.5399999618530273, + 2.5439999103546143, + 2.552000045776367, + 2.555999994277954, + 2.555999994277954, + 2.552000045776367, + 2.5480000972747803, + 250, + 1.3179999589920044, + 1.3179999589920044, + 1.3179999589920044, + 1.3179999589920044, + 1.3079999685287476, + 1.2999999523162842, + 1.2940000295639038, + 1.2940000295639038, + 1.2879999876022339, + 1.2860000133514404, + 1.2940000295639038, + 1.3040000200271606, + 1.3040000200271606, + 1.3040000200271606, + 1.2999999523162842, + 1.2979999780654907, + 1.3020000457763672, + 1.309999942779541, + 1.3140000104904175, + 1.3140000104904175, + 1.3140000104904175, + 1.3240000009536743, + 1.3359999656677246, + 1.3420000076293945, + 1.343999981880188, + 1.343999981880188, + 1.2200000286102295, + 250, + 250, + 250, + 250, + 250, + 1.093999981880188, + 1.093999981880188, + 1.093999981880188, + 1.093999981880188, + 1.090000033378601, + 1.090000033378601, + 1.0920000076293945, + 1.0980000495910645, + 1.0980000495910645, + 1.100000023841858, + 1.100000023841858, + 1.100000023841858, + 1.100000023841858, + 1.0980000495910645, + 1.0980000495910645, + 1.0980000495910645, + 1.0959999561309814, + 1.093999981880188, + 1.093999981880188, + 1.093999981880188, + 1.093999981880188, + 1.093999981880188, + 1.0920000076293945, + 1.093999981880188, + 1.093999981880188, + 1.093999981880188, + 1.1019999980926514, + 1.1119999885559082, + 1.121999979019165, + 1.1260000467300415, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 5.791999816894531, + 5.791999816894531, + 5.791999816894531, + 5.791999816894531, + 5.776000022888184, + 5.776000022888184, + 5.776000022888184, + 5.751999855041504, + 5.751999855041504, + 5.751999855041504, + 5.751999855041504, + 5.751999855041504, + 5.736000061035156, + 5.679999828338623, + 5.671999931335449, + 5.671999931335449, + 5.631999969482422, + 5.631999969482422, + 5.552000045776367, + 5.552000045776367, + 5.552000045776367, + 5.552000045776367, + 5.559999942779541, + 5.584000110626221, + 6.104000091552734, + 6.783999919891357, + 6.831999778747559, + 6.831999778747559, + 6.831999778747559, + 6.760000228881836, + 6.616000175476074, + 6.455999851226807, + 6.343999862670898, + 6.343999862670898, + 6.320000171661377, + 6.311999797821045, + 9.192000389099121, + 9.175999641418457, + 9.15999984741211, + 9.15999984741211, + 9.128000259399414, + 9.119999885559082, + 7.992000102996826, + 7.9120001792907715, + 7.8480000495910645, + 7.8480000495910645, + 7.800000190734863, + 7.74399995803833, + 7.71999979019165, + 7.711999893188477, + 7.711999893188477, + 7.711999893188477, + 7.711999893188477, + 7.71999979019165, + 7.71999979019165, + 7.711999893188477, + 7.703999996185303, + 7.703999996185303, + 7.703999996185303, + 7.688000202178955, + 7.688000202178955, + 7.688000202178955, + 7.688000202178955, + 7.671999931335449, + 8.67199993133545, + 8.640000343322754, + 8.607999801635742, + 8.583999633789062, + 8.583999633789062, + 8.5600004196167, + 8.552000045776367, + 8.53600025177002, + 8.520000457763672, + 8.503999710083008, + 8.503999710083008, + 8.472000122070312, + 8.447999954223633, + 8.416000366210938, + 8.383999824523926, + 8.383999824523926, + 8.383999824523926, + 8.368000030517578, + 8.35200023651123, + 8.35200023651123, + 8.336000442504883, + 8.303999900817871, + 8.303999900817871, + 8.263999938964844, + 8.239999771118164, + 8.279999732971191, + 8.392000198364258, + 8.496000289916992, + 8.496000289916992, + 8.656000137329102, + 8.807999610900879, + 8.928000450134277, + 9.015999794006348, + 11.767999649047852, + 11.767999649047852, + 12.071999549865723, + 12.175999641418457, + 12.567999839782715, + 12.663999557495117, + 13.064000129699707, + 13.064000129699707, + 13.168000221252441, + 13.607999801635742, + 13.640000343322754, + 13.53600025177002, + 13.968000411987305, + 13.968000411987305, + 14.008000373840332, + 14.104000091552734, + 14.175999641418457, + 14.104000091552734, + 15.4399995803833, + 15.4399995803833, + 15.543999671936035, + 15.48799991607666, + 15.48799991607666, + 15.503999710083008, + 15.479999542236328, + 15.479999542236328, + 15.447999954223633, + 15.399999618530273, + 15.37600040435791, + 15.35200023651123, + 15.319999694824219, + 15.319999694824219, + 15.272000312805176, + 15.272000312805176, + 15.336000442504883, + 15.303999900817871, + 15.248000144958496, + 15.248000144958496, + 14.744000434875488, + 15.303999900817871, + 15.272000312805176, + 15.248000144958496, + 15.215999603271484, + 15.223999977111816, + 15.223999977111816, + 15.248000144958496, + 16.784000396728516, + 16.719999313354492, + 16.719999313354492, + 16.719999313354492, + 16.719999313354492, + 16.672000885009766, + 16.687999725341797, + 16.719999313354492, + 16.70400047302246, + 16.607999801635742, + 16.607999801635742, + 16.624000549316406, + 16.6560001373291, + 16.23200035095215, + 16.143999099731445, + 16.06399917602539, + 16.06399917602539, + 16.016000747680664, + 17.472000122070312, + 19.007999420166016, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250}; + +std::vector testRanges2 = {250, + 250, + 250, + 250, + 16.799999237060547, + 16.799999237060547, + 16.799999237060547, + 16.799999237060547, + 14.095999717712402, + 14.095999717712402, + 14.095999717712402, + 14.095999717712402, + 13.255999565124512, + 13.255999565124512, + 13.255999565124512, + 12.767999649047852, + 12.175999641418457, + 12.175999641418457, + 10.911999702453613, + 10.4399995803833, + 10.4399995803833, + 10.4399995803833, + 10.32800006866455, + 10.32800006866455, + 10.32800006866455, + 10.343999862670898, + 10.336000442504883, + 10.312000274658203, + 10.368000030517578, + 10.368000030517578, + 10.407999992370605, + 10.472000122070312, + 10.48799991607666, + 10.520000457763672, + 10.48799991607666, + 10.48799991607666, + 10.472000122070312, + 10.503999710083008, + 10.51200008392334, + 10.51200008392334, + 10.520000457763672, + 10.520000457763672, + 8.295999526977539, + 8.13599967956543, + 7.984000205993652, + 7.791999816894531, + 7.624000072479248, + 7.624000072479248, + 7.5279998779296875, + 7.4079999923706055, + 7.263999938964844, + 7.119999885559082, + 7.007999897003174, + 7.007999897003174, + 6.9120001792907715, + 6.760000228881836, + 6.639999866485596, + 6.544000148773193, + 6.415999889373779, + 6.415999889373779, + 6.311999797821045, + 6.271999835968018, + 6.263999938964844, + 6.288000106811523, + 6.303999900817871, + 6.303999900817871, + 6.320000171661377, + 6.335999965667725, + 6.343999862670898, + 6.335999965667725, + 6.303999900817871, + 6.303999900817871, + 6.271999835968018, + 4.5920000076293945, + 250, + 4.480000019073486, + 4.480000019073486, + 4.480000019073486, + 4.480000019073486, + 4.4720001220703125, + 4.504000186920166, + 4.519999980926514, + 4.5279998779296875, + 4.5279998779296875, + 4.5279998779296875, + 4.5279998779296875, + 4.567999839782715, + 4.559999942779541, + 250, + 4.48799991607666, + 4.48799991607666, + 4.48799991607666, + 4.48799991607666, + 4.440000057220459, + 4.392000198364258, + 4.360000133514404, + 4.360000133514404, + 4.320000171661377, + 4.263999938964844, + 4.223999977111816, + 4.191999912261963, + 4.168000221252441, + 4.168000221252441, + 4.168000221252441, + 4.176000118255615, + 4.176000118255615, + 4.191999912261963, + 4.208000183105469, + 4.208000183105469, + 4.208000183105469, + 4.216000080108643, + 4.216000080108643, + 4.216000080108643, + 4.23199987411499, + 4.23199987411499, + 4.223999977111816, + 4.184000015258789, + 250, + 4.136000156402588, + 4.136000156402588, + 4.136000156402588, + 4.136000156402588, + 4.111999988555908, + 4.111999988555908, + 4.0960001945495605, + 4.064000129699707, + 4.015999794006348, + 4.015999794006348, + 3.9839999675750732, + 3.9679999351501465, + 3.947999954223633, + 3.9119999408721924, + 3.875999927520752, + 3.875999927520752, + 3.8480000495910645, + 3.8239998817443848, + 3.7039999961853027, + 3.5920000076293945, + 3.259999990463257, + 3.259999990463257, + 3.240000009536743, + 3.2160000801086426, + 3.196000099182129, + 3.171999931335449, + 3.1519999504089355, + 3.1519999504089355, + 3.140000104904175, + 3.124000072479248, + 3.115999937057495, + 2.5959999561309814, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 2.51200008392334, + 2.51200008392334, + 2.51200008392334, + 2.51200008392334, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.6040000915527344, + 2.6040000915527344, + 2.6040000915527344, + 2.6040000915527344, + 250, + 250, + 250, + 2.6480000019073486, + 2.6480000019073486, + 2.6480000019073486, + 2.6480000019073486, + 250, + 250, + 250, + 2.6480000019073486, + 2.6480000019073486, + 2.6480000019073486, + 2.6480000019073486, + 2.635999917984009, + 2.628000020980835, + 2.619999885559082, + 2.619999885559082, + 2.6080000400543213, + 2.5959999561309814, + 2.5880000591278076, + 2.5759999752044678, + 2.563999891281128, + 2.563999891281128, + 2.552000045776367, + 2.5399999618530273, + 2.5280001163482666, + 2.5199999809265137, + 2.51200008392334, + 2.51200008392334, + 2.5, + 2.48799991607666, + 2.4839999675750732, + 2.4719998836517334, + 2.4639999866485596, + 2.4639999866485596, + 2.4560000896453857, + 2.444000005722046, + 2.436000108718872, + 2.431999921798706, + 2.4200000762939453, + 2.4200000762939453, + 2.4159998893737793, + 2.4159998893737793, + 2.4200000762939453, + 2.431999921798706, + 2.444000005722046, + 2.4600000381469727, + 2.4600000381469727, + 2.4760000705718994, + 2.48799991607666, + 2.5, + 2.5160000324249268, + 2.5360000133514404, + 2.5360000133514404, + 2.5480000972747803, + 2.563999891281128, + 2.5840001106262207, + 2.7079999446868896, + 2.684000015258789, + 2.684000015258789, + 2.5880000591278076, + 2.5880000591278076, + 2.5880000591278076, + 2.631999969482422, + 2.631999969482422, + 2.631999969482422, + 2.631999969482422, + 2.624000072479248, + 2.615999937057495, + 2.611999988555908, + 2.6040000915527344, + 2.6040000915527344, + 2.5920000076293945, + 2.5880000591278076, + 2.5840001106262207, + 2.5759999752044678, + 2.572000026702881, + 2.559999942779541, + 2.559999942779541, + 2.552000045776367, + 2.5480000972747803, + 2.5439999103546143, + 2.5399999618530273, + 2.5320000648498535, + 2.5320000648498535, + 2.5280001163482666, + 2.5239999294281006, + 2.5160000324249268, + 2.507999897003174, + 2.503999948501587, + 2.503999948501587, + 2.5, + 2.492000102996826, + 2.492000102996826, + 2.4839999675750732, + 2.4800000190734863, + 2.4800000190734863, + 2.4760000705718994, + 2.4719998836517334, + 2.4679999351501465, + 2.4600000381469727, + 2.4600000381469727, + 2.4519999027252197, + 2.447999954223633, + 2.447999954223633, + 2.447999954223633, + 2.440000057220459, + 2.440000057220459, + 2.431999921798706, + 2.427999973297119, + 2.427999973297119, + 2.4240000247955322, + 2.4200000762939453, + 2.4200000762939453, + 2.4159998893737793, + 2.4119999408721924, + 2.4079999923706055, + 2.4040000438690186, + 2.4000000953674316, + 2.4000000953674316, + 2.4000000953674316, + 2.3959999084472656, + 2.3919999599456787, + 2.388000011444092, + 2.384000062942505, + 2.384000062942505, + 2.384000062942505, + 2.380000114440918, + 2.375999927520752, + 2.371999979019165, + 2.371999979019165, + 2.371999979019165, + 2.368000030517578, + 2.364000082015991, + 2.364000082015991, + 2.364000082015991, + 2.364000082015991, + 2.364000082015991, + 2.359999895095825, + 2.371999979019165, + 2.388000011444092, + 2.388000011444092, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.002000093460083, + 2.002000093460083, + 2.002000093460083, + 2.002000093460083, + 2.002000093460083, + 1.9980000257492065, + 1.9980000257492065, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.9980000257492065, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.99399995803833, + 1.9919999837875366, + 1.99399995803833, + 1.9919999837875366, + 1.9919999837875366, + 1.9919999837875366, + 1.996000051498413, + 1.996000051498413, + 2.0, + 2.009999990463257, + 2.009999990463257, + 2.0320000648498535, + 2.0480000972747803, + 2.0320000648498535, + 2.0179998874664307, + 2.007999897003174, + 2.007999897003174, + 1.996000051498413, + 1.9919999837875366, + 1.9900000095367432, + 1.9880000352859497, + 1.99399995803833, + 1.99399995803833, + 1.99399995803833, + 1.99399995803833, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.996000051498413, + 1.99399995803833, + 1.99399995803833, + 1.9980000257492065, + 1.9980000257492065, + 2.0, + 2.002000093460083, + 2.0, + 2.002000093460083, + 1.9759999513626099, + 1.9759999513626099, + 1.9359999895095825, + 2.0360000133514404, + 2.0859999656677246, + 2.0439999103546143, + 1.9819999933242798, + 1.9819999933242798, + 1.9279999732971191, + 1.9559999704360962, + 1.9559999704360962, + 1.9600000381469727, + 1.9579999446868896, + 1.9559999704360962, + 1.9559999704360962, + 1.9579999446868896, + 1.9620000123977661, + 1.9639999866485596, + 1.968000054359436, + 1.9700000286102295, + 1.9700000286102295, + 1.972000002861023, + 1.9739999771118164, + 1.9739999771118164, + 1.9739999771118164, + 1.9780000448226929, + 1.9780000448226929, + 1.9839999675750732, + 1.9859999418258667, + 1.9859999418258667, + 1.9900000095367432, + 1.99399995803833, + 1.99399995803833, + 1.996000051498413, + 1.99399995803833, + 1.9980000257492065, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.003999948501587, + 2.5999999046325684, + 1.6720000505447388, + 2.0480000972747803, + 2.115999937057495, + 2.115999937057495, + 2.191999912261963, + 2.196000099182129, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 2.5160000324249268, + 2.5160000324249268, + 2.5160000324249268, + 2.5160000324249268, + 2.5160000324249268, + 2.5160000324249268, + 2.5160000324249268, + 2.5239999294281006, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5280001163482666, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5239999294281006, + 2.5280001163482666, + 2.5320000648498535, + 2.5320000648498535, + 2.5239999294281006, + 2.5239999294281006, + 2.5280001163482666, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.5320000648498535, + 2.5360000133514404, + 2.5360000133514404, + 2.5360000133514404, + 2.5360000133514404, + 2.5399999618530273, + 2.5439999103546143, + 2.5439999103546143, + 2.5439999103546143, + 2.5439999103546143, + 2.5439999103546143, + 2.5480000972747803, + 2.555999994277954, + 2.559999942779541, + 1.315999984741211, + 1.315999984741211, + 1.315999984741211, + 1.315999984741211, + 1.3040000200271606, + 1.3040000200271606, + 1.2979999780654907, + 1.2920000553131104, + 1.2920000553131104, + 1.2860000133514404, + 1.2899999618530273, + 1.2999999523162842, + 1.3020000457763672, + 1.3020000457763672, + 1.3020000457763672, + 1.2999999523162842, + 1.2960000038146973, + 1.3020000457763672, + 1.312000036239624, + 1.315999984741211, + 1.315999984741211, + 1.3179999589920044, + 1.3279999494552612, + 1.3359999656677246, + 1.3420000076293945, + 1.2200000286102295, + 1.2200000286102295, + 250, + 250, + 250, + 250, + 250, + 1.090000033378601, + 1.090000033378601, + 1.090000033378601, + 1.090000033378601, + 1.0880000591278076, + 1.0880000591278076, + 1.0880000591278076, + 1.0920000076293945, + 1.0959999561309814, + 1.0980000495910645, + 1.0980000495910645, + 1.0980000495910645, + 1.0980000495910645, + 1.0980000495910645, + 1.0980000495910645, + 1.0959999561309814, + 1.0959999561309814, + 1.0959999561309814, + 1.0959999561309814, + 1.093999981880188, + 1.093999981880188, + 1.093999981880188, + 1.0920000076293945, + 1.0920000076293945, + 1.0920000076293945, + 1.0920000076293945, + 1.093999981880188, + 1.1019999980926514, + 1.1139999628067017, + 1.1239999532699585, + 1.1239999532699585, + 1.1299999952316284, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 250, + 5.888000011444092, + 5.888000011444092, + 5.888000011444092, + 5.888000011444092, + 5.863999843597412, + 5.863999843597412, + 5.808000087738037, + 5.751999855041504, + 5.751999855041504, + 5.74399995803833, + 5.74399995803833, + 5.656000137329102, + 5.656000137329102, + 5.656000137329102, + 5.584000110626221, + 5.584000110626221, + 5.584000110626221, + 5.584000110626221, + 5.552000045776367, + 5.559999942779541, + 5.599999904632568, + 5.599999904632568, + 6.71999979019165, + 6.783999919891357, + 6.831999778747559, + 6.815999984741211, + 6.696000099182129, + 6.552000045776367, + 6.552000045776367, + 6.423999786376953, + 6.343999862670898, + 6.328000068664551, + 9.232000350952148, + 9.192000389099121, + 9.192000389099121, + 9.175999641418457, + 9.144000053405762, + 9.039999961853027, + 8.960000038146973, + 7.943999767303467, + 7.943999767303467, + 7.863999843597412, + 7.8480000495910645, + 7.791999816894531, + 7.751999855041504, + 7.736000061035156, + 7.736000061035156, + 7.71999979019165, + 7.728000164031982, + 7.728000164031982, + 7.711999893188477, + 7.696000099182129, + 7.696000099182129, + 7.688000202178955, + 7.696000099182129, + 7.696000099182129, + 7.688000202178955, + 7.688000202178955, + 7.688000202178955, + 7.736000061035156, + 7.800000190734863, + 8.616000175476074, + 8.600000381469727, + 8.600000381469727, + 8.576000213623047, + 8.543999671936035, + 8.520000457763672, + 8.51200008392334, + 8.496000289916992, + 8.496000289916992, + 8.472000122070312, + 8.456000328063965, + 8.4399995803833, + 8.4399995803833, + 8.423999786376953, + 8.423999786376953, + 8.383999824523926, + 8.37600040435791, + 8.37600040435791, + 8.368000030517578, + 8.35200023651123, + 8.35200023651123, + 8.35200023651123, + 8.312000274658203, + 8.248000144958496, + 8.223999977111816, + 8.288000106811523, + 8.288000106811523, + 8.4399995803833, + 8.607999801635742, + 8.776000022888184, + 8.895999908447266, + 9.064000129699707, + 9.064000129699707, + 9.168000221252441, + 11.84000015258789, + 12.175999641418457, + 12.312000274658203, + 12.656000137329102, + 12.656000137329102, + 12.767999649047852, + 13.208000183105469, + 13.35200023651123, + 13.656000137329102, + 13.64799976348877, + 13.64799976348877, + 13.527999877929688, + 14.015999794006348, + 13.960000038146973, + 14.119999885559082, + 14.255999565124512, + 14.255999565124512, + 13.784000396728516, + 15.407999992370605, + 15.496000289916992, + 15.53600025177002, + 15.423999786376953, + 15.423999786376953, + 15.37600040435791, + 15.416000366210938, + 15.423999786376953, + 15.383999824523926, + 15.343999862670898, + 15.343999862670898, + 15.368000030517578, + 15.32800006866455, + 15.255999565124512, + 15.272000312805176, + 15.272000312805176, + 15.272000312805176, + 15.215999603271484, + 15.272000312805176, + 15.295999526977539, + 15.208000183105469, + 15.215999603271484, + 15.215999603271484, + 15.208000183105469, + 15.208000183105469, + 15.192000389099121, + 15.168000221252441, + 15.168000221252441, + 16.719999313354492, + 16.719999313354492, + 16.639999389648438, + 16.6560001373291, + 16.719999313354492, + 16.719999313354492, + 16.639999389648438, + 16.672000885009766, + 16.687999725341797, + 16.54400062561035, + 16.576000213623047, + 16.6560001373291, + 16.6560001373291, + 16.27199935913086, + 16.583999633789062, + 16.079999923706055, + 16.399999618530273, + 16.736000061035156, + 16.736000061035156, + 16.143999099731445, + 250, + 20.304000854492188, + 20.304000854492188, + 20.304000854492188, + 20.304000854492188, + 250, + 250, + 250, + 20.20800018310547, + 20.20800018310547, + 20.20800018310547, + 20.20800018310547, + 250, + 250, + 250, + 250, + 250, + 250}; + +std::vector target_scan_1 = {2.03169,1.9677,2.02369,2.01969,1.9517,1.93171,1.93171,1.89171,1.92771,1.92771,1.89171,1.85172,1.83172,1.85972,1.85172,1.79173,1.55576,1.54376,1.53577,1.49577,1.49577,1.49577,1.45178,1.47977,1.47977,1.45578,1.59576,250,250,250,1.59576,1.59576,1.59576,1.59576,1.54776,1.55176,1.53577,1.53577,1.51977,1.50377,1.49577,1.48377,1.47977,1.44378,1.47178,1.47578,1.44378,1.42378,1.41978,1.42778,1.42378,1.41179,1.3198,1.26781,1.22781,1.24781,1.21981,1.20782,1.23581,1.19582,1.22781,1.19982,1.17982,1.20382,1.17582,1.17982,1.16782,1.16382,1.15582,1.15182,1.15982,1.14783,1.13183,1.14783,1.12383,1.15182,1.14383,1.13583,1.24781,1.24781,1.19582,1.2958,1.3118,1.3038,1.2958,1.2998,1.2998,1.3038,1.2978,1.2858,1.2818,1.27581,1.2838,1.2878,1.27981,1.3078,1.2898,1.27781,1.2878,1.27981,1.3018,1.2998,1.2938,1.2958,250,250,1.19582,1.21182,1.20382,1.18382,1.23581,1.19582,250,250,250,1.9777,1.9837,1.9777,1.9857,1.9917,2.09168,250,4.49132,4.48732,4.48332,4.48532,4.47732,4.47132,4.48532,4.47332,4.47532,4.46332,4.47332,4.47132,4.46332,4.49931,4.96124,5.04323,6.571,6.567,6.579,6.579,6.64299,7.55685,7.52685,7.56485,7.93079,250,250,8.70467,8.69668,8.69268,8.72067,8.72067,8.72067,8.73267,8.73667,8.76467,8.76267,8.76467,5.01924,5.01524,5.02723,5.02723,5.03123,5.05523,5.04323,5.07123,5.07123,5.07123,5.09722,5.01924,4.92325,4.68529,4.64729,4.66929,4.66529,4.68929,4.68729,4.70128,4.71728,4.70928,4.73928,4.74928,4.75128,4.76927,4.76327,4.79527,4.79527,4.80927,4.82926,4.83526,4.85326,4.86926,4.86726,4.88926,4.90725,4.92325,4.94325,4.95125,4.97124,5.00524,3.13952,3.11753,3.06953,3.03754,2.99154,2.96555,2.91156,2.87756,2.83757,2.22366,2.18967,2.16167,2.15367,2.15167,2.14167,2.17367,2.16767,2.16367,2.18567,2.32765,2.37964,2.44363,2.43163,2.41763,2.38564,2.37364,2.34164,2.30165,2.27565,2.26965,2.30565,2.31765,2.33764,2.34364,2.36164,2.37364,2.38364,2.40563,2.41163,2.41963,2.44163,2.45363,2.47362,2.48362,2.49162,2.51162,2.52961,2.55961,2.56561,2.57361,2.5996,2.6136,2.6396,2.6576,2.66959,2.69559,250,2.71959,7.15891,7.17491,7.2469,7.27889,7.37888,9.08662,9.06062,8.99063,8.90664,8.84865,8.76266,8.68668,8.67868,8.66668,8.66268,8.73067,8.81066,8.91064,9.01063,9.11461,9.2106,9.28259,9.43456,9.49056,9.62653,9.73252,9.8625,9.96848,10.0825,10.2244,10.3484,10.4724,10.6244,10.7524,10.9003,11.0623,11.2203,11.4623,11.6362,11.8122,11.9922,3.79342,3.78142,3.77343,3.76743,3.75143,3.73943,3.73743,3.71743,3.71143,3.70544,3.69144,3.68544,3.67344,3.67544,3.66144,3.63945,3.64145,3.62945,3.63345,3.61745,3.61345,3.61145,3.60345,3.60345,3.58545,3.58545,3.57746,3.55746,3.56946,3.56946,3.54746,3.55546,3.53346,3.54146,3.53346,3.52746,3.52146,3.50947,3.51547,3.50547,3.48947,3.50347,3.49947,3.50147,3.48547,3.47747,3.49147,3.47947,1.90571,1.90571,2.38764,2.37764,2.36764,3.47747,3.47747,3.45947,3.46147,3.46547,3.47747,3.45747,3.46747,3.48147,3.46747,3.47947,3.48147,3.46147,3.47947,3.47347,3.48147,3.47347,3.47147,2.38764,2.36364,1.90171,1.88771,1.88771,3.48547,3.59145,5.19121,4.08338,3.88341,3.69544,3.58345,4.18136,4.17536,3.2915,2.89156,2.83157,2.77958,2.79157,2.82357,2.81957,2.82557,2.84357,250,250,1.9677,2.0037,250,250,2.87956,2.86156,2.85956,2.87756,2.87356,2.89956,2.89956,2.89356,2.91356,2.90756,2.92955,4.41933,4.42333,4.44332,4.44332,4.47132,4.47532,1.43178,1.42778,1.37579,1.35979,1.3418,1.3298,1.3098,1.2858,1.2938,1.2818,1.25581,1.34779,1.44978,1.46778,1.47178,1.48177,1.49377,1.49177,1.51777,1.51377,1.49977,1.51977,1.52377,1.54376,1.54976,1.54376,1.55976,1.55976,1.57176,1.57576,1.57576,1.59176,1.59376,1.60975,1.61575,1.61975,1.62775,1.63775,1.65375,1.65175,1.66575,1.68174,1.68174,1.69774,1.70574,1.70574,1.73174,1.72774,1.75173,1.74973,1.76573,1.77573,1.78573,1.80573,1.81372,1.81372,1.83372,1.84572,1.86772,1.87571,1.88771,1.91171,1.92571,1.93371,1.9597,1.9637,1.9917,1.9857,2.01969,2.03169,2.03569,2.05969,2.07168,2.09368,2.11768,2.13168,2.15967,2.16967,2.19967,2.21566,2.23166,2.25966,2.28365,2.27765,2.25766,2.23566,2.23166,2.21166,2.21966,2.20766,2.19767,2.21566,2.24766,4.41933,4.39133,4.38333,4.37533,4.35134,4.32334,4.30334,4.29935,4.28335,4.29135,4.25935,4.24335,4.21536,4.21136,4.19536,4.19536,4.17137,4.15137,4.15137,4.11537,4.12737,4.09538,3.79542,3.79142,3.87541,3.9314,4.03139,4.45332,4.44332,3.04754,3.03554,3.05353,3.08753,2.98755,2.98755,3.21951,3.2715,3.07753,3.03954,3.03554,2.85357,2.86556,2.91156,2.90956,3.01554,3.03754,3.01754,3.00754,2.89556,2.90156,2.88556,3.02154,3.37349,3.33949,2.95155,2.92156,2.91956,2.91156,2.91156,2.91956,2.95155,2.99954,3.2595,3.65944,4.18736,4.18336,4.19536,4.19536,2.79557,2.93355,2.97955,3.01954,3.03554,3.05554,3.08753,3.08753,3.12552,3.12552,3.12952,3.15352,3.13952,3.17352,3.69944,3.82142,3.89141,250,2.19167,2.17967,2.13567,2.08368,2.04369,2.04369,1.9837,1.9677,1.92371,1.69174,1.64975,1.63375,1.62775,1.62775,1.62375,1.63375,1.63775,1.64175,1.65775,1.67774,1.68774,1.90371,1.9477,1.9837,1.9797,1.9877,1.9757,1.93171,1.9557,1.9437,1.9637,1.89171,1.78173,1.76173,1.75173,1.76573,1.74573,1.73974,1.76373,1.75973,1.77973,1.77373,1.75373,1.79573,4.64729,4.6193,4.63129,4.63929,4.65529,4.67529,4.70328,4.71928,4.70328,4.33934,250,4.11537,4.09938,4.07538,4.01739,3.98539,3.98339,3.9614,3.97539,4.01539,1.70574,1.67774,1.65975,1.66575,1.64775,1.63975,1.62175,1.59976,1.65175,1.68974,1.69974,1.69774,1.63975,1.64575,1.63175,1.63575,1.64975,1.62775,1.63975,1.64575,1.64375,1.65375,1.64975,1.65175,1.65575,1.65775,1.67574,1.67175,1.68974,1.68974,1.69974,1.70574,1.71774,1.72974,1.73574,1.76173,1.76973,1.78373,1.81972,1.89171,250,250,250,250,5.05923,5.16521,5.15122,5.10922,5.09722,5.05923,5.03123,5.00924,4.97724,4.95525,4.92925,4.90325,4.89126,4.85326,4.85126,4.81727,4.52731,4.51531,4.49132,4.48332,4.45932,4.43532,4.42333,4.39533,4.39333,4.37533,4.35734,4.33134,4.31334,4.30534,4.28735,4.32134,4.38733,4.43732,4.47332,4.44732,4.43332,4.42533,4.26935,3.82742,3.81542,3.79942,3.80142,3.77942,3.77543,3.76943,3.74143,3.73943,3.72143,3.71543,3.69544,3.67544,3.65944,3.64345,3.63945,3.65744,3.63945,3.63345,3.61545,3.62345,3.60345,3.60545,3.58145,3.57146,2.75158,2.77758,2.77158,2.74758,2.71159,2.70559,2.68359,2.69559,2.71359,2.73358,2.75958,2.75758,2.81357,2.82957,3.00354,3.18152,3.2755,3.2735,3.2635,3.2615,3.2595,3.23951,3.24951,3.24351,3.23151,3.23951,3.22351,2.99754,2.98355,2.98555,2.99154,2.97555,2.99954,2.99154,2.98155,2.99354,2.97155,2.97755,2.97355,2.97755,2.98355,2.98155,2.98954,2.98755,2.97555,2.98555,2.96555,2.99154,4.01739,4.02139,4.02339,3.72943,3.57946,3.44348,3.2695,3.2555,3.2695,3.2755,3.2695,3.2735,3.2915,3.2815,3.3015,3.3075,3.3095,3.31949,3.3155,3.33749,3.32749,3.32749,4.04538,4.04738,4.12537,3.9674,3.82942,3.69744,3.51147,3.46547,3.48147,3.41148,3.46947,4.21136,4.21936,4.21936,4.23136,4.24935,4.25135,4.26135,3.56346,3.50347,3.45147,3.40548,3.40348,3.38348,3.38748,3.38149,3.37749,3.39948,3.39948,3.43748,3.47347,3.49947,3.59145,3.85141,3.84541,4.47132,4.52331,4.54331,3.9334,4.5813,4.17736,4.5953,4.62929,2.25366,2.19966,2.18567,2.17967,2.19967,2.23166,250,3.41948,2.06369,2.09168,2.07168,2.06369,2.07168,1.9917,1.9917}; +std::vector reference_scan_1 = {3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144}; \ No newline at end of file diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 378ec13..c711ab9 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -1,9 +1,12 @@ #include "../src/loop_closure_base.h" #include "../src/loop_closure_falko.h" -#include "testData2.h" +// #include "testData2.h" +#include "data/scan_data.h" #include "gtest/utils_gtest.h" +#include "matplotlibcpp.h" using namespace laserscanutils; +namespace plt = matplotlibcpp; TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { @@ -19,8 +22,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) scan2.ranges_raw_.push_back(testRanges2[i]); } - ParameterLoopClosureFalko param; - param.matcher_distance_th_= 0.3; + ParameterLoopClosureFalko param; + param.matcher_distance_th_ = 0.3; LoopClosureFalko loop_cl_falko(param); // Test convert2LaserScanFALKO @@ -37,7 +40,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedDescriptors, 18); - // Test matcheScene + // Test matchScene auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); ASSERT_EQ(new_match->keypoints_number_match, 18); @@ -52,7 +55,212 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ASSERT_EQ(best_match->match, true); ASSERT_EQ(best_match->scene_1, new_scene); ASSERT_EQ(best_match->scene_2, new_scene); - ASSERT_EQ(best_match->score, 1); + ASSERT_EQ(best_match->score, 1.8); +} + +TEST(loop_closure_falko, TestDescriptorsRotation) +{ + // Initialization + int scan_size = 1440; + LaserScan scan_1, scan2, scan_2; + LaserScanParams laser_params; + + laser_params.angle_min_ = 0; + laser_params.angle_max_ = 2.0 * M_PI; + laser_params.angle_step_ = laser_params.angle_max_ / 1440; + + for (int i = 0; i < scan_size; i++) + { + scan_1.ranges_raw_.push_back(testRanges1[i]); + scan_2.ranges_raw_.push_back(testRanges1[i]); + scan2.ranges_raw_.push_back(testRanges2[i]); + } + // Rotate scans + int rot = 800; + std::rotate(scan_2.ranges_raw_.begin(), scan_2.ranges_raw_.begin() + rot, scan_2.ranges_raw_.end()); + + ParameterLoopClosureFalko param; + param.matcher_distance_th_ = 0.3; + LoopClosureFalko loop_cl_falko(param); + + // Test extractScene + auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_1, laser_params)); + auto new_scene_2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_2, laser_params)); + auto key_1 = new_scene->keypoints_list_; + auto key_2 = new_scene_2->keypoints_list_; + auto desc_1 = new_scene->descriptors_list_rotated; + auto desc_2 = new_scene_2->descriptors_list_rotated; + + int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber; + int circularSectorNumber = new_scene->descriptors_list_[0].circularSectorNumber; + + int acum_distance = 0; + for (int i = 0; i < desc_1.size(); i++) + { + for (int j = 0; j < desc_2.size(); j++) + { + if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or + key_1[i].index == key_2[j].index - 1440 + rot) + { + + std::cout << "pair : " << i << " , " << j + << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl; + + acum_distance += desc_1[i].distance(desc_2[j]); + + // grid to x and y + std::vector x_pos, x_pos_rotated; + std::vector y_pos, y_pos_rotated; + + int desc_1_number = i; + int desc_2_number = j; + auto grid_1 = new_scene->descriptors_list_rotated[i].grid; + auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid; + + for (int i = 0; i < radialRingNumber; i++) + for (int j = 0; j < circularSectorNumber; j++) + { + if (grid_1[i][j] > 0) + { + x_pos.push_back(i); + y_pos.push_back(j); + } + + if (grid_2[i][j] > 0) + { + x_pos_rotated.push_back(i); + y_pos_rotated.push_back(j); + } + } + + // Plotting descriptors + plt::title("NNMatcher BSC only keypoints"); + plt::subplot(2, 1, 1); + plt::xlabel(" descriprtor not rotated"); + plt::plot(x_pos, y_pos, "ob"); + + plt::subplot(2, 1, 2); + plt::xlabel(" descriprtor rotated"); + plt::plot(x_pos_rotated, y_pos_rotated, "or"); + // plt::show(); + } + } + } + // std::cout << "acum_distance : " << acum_distance << std::endl; + ASSERT_EQ(acum_distance, 39); +} + +TEST(loop_closure_falko, TestMatch) +{ + // Initialization + int scan_size = 1440; + LaserScan scan_1, scan_2, scan_3; + LaserScanParams laser_params; + + laser_params.angle_min_ = 0; + laser_params.angle_max_ = 2.0 * M_PI; + laser_params.angle_step_ = laser_params.angle_max_ / 1440; + + for (int i = 0; i < scan_size; i++) + { + scan_1.ranges_raw_.push_back(testRanges1[i]); + scan_2.ranges_raw_.push_back(testRanges1[i]); + scan_3.ranges_raw_.push_back(testRanges2[i]); + } + + // Rotate scans + int rot = 800; + std::rotate(scan_2.ranges_raw_.begin(), scan_2.ranges_raw_.begin() + rot, scan_2.ranges_raw_.end()); + + ParameterLoopClosureFalko param; + param.matcher_distance_th_ = 100; + param.matcher_ddesc_th_ = 5; + LoopClosureFalko loop_cl_falko(param); + + // Test extractScene + auto new_scene_1 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_1, laser_params)); + auto new_scene_2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_2, laser_params)); + auto new_scene_3 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_3, laser_params)); + + auto match_1_2 = loop_cl_falko.matchScene(new_scene_1, new_scene_2); + + std::vector> asso_1_2; + for (auto asso : match_1_2->associations) + if (asso.second != -1) + asso_1_2.push_back(asso); + + ASSERT_EQ(asso_1_2.size(), 13); + + std::cout << " n associations : " << asso_1_2.size() << std::endl; + + for (int i = 0; i < match_1_2->associations.size(); i++) + { + std::cout << "id first : " << match_1_2->associations[i].first << std::endl; + std::cout << "id second : " << match_1_2->associations[i].second << std::endl; + } + + // auto key_1 = new_scene->keypoints_list_; + // auto key_2 = new_scene_2->keypoints_list_; + // auto desc_1 = new_scene->descriptors_list_rotated; + // auto desc_2 = new_scene_2->descriptors_list_rotated; + + // int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber; + // int circularSectorNumber = new_scene->descriptors_list_[0].circularSectorNumber; + + // int acum_distance = 0; + // for (int i = 0; i < desc_1.size(); i++) + // { + // for (int j = 0; j < desc_2.size(); j++) + // { + // if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or + // key_1[i].index == key_2[j].index - 1440 + rot) + // { + + // std::cout << "pair : " << i << " , " << j + // << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl; + + // acum_distance += desc_1[i].distance(desc_2[j]); + + // // grid to x and y + // std::vector x_pos, x_pos_rotated; + // std::vector y_pos, y_pos_rotated; + + // int desc_1_number = i; + // int desc_2_number = j; + // auto grid_1 = new_scene->descriptors_list_rotated[i].grid; + // auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid; + + // for (int i = 0; i < radialRingNumber; i++) + // for (int j = 0; j < circularSectorNumber; j++) + // { + // if (grid_1[i][j] > 0) + // { + // x_pos.push_back(i); + // y_pos.push_back(j); + // } + + // if (grid_2[i][j] > 0) + // { + // x_pos_rotated.push_back(i); + // y_pos_rotated.push_back(j); + // } + // } + + // // Plotting descriptors + // plt::title("NNMatcher BSC only keypoints"); + // plt::subplot(2, 1, 1); + // plt::xlabel(" descriprtor not rotated"); + // plt::plot(x_pos, y_pos, "ob"); + + // plt::subplot(2, 1, 2); + // plt::xlabel(" descriprtor rotated"); + // plt::plot(x_pos_rotated, y_pos_rotated, "or"); + // // plt::show(); + // } + // } + // } + // // std::cout << "acum_distance : " << acum_distance << std::endl; + // ASSERT_EQ(acum_distance, 39); } int main(int argc, char **argv) diff --git a/test/matplotlibcpp.h b/test/matplotlibcpp.h new file mode 100644 index 0000000..23553ae --- /dev/null +++ b/test/matplotlibcpp.h @@ -0,0 +1,2555 @@ +#pragma once + +// Python headers must be included before any system headers, since +// they define _POSIX_C_SOURCE +#include + +#include +#include +#include +#include +#include +#include +#include +#include // requires c++11 support +#include + +#ifndef WITHOUT_NUMPY +# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +# include + +# ifdef WITH_OPENCV +# include +# endif // WITH_OPENCV + +/* + * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so + * define the ones we need here. + */ +# if CV_MAJOR_VERSION > 3 +# define CV_BGR2RGB cv::COLOR_BGR2RGB +# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA +# endif +#endif // WITHOUT_NUMPY + +#if PY_MAJOR_VERSION >= 3 +# define PyString_FromString PyUnicode_FromString +# define PyInt_FromLong PyLong_FromLong +# define PyString_FromString PyUnicode_FromString +#endif + + +namespace matplotlibcpp { +namespace detail { + +static std::string s_backend; + +struct _interpreter { + PyObject* s_python_function_arrow; + PyObject *s_python_function_show; + PyObject *s_python_function_close; + PyObject *s_python_function_draw; + PyObject *s_python_function_pause; + PyObject *s_python_function_save; + PyObject *s_python_function_figure; + PyObject *s_python_function_fignum_exists; + PyObject *s_python_function_plot; + PyObject *s_python_function_quiver; + PyObject* s_python_function_contour; + PyObject *s_python_function_semilogx; + PyObject *s_python_function_semilogy; + PyObject *s_python_function_loglog; + PyObject *s_python_function_fill; + PyObject *s_python_function_fill_between; + PyObject *s_python_function_hist; + PyObject *s_python_function_imshow; + PyObject *s_python_function_scatter; + PyObject *s_python_function_boxplot; + PyObject *s_python_function_subplot; + PyObject *s_python_function_subplot2grid; + PyObject *s_python_function_legend; + PyObject *s_python_function_xlim; + PyObject *s_python_function_ion; + PyObject *s_python_function_ginput; + PyObject *s_python_function_ylim; + PyObject *s_python_function_title; + PyObject *s_python_function_axis; + PyObject *s_python_function_axvline; + PyObject *s_python_function_axvspan; + PyObject *s_python_function_xlabel; + PyObject *s_python_function_ylabel; + PyObject *s_python_function_gca; + PyObject *s_python_function_xticks; + PyObject *s_python_function_yticks; + PyObject* s_python_function_margins; + PyObject *s_python_function_tick_params; + PyObject *s_python_function_grid; + PyObject* s_python_function_cla; + PyObject *s_python_function_clf; + PyObject *s_python_function_errorbar; + PyObject *s_python_function_annotate; + PyObject *s_python_function_tight_layout; + PyObject *s_python_colormap; + PyObject *s_python_empty_tuple; + PyObject *s_python_function_stem; + PyObject *s_python_function_xkcd; + PyObject *s_python_function_text; + PyObject *s_python_function_suptitle; + PyObject *s_python_function_bar; + PyObject *s_python_function_barh; + PyObject *s_python_function_colorbar; + PyObject *s_python_function_subplots_adjust; + + + /* For now, _interpreter is implemented as a singleton since its currently not possible to have + multiple independent embedded python interpreters without patching the python source code + or starting a separate process for each. [1] + Furthermore, many python objects expect that they are destructed in the same thread as they + were constructed. [2] So for advanced usage, a `kill()` function is provided so that library + users can manually ensure that the interpreter is constructed and destroyed within the + same thread. + + 1: http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program + 2: https://github.com/lava/matplotlib-cpp/pull/202#issue-436220256 + */ + + static _interpreter& get() { + return interkeeper(false); + } + + static _interpreter& kill() { + return interkeeper(true); + } + + // Stores the actual singleton object referenced by `get()` and `kill()`. + static _interpreter& interkeeper(bool should_kill) { + static _interpreter ctx; + if (should_kill) + ctx.~_interpreter(); + return ctx; + } + + PyObject* safe_import(PyObject* module, std::string fname) { + PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); + + if (!fn) + throw std::runtime_error(std::string("Couldn't find required function: ") + fname); + + if (!PyFunction_Check(fn)) + throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); + + return fn; + } + +private: + +#ifndef WITHOUT_NUMPY +# if PY_MAJOR_VERSION >= 3 + + void *import_numpy() { + import_array(); // initialize C-API + return NULL; + } + +# else + + void import_numpy() { + import_array(); // initialize C-API + } + +# endif +#endif + + _interpreter() { + + // optional but recommended +#if PY_MAJOR_VERSION >= 3 + wchar_t name[] = L"plotting"; +#else + char name[] = "plotting"; +#endif + Py_SetProgramName(name); + Py_Initialize(); + + wchar_t const *dummy_args[] = {L"Python", NULL}; // const is needed because literals must not be modified + wchar_t const **argv = dummy_args; + int argc = sizeof(dummy_args)/sizeof(dummy_args[0])-1; + char ** argm = (char **)(argv); + PySys_SetArgv(argc, argm); + +#ifndef WITHOUT_NUMPY + import_numpy(); // initialize numpy C-API +#endif + + PyObject* matplotlibname = PyString_FromString("matplotlib"); + PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); + PyObject* cmname = PyString_FromString("matplotlib.cm"); + PyObject* pylabname = PyString_FromString("pylab"); + if (!pyplotname || !pylabname || !matplotlibname || !cmname) { + throw std::runtime_error("couldnt create string"); + } + + PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); + if (!matplotlib) { + PyErr_Print(); + throw std::runtime_error("Error loading module matplotlib!"); + } + + // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, + // or matplotlib.backends is imported for the first time + if (!s_backend.empty()) { + PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); + } + + PyObject* pymod = PyImport_Import(pyplotname); + Py_DECREF(pyplotname); + if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } + + s_python_colormap = PyImport_Import(cmname); + Py_DECREF(cmname); + if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } + + PyObject* pylabmod = PyImport_Import(pylabname); + Py_DECREF(pylabname); + if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } + + s_python_function_arrow = safe_import(pymod, "arrow"); + s_python_function_show = safe_import(pymod, "show"); + s_python_function_close = safe_import(pymod, "close"); + s_python_function_draw = safe_import(pymod, "draw"); + s_python_function_pause = safe_import(pymod, "pause"); + s_python_function_figure = safe_import(pymod, "figure"); + s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); + s_python_function_plot = safe_import(pymod, "plot"); + s_python_function_quiver = safe_import(pymod, "quiver"); + s_python_function_contour = safe_import(pymod, "contour"); + s_python_function_semilogx = safe_import(pymod, "semilogx"); + s_python_function_semilogy = safe_import(pymod, "semilogy"); + s_python_function_loglog = safe_import(pymod, "loglog"); + s_python_function_fill = safe_import(pymod, "fill"); + s_python_function_fill_between = safe_import(pymod, "fill_between"); + s_python_function_hist = safe_import(pymod,"hist"); + s_python_function_scatter = safe_import(pymod,"scatter"); + s_python_function_boxplot = safe_import(pymod,"boxplot"); + s_python_function_subplot = safe_import(pymod, "subplot"); + s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); + s_python_function_legend = safe_import(pymod, "legend"); + s_python_function_ylim = safe_import(pymod, "ylim"); + s_python_function_title = safe_import(pymod, "title"); + s_python_function_axis = safe_import(pymod, "axis"); + s_python_function_axvline = safe_import(pymod, "axvline"); + s_python_function_axvspan = safe_import(pymod, "axvspan"); + s_python_function_xlabel = safe_import(pymod, "xlabel"); + s_python_function_ylabel = safe_import(pymod, "ylabel"); + s_python_function_gca = safe_import(pymod, "gca"); + s_python_function_xticks = safe_import(pymod, "xticks"); + s_python_function_yticks = safe_import(pymod, "yticks"); + s_python_function_margins = safe_import(pymod, "margins"); + s_python_function_tick_params = safe_import(pymod, "tick_params"); + s_python_function_grid = safe_import(pymod, "grid"); + s_python_function_xlim = safe_import(pymod, "xlim"); + s_python_function_ion = safe_import(pymod, "ion"); + s_python_function_ginput = safe_import(pymod, "ginput"); + s_python_function_save = safe_import(pylabmod, "savefig"); + s_python_function_annotate = safe_import(pymod,"annotate"); + s_python_function_cla = safe_import(pymod, "cla"); + s_python_function_clf = safe_import(pymod, "clf"); + s_python_function_errorbar = safe_import(pymod, "errorbar"); + s_python_function_tight_layout = safe_import(pymod, "tight_layout"); + s_python_function_stem = safe_import(pymod, "stem"); + s_python_function_xkcd = safe_import(pymod, "xkcd"); + s_python_function_text = safe_import(pymod, "text"); + s_python_function_suptitle = safe_import(pymod, "suptitle"); + s_python_function_bar = safe_import(pymod,"bar"); + s_python_function_barh = safe_import(pymod, "barh"); + s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); + s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); +#ifndef WITHOUT_NUMPY + s_python_function_imshow = safe_import(pymod, "imshow"); +#endif + s_python_empty_tuple = PyTuple_New(0); + } + + ~_interpreter() { + Py_Finalize(); + } +}; + +} // end namespace detail + +/// Select the backend +/// +/// **NOTE:** This must be called before the first plot command to have +/// any effect. +/// +/// Mainly useful to select the non-interactive 'Agg' backend when running +/// matplotlibcpp in headless mode, for example on a machine with no display. +/// +/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use +inline void backend(const std::string& name) +{ + detail::s_backend = name; +} + +inline bool annotate(std::string annotation, double x, double y) +{ + detail::_interpreter::get(); + + PyObject * xy = PyTuple_New(2); + PyObject * str = PyString_FromString(annotation.c_str()); + + PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); + PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "xy", xy); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +namespace detail { + +#ifndef WITHOUT_NUMPY +// Type selector for numpy array conversion +template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default +template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; + +// Sanity checks; comment them out or change the numpy type below if you're compiling on +// a platform where they don't apply +//static_assert(sizeof(long long) == 8); +//template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +//static_assert(sizeof(unsigned long long) == 8); +//template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; +// TODO: add int, long, etc. + +template +PyObject* get_array(const std::vector& v) +{ + npy_intp vsize = v.size(); + NPY_TYPES type = select_npy_type::type; + if (type == NPY_NOTYPE) { + size_t memsize = v.size()*sizeof(double); + double* dp = static_cast(::malloc(memsize)); + for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); + return varray; + } + + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); + return varray; +} + + +template +PyObject* get_2darray(const std::vector<::std::vector>& v) +{ + if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); + + npy_intp vsize[2] = {static_cast(v.size()), + static_cast(v[0].size())}; + + PyArrayObject *varray = + (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); + + double *vd_begin = static_cast(PyArray_DATA(varray)); + + for (const ::std::vector &v_row : v) { + if (v_row.size() != static_cast(vsize[1])) + throw std::runtime_error("Missmatched array size"); + std::copy(v_row.begin(), v_row.end(), vd_begin); + vd_begin += vsize[1]; + } + + return reinterpret_cast(varray); +} + +#else // fallback if we don't have numpy: copy every element of the given vector + +template +PyObject* get_array(const std::vector& v) +{ + PyObject* list = PyList_New(v.size()); + for(size_t i = 0; i < v.size(); ++i) { + PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); + } + return list; +} + +#endif // WITHOUT_NUMPY + +// sometimes, for labels and such, we need string arrays +inline PyObject * get_array(const std::vector& strings) +{ + PyObject* list = PyList_New(strings.size()); + for (std::size_t i = 0; i < strings.size(); ++i) { + PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); + } + return list; +} + +// not all matplotlib need 2d arrays, some prefer lists of lists +template +PyObject* get_listlist(const std::vector>& ll) +{ + PyObject* listlist = PyList_New(ll.size()); + for (std::size_t i = 0; i < ll.size(); ++i) { + PyList_SetItem(listlist, i, get_array(ll[i])); + } + return listlist; +} + +} // namespace detail + +/// Plot a line through the given x and y data points.. +/// +/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html +template +bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +// TODO - it should be possible to make this work by implementing +// a non-numpy alternative for `detail::get_2darray()`. +#ifndef WITHOUT_NUMPY +template +void plot_surface(const std::vector<::std::vector> &x, + const std::vector<::std::vector> &y, + const std::vector<::std::vector> &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // We lazily load the modules here the first time this function is called + // because I'm not sure that we can assume "matplotlib installed" implies + // "mpl_toolkits installed" on all platforms, and we don't want to require + // it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + // using numpy arrays + PyObject *xarray = detail::get_2darray(x); + PyObject *yarray = detail::get_2darray(y); + PyObject *zarray = detail::get_2darray(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); + PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); + + PyObject *python_colormap_coolwarm = PyObject_GetAttrString( + detail::_interpreter::get().s_python_colormap, "coolwarm"); + + PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); + if (!plot_surface) throw std::runtime_error("No surface"); + Py_INCREF(plot_surface); + PyObject *res = PyObject_Call(plot_surface, args, kwargs); + if (!res) throw std::runtime_error("failed surface"); + Py_DECREF(plot_surface); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} +#endif // WITHOUT_NUMPY + +template +void plot3(const std::vector &x, + const std::vector &y, + const std::vector &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + PyObject *zarray = detail::get_array(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); + if (!plot3) throw std::runtime_error("No 3D line plot"); + Py_INCREF(plot3); + PyObject *res = PyObject_Call(plot3, args, kwargs); + if (!res) throw std::runtime_error("Failed 3D line plot"); + Py_DECREF(plot3); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +template +bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_stem, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if (res) Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) +{ + assert(x.size() == y1.size()); + assert(x.size() == y2.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* y1array = detail::get_array(y1); + PyObject* y2array = detail::get_array(y2); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, y1array); + PyTuple_SetItem(args, 2, y2array); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", + const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { + PyObject* obj_x = PyFloat_FromDouble(x); + PyObject* obj_y = PyFloat_FromDouble(y); + PyObject* obj_end_x = PyFloat_FromDouble(end_x); + PyObject* obj_end_y = PyFloat_FromDouble(end_y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); + PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, obj_x); + PyTuple_SetItem(plot_args, 1, obj_y); + PyTuple_SetItem(plot_args, 2, obj_end_x); + PyTuple_SetItem(plot_args, 3, obj_end_y); + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool hist(const std::vector& y, long bins=10,std::string color="b", + double alpha=1.0, bool cumulative=false) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); + + PyObject* plot_args = PyTuple_New(1); + + PyTuple_SetItem(plot_args, 0, yarray); + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +#ifndef WITHOUT_NUMPY +namespace detail { + +inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) +{ + assert(type == NPY_UINT8 || type == NPY_FLOAT); + assert(colors == 1 || colors == 3 || colors == 4); + + detail::_interpreter::get(); + + // construct args + npy_intp dims[3] = { rows, columns, colors }; + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) + throw std::runtime_error("Call to imshow() failed"); + if (out) + *out = res; + else + Py_DECREF(res); +} + +} // namespace detail + +inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); +} + +inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); +} + +#ifdef WITH_OPENCV +void imshow(const cv::Mat &image, const std::map &keywords = {}) +{ + // Convert underlying type of matrix, if needed + cv::Mat image2; + NPY_TYPES npy_type = NPY_UINT8; + switch (image.type() & CV_MAT_DEPTH_MASK) { + case CV_8U: + image2 = image; + break; + case CV_32F: + image2 = image; + npy_type = NPY_FLOAT; + break; + default: + image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); + } + + // If color image, convert from BGR to RGB + switch (image2.channels()) { + case 3: + cv::cvtColor(image2, image2, CV_BGR2RGB); + break; + case 4: + cv::cvtColor(image2, image2, CV_BGRA2RGBA); + } + + detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); +} +#endif // WITH_OPENCV +#endif // WITHOUT_NUMPY + +template +bool scatter(const std::vector& x, + const std::vector& y, + const double s=1.0, // The marker size in points**2 + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector>& data, + const std::vector& labels = {}, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* listlist = detail::get_listlist(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, listlist); + + PyObject* kwargs = PyDict_New(); + + // kwargs needs the labels, if there are (the correct number of) labels + if (!labels.empty() && labels.size() == data.size()) { + PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); + } + + // take care of the remaining keywords + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector& data, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* vector = detail::get_array(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, vector); + + PyObject* kwargs = PyDict_New(); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & x, + const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject * xarray = detail::get_array(x); + PyObject * yarray = detail::get_array(y); + + PyObject * kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); + PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); + + for (std::map::const_iterator it = + keywords.begin(); + it != keywords.end(); + ++it) { + PyDict_SetItemString( + kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject * plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject * res = PyObject_Call( + detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + using T = typename std::remove_reference::type::value_type; + + detail::_interpreter::get(); + + std::vector x; + for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } + + return bar(x, y, ec, ls, lw, keywords); +} + + +template +bool barh(const std::vector &x, const std::vector &y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map &keywords = { }) { + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + + PyObject *kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); + PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); + + for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_barh, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); + + return res; +} + + +inline bool subplots_adjust(const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyFloat_FromDouble(it->second)); + } + + + PyObject* plot_args = PyTuple_New(0); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool contour(const std::vector& x, const std::vector& y, + const std::vector& z, + const std::map& keywords = {}) { + assert(x.size() == y.size() && x.size() == z.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + PyObject* zarray = get_array(z); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, zarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) +{ + assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* uarray = detail::get_array(u); + PyObject* warray = detail::get_array(w); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, uarray); + PyTuple_SetItem(plot_args, 3, warray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_stem, plot_args); + + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* yerrarray = detail::get_array(yerr); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyDict_SetItemString(kwargs, "yerr", yerrarray); + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if (res) + Py_DECREF(res); + else + throw std::runtime_error("Call to errorbar() failed."); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(2); + + PyTuple_SetItem(plot_args, 0, yarray); + PyTuple_SetItem(plot_args, 1, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for(size_t i=0; i +bool plot(const std::vector& y, const std::map& keywords) +{ + std::vector x(y.size()); + for(size_t i=0; i +bool stem(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; + return stem(x, y, format); +} + +template +void text(Numeric x, Numeric y, const std::string& s = "") +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); + PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); + if(!res) throw std::runtime_error("Call to text() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) +{ + if (mappable == NULL) + throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); + + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, mappable); + + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); + if(!res) throw std::runtime_error("Call to colorbar() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + + +inline long figure(long number = -1) +{ + detail::_interpreter::get(); + + PyObject *res; + if (number == -1) + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); + else { + assert(number > 0); + + // Make sure interpreter is initialised + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); + Py_DECREF(args); + } + + if(!res) throw std::runtime_error("Call to figure() failed."); + + PyObject* num = PyObject_GetAttrString(res, "number"); + if (!num) throw std::runtime_error("Could not get number attribute of figure object"); + const long figureNumber = PyLong_AsLong(num); + + Py_DECREF(num); + Py_DECREF(res); + + return figureNumber; +} + +inline bool fignum_exists(long number) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); + if(!res) throw std::runtime_error("Call to fignum_exists() failed."); + + bool ret = PyObject_IsTrue(res); + Py_DECREF(res); + Py_DECREF(args); + + return ret; +} + +inline void figure_size(size_t w, size_t h) +{ + detail::_interpreter::get(); + + const size_t dpi = 100; + PyObject* size = PyTuple_New(2); + PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); + PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "figsize", size); + PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if(!res) throw std::runtime_error("Call to figure_size() failed."); + Py_DECREF(res); +} + +inline void legend() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(res); +} + +inline void legend(const std::map& keywords) +{ + detail::_interpreter::get(); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(kwargs); + Py_DECREF(res); +} + +template +void ylim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template +void xlim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline double* xlim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(res); + return arr; +} + + +inline double* ylim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(res); + return arr; +} + +template +inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to xticks() failed"); + + Py_DECREF(res); +} + +template +inline void xticks(const std::vector &ticks, const std::map& keywords) +{ + xticks(ticks, {}, keywords); +} + +template +inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to yticks() failed"); + + Py_DECREF(res); +} + +template +inline void yticks(const std::vector &ticks, const std::map& keywords) +{ + yticks(ticks, {}, keywords); +} + +template inline void margins(Numeric margin) +{ + // construct positional args + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template inline void margins(Numeric margin_x, Numeric margin_y) +{ + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline void tick_params(const std::map& keywords, const std::string axis = "both") +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args; + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) throw std::runtime_error("Call to tick_params() failed"); + + Py_DECREF(res); +} + +inline void subplot(long nrows, long ncols, long plot_number) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); + if(!res) throw std::runtime_error("Call to subplot() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) +{ + detail::_interpreter::get(); + + PyObject* shape = PyTuple_New(2); + PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); + PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); + + PyObject* loc = PyTuple_New(2); + PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); + PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); + + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, shape); + PyTuple_SetItem(args, 1, loc); + PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); + PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); + if(!res) throw std::runtime_error("Call to subplot2grid() failed."); + + Py_DECREF(shape); + Py_DECREF(loc); + Py_DECREF(args); + Py_DECREF(res); +} + +inline void title(const std::string &titlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pytitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pysuptitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); + if(!res) throw std::runtime_error("Call to suptitle() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void axis(const std::string &axisstr) +{ + detail::_interpreter::get(); + + PyObject* str = PyString_FromString(axisstr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + // construct positional args + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + if (it->first == "linewidth" || it->first == "alpha") + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); + else + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void xlabel(const std::string &str, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); + if(!res) throw std::runtime_error("Call to xlabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void ylabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); + if(!res) throw std::runtime_error("Call to ylabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void set_zlabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *ax = + PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, + detail::_interpreter::get().s_python_empty_tuple); + if (!ax) throw std::runtime_error("Call to gca() failed."); + Py_INCREF(ax); + + PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); + if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); + Py_INCREF(zlabel); + + PyObject *res = PyObject_Call(zlabel, args, kwargs); + if (!res) throw std::runtime_error("Call to set_zlabel() failed."); + Py_DECREF(zlabel); + + Py_DECREF(ax); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +inline void grid(bool flag) +{ + detail::_interpreter::get(); + + PyObject* pyflag = flag ? Py_True : Py_False; + Py_INCREF(pyflag); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyflag); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); + if(!res) throw std::runtime_error("Call to grid() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void show(const bool block = true) +{ + detail::_interpreter::get(); + + PyObject* res; + if(block) + { + res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_show, + detail::_interpreter::get().s_python_empty_tuple); + } + else + { + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "block", Py_False); + res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); + Py_DECREF(kwargs); + } + + + if (!res) throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void close() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_close, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to close() failed."); + + Py_DECREF(res); +} + +inline void xkcd() { + detail::_interpreter::get(); + + PyObject* res; + PyObject *kwargs = PyDict_New(); + + res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if (!res) + throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void draw() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_draw, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to draw() failed."); + + Py_DECREF(res); +} + +template +inline void pause(Numeric interval) +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); + if(!res) throw std::runtime_error("Call to pause() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void save(const std::string& filename) +{ + detail::_interpreter::get(); + + PyObject* pyfilename = PyString_FromString(filename.c_str()); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyfilename); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args); + if (!res) throw std::runtime_error("Call to save() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void clf() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_clf, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to clf() failed."); + + Py_DECREF(res); +} + +inline void cla() { + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_cla, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) + throw std::runtime_error("Call to cla() failed."); + + Py_DECREF(res); +} + +inline void ion() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_ion, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to ion() failed."); + + Py_DECREF(res); +} + +inline std::vector> ginput(const int numClicks = 1, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_ginput, args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(args); + if (!res) throw std::runtime_error("Call to ginput() failed."); + + const size_t len = PyList_Size(res); + std::vector> out; + out.reserve(len); + for (size_t i = 0; i < len; i++) { + PyObject *current = PyList_GetItem(res, i); + std::array position; + position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); + position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); + out.push_back(position); + } + Py_DECREF(res); + + return out; +} + +// Actually, is there any reason not to call this automatically for every plot? +inline void tight_layout() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_tight_layout, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to tight_layout() failed."); + + Py_DECREF(res); +} + +// Support for variadic plot() and initializer lists: + +namespace detail { + +template +using is_function = typename std::is_function>>::type; + +template +struct is_callable_impl; + +template +struct is_callable_impl +{ + typedef is_function type; +}; // a non-object is callable iff it is a function + +template +struct is_callable_impl +{ + struct Fallback { void operator()(); }; + struct Derived : T, Fallback { }; + + template struct Check; + + template + static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match + + template + static std::false_type test( Check* ); + +public: + typedef decltype(test(nullptr)) type; + typedef decltype(&Fallback::operator()) dtype; + static constexpr bool value = type::value; +}; // an object is callable iff it defines operator() + +template +struct is_callable +{ + // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not + typedef typename is_callable_impl::value, T>::type type; +}; + +template +struct plot_impl { }; + +template<> +struct plot_impl +{ + template + bool operator()(const IterableX& x, const IterableY& y, const std::string& format) + { + detail::_interpreter::get(); + + // 2-phase lookup for distance, begin, end + using std::distance; + using std::begin; + using std::end; + + auto xs = distance(begin(x), end(x)); + auto ys = distance(begin(y), end(y)); + assert(xs == ys && "x and y data must have the same number of elements!"); + + PyObject* xlist = PyList_New(xs); + PyObject* ylist = PyList_New(ys); + PyObject* pystring = PyString_FromString(format.c_str()); + + auto itx = begin(x), ity = begin(y); + for(size_t i = 0; i < xs; ++i) { + PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); + PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); + } + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xlist); + PyTuple_SetItem(plot_args, 1, ylist); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } +}; + +template<> +struct plot_impl +{ + template + bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) + { + if(begin(ticks) == end(ticks)) return true; + + // We could use additional meta-programming to deduce the correct element type of y, + // but all values have to be convertible to double anyways + std::vector y; + for(auto x : ticks) y.push_back(f(x)); + return plot_impl()(ticks,y,format); + } +}; + +} // end namespace detail + +// recursion stop for the above +template +bool plot() { return true; } + +template +bool plot(const A& a, const B& b, const std::string& format, Args... args) +{ + return detail::plot_impl::type>()(a,b,format) && plot(args...); +} + +/* + * This group of plot() functions is needed to support initializer lists, i.e. calling + * plot( {1,2,3,4} ) + */ +inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { + return plot(x,y,format); +} + +inline bool plot(const std::vector& y, const std::string& format = "") { + return plot(y,format); +} + +inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { + return plot(x,y,keywords); +} + +/* + * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting + */ +class Plot +{ +public: + // default initialization with plot label, some data and format + template + Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* kwargs = PyDict_New(); + if(name != "") + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if(res) + { + line= PyList_GetItem(res, 0); + + if(line) + set_data_fct = PyObject_GetAttrString(line,"set_data"); + else + Py_DECREF(line); + Py_DECREF(res); + } + } + + // shorter initialization with name or format only + // basically calls line, = plot([], []) + Plot(const std::string& name = "", const std::string& format = "") + : Plot(name, std::vector(), std::vector(), format) {} + + template + bool update(const std::vector& x, const std::vector& y) { + assert(x.size() == y.size()); + if(set_data_fct) + { + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_CallObject(set_data_fct, plot_args); + if (res) Py_DECREF(res); + return res; + } + return false; + } + + // clears the plot but keep it available + bool clear() { + return update(std::vector(), std::vector()); + } + + // definitely remove this line + void remove() { + if(line) + { + auto remove_fct = PyObject_GetAttrString(line,"remove"); + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(remove_fct, args); + if (res) Py_DECREF(res); + } + decref(); + } + + ~Plot() { + decref(); + } +private: + + void decref() { + if(line) + Py_DECREF(line); + if(set_data_fct) + Py_DECREF(set_data_fct); + } + + + PyObject* line = nullptr; + PyObject* set_data_fct = nullptr; +}; + +} // end namespace matplotlibcpp -- GitLab From 97156f7903706b7548a108076511d99fcff26f5a Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 24 May 2021 12:55:48 +0200 Subject: [PATCH 088/100] deleted testData files --- test/testData2.cpp | 23 ----------------------- test/testData2.h | 28 ---------------------------- 2 files changed, 51 deletions(-) delete mode 100644 test/testData2.cpp delete mode 100644 test/testData2.h diff --git a/test/testData2.cpp b/test/testData2.cpp deleted file mode 100644 index 5cca59d..0000000 --- a/test/testData2.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/** - * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant - * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. - * - * This file is part of FALKOLib. - * - * FALKOLib is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * FALKOLib is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with FALKOLib. If not, see . - */ - - double testRanges1[]= {250, 250, 250, 250, 250, 17.263999938964844, 17.263999938964844, 17.263999938964844, 17.263999938964844, 13.552000045776367, 13.552000045776367, 13.552000045776367, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.95199966430664, 12.51200008392334, 11.567999839782715, 11.567999839782715, 10.720000267028809, 10.720000267028809, 10.720000267028809, 10.359999656677246, 10.336000442504883, 10.343999862670898, 10.35200023651123, 10.35200023651123, 10.32800006866455, 10.319999694824219, 10.383999824523926, 10.407999992370605, 10.432000160217285, 10.432000160217285, 10.447999954223633, 10.46399974822998, 10.46399974822998, 10.503999710083008, 10.503999710083008, 10.527999877929688, 10.592000007629395, 10.62399959564209, 8.368000030517578, 8.343999862670898, 8.343999862670898, 8.031999588012695, 7.831999778747559, 7.6479997634887695, 7.552000045776367, 7.415999889373779, 7.415999889373779, 7.263999938964844, 7.144000053405762, 7.0320000648498535, 6.920000076293945, 6.776000022888184, 6.776000022888184, 6.671999931335449, 6.576000213623047, 6.440000057220459, 6.328000068664551, 6.271999835968018, 6.271999835968018, 6.263999938964844, 6.271999835968018, 6.296000003814697, 6.320000171661377, 6.328000068664551, 6.328000068664551, 6.335999965667725, 6.320000171661377, 3.187999963760376, 3.187999963760376, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.495999813079834, 4.504000186920166, 4.504000186920166, 4.504000186920166, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.544000148773193, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.51200008392334, 4.599999904632568, 4.576000213623047, 4.495999813079834, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.455999851226807, 4.4079999923706055, 4.368000030517578, 4.320000171661377, 4.271999835968018, 4.271999835968018, 4.223999977111816, 4.184000015258789, 4.168000221252441, 4.159999847412109, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.184000015258789, 4.199999809265137, 4.199999809265137, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.223999977111816, 4.23199987411499, 4.23199987411499, 4.176000118255615, 250, 250, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.111999988555908, 4.099999904632568, 4.099999904632568, 4.099999904632568, 4.079999923706055, 4.0320000648498535, 3.98799991607666, 3.9719998836517334, 3.9719998836517334, 3.9560000896453857, 3.9240000247955322, 3.888000011444092, 3.859999895095825, 3.8320000171661377, 3.8320000171661377, 3.803999900817871, 3.7880001068115234, 3.259999990463257, 3.240000009536743, 3.2200000286102295, 3.2200000286102295, 3.200000047683716, 3.1760001182556152, 3.1559998989105225, 3.1440000534057617, 3.128000020980835, 3.128000020980835, 3.119999885559082, 2.5959999561309814, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.4839999675750732, 2.496000051498413, 2.5320000648498535, 250, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 250, 250, 250, 250, 250, 250, 250, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.6679999828338623, 2.640000104904175, 2.640000104904175, 2.640000104904175, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.555999994277954, 2.5439999103546143, 2.5320000648498535, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.503999948501587, 2.492000102996826, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4600000381469727, 2.4519999027252197, 2.440000057220459, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4079999923706055, 2.427999973297119, 2.444000005722046, 2.444000005722046, 2.4600000381469727, 2.4719998836517334, 2.48799991607666, 2.5, 2.5160000324249268, 2.5160000324249268, 2.5280001163482666, 2.5399999618530273, 2.559999942779541, 2.5799999237060547, 2.5840001106262207, 2.5840001106262207, 3.0480000972747803, 3.0480000972747803, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5959999561309814, 2.5920000076293945, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.572000026702881, 2.563999891281128, 2.555999994277954, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5399999618530273, 2.5360000133514404, 2.5320000648498535, 2.5239999294281006, 2.5199999809265137, 2.507999897003174, 2.507999897003174, 2.503999948501587, 2.5, 2.496000051498413, 2.492000102996826, 2.48799991607666, 2.48799991607666, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4639999866485596, 2.4560000896453857, 2.4560000896453857, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.444000005722046, 2.436000108718872, 2.436000108718872, 2.431999921798706, 2.427999973297119, 2.4240000247955322, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4079999923706055, 2.4040000438690186, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.3559999465942383, 2.3559999465942383, 2.3559999465942383, 2.6600000858306885, 2.6600000858306885, 2.6600000858306885, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.0, 2.0, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 2.0, 2.0, 2.009999990463257, 2.0220000743865967, 2.0460000038146973, 2.0460000038146973, 2.0260000228881836, 2.0260000228881836, 2.00600004196167, 1.996000051498413, 1.99399995803833, 1.9900000095367432, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.9980000257492065, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 1.9980000257492065, 2.0, 2.0, 1.9980000257492065, 1.9980000257492065, 1.9919999837875366, 1.9520000219345093, 2.0320000648498535, 2.0999999046325684, 2.1040000915527344, 2.1040000915527344, 2.0280001163482666, 1.9040000438690186, 1.9279999732971191, 1.9520000219345093, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9600000381469727, 1.9620000123977661, 1.9639999866485596, 1.9639999866485596, 1.965999960899353, 1.965999960899353, 1.968000054359436, 1.972000002861023, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9759999513626099, 1.9800000190734863, 1.9839999675750732, 1.9800000190734863, 1.9800000190734863, 1.9819999933242798, 1.9880000352859497, 1.996000051498413, 2.009999990463257, 2.00600004196167, 2.00600004196167, 1.99399995803833, 2.687999963760376, 2.687999963760376, 2.687999963760376, 1.74399995803833, 1.74399995803833, 2.063999891281128, 2.115999937057495, 2.196000099182129, 2.191999912261963, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5199999809265137, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5399999618530273, 2.5480000972747803, 2.5480000972747803, 2.5480000972747803, 2.5399999618530273, 2.5439999103546143, 2.552000045776367, 2.555999994277954, 2.555999994277954, 2.552000045776367, 2.5480000972747803, 250, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3079999685287476, 1.2999999523162842, 1.2940000295639038, 1.2940000295639038, 1.2879999876022339, 1.2860000133514404, 1.2940000295639038, 1.3040000200271606, 1.3040000200271606, 1.3040000200271606, 1.2999999523162842, 1.2979999780654907, 1.3020000457763672, 1.309999942779541, 1.3140000104904175, 1.3140000104904175, 1.3140000104904175, 1.3240000009536743, 1.3359999656677246, 1.3420000076293945, 1.343999981880188, 1.343999981880188, 1.2200000286102295, 250, 250, 250, 250, 250, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.090000033378601, 1.090000033378601, 1.0920000076293945, 1.0980000495910645, 1.0980000495910645, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.100000023841858, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.1019999980926514, 1.1119999885559082, 1.121999979019165, 1.1260000467300415, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.791999816894531, 5.776000022888184, 5.776000022888184, 5.776000022888184, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.751999855041504, 5.736000061035156, 5.679999828338623, 5.671999931335449, 5.671999931335449, 5.631999969482422, 5.631999969482422, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.552000045776367, 5.559999942779541, 5.584000110626221, 6.104000091552734, 6.783999919891357, 6.831999778747559, 6.831999778747559, 6.831999778747559, 6.760000228881836, 6.616000175476074, 6.455999851226807, 6.343999862670898, 6.343999862670898, 6.320000171661377, 6.311999797821045, 9.192000389099121, 9.175999641418457, 9.15999984741211, 9.15999984741211, 9.128000259399414, 9.119999885559082, 7.992000102996826, 7.9120001792907715, 7.8480000495910645, 7.8480000495910645, 7.800000190734863, 7.74399995803833, 7.71999979019165, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.711999893188477, 7.71999979019165, 7.71999979019165, 7.711999893188477, 7.703999996185303, 7.703999996185303, 7.703999996185303, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.671999931335449, 8.67199993133545, 8.640000343322754, 8.607999801635742, 8.583999633789062, 8.583999633789062, 8.5600004196167, 8.552000045776367, 8.53600025177002, 8.520000457763672, 8.503999710083008, 8.503999710083008, 8.472000122070312, 8.447999954223633, 8.416000366210938, 8.383999824523926, 8.383999824523926, 8.383999824523926, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.336000442504883, 8.303999900817871, 8.303999900817871, 8.263999938964844, 8.239999771118164, 8.279999732971191, 8.392000198364258, 8.496000289916992, 8.496000289916992, 8.656000137329102, 8.807999610900879, 8.928000450134277, 9.015999794006348, 11.767999649047852, 11.767999649047852, 12.071999549865723, 12.175999641418457, 12.567999839782715, 12.663999557495117, 13.064000129699707, 13.064000129699707, 13.168000221252441, 13.607999801635742, 13.640000343322754, 13.53600025177002, 13.968000411987305, 13.968000411987305, 14.008000373840332, 14.104000091552734, 14.175999641418457, 14.104000091552734, 15.4399995803833, 15.4399995803833, 15.543999671936035, 15.48799991607666, 15.48799991607666, 15.503999710083008, 15.479999542236328, 15.479999542236328, 15.447999954223633, 15.399999618530273, 15.37600040435791, 15.35200023651123, 15.319999694824219, 15.319999694824219, 15.272000312805176, 15.272000312805176, 15.336000442504883, 15.303999900817871, 15.248000144958496, 15.248000144958496, 14.744000434875488, 15.303999900817871, 15.272000312805176, 15.248000144958496, 15.215999603271484, 15.223999977111816, 15.223999977111816, 15.248000144958496, 16.784000396728516, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.719999313354492, 16.672000885009766, 16.687999725341797, 16.719999313354492, 16.70400047302246, 16.607999801635742, 16.607999801635742, 16.624000549316406, 16.6560001373291, 16.23200035095215, 16.143999099731445, 16.06399917602539, 16.06399917602539, 16.016000747680664, 17.472000122070312, 19.007999420166016, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250}; - - double testRanges2[]={250, 250, 250, 250, 16.799999237060547, 16.799999237060547, 16.799999237060547, 16.799999237060547, 14.095999717712402, 14.095999717712402, 14.095999717712402, 14.095999717712402, 13.255999565124512, 13.255999565124512, 13.255999565124512, 12.767999649047852, 12.175999641418457, 12.175999641418457, 10.911999702453613, 10.4399995803833, 10.4399995803833, 10.4399995803833, 10.32800006866455, 10.32800006866455, 10.32800006866455, 10.343999862670898, 10.336000442504883, 10.312000274658203, 10.368000030517578, 10.368000030517578, 10.407999992370605, 10.472000122070312, 10.48799991607666, 10.520000457763672, 10.48799991607666, 10.48799991607666, 10.472000122070312, 10.503999710083008, 10.51200008392334, 10.51200008392334, 10.520000457763672, 10.520000457763672, 8.295999526977539, 8.13599967956543, 7.984000205993652, 7.791999816894531, 7.624000072479248, 7.624000072479248, 7.5279998779296875, 7.4079999923706055, 7.263999938964844, 7.119999885559082, 7.007999897003174, 7.007999897003174, 6.9120001792907715, 6.760000228881836, 6.639999866485596, 6.544000148773193, 6.415999889373779, 6.415999889373779, 6.311999797821045, 6.271999835968018, 6.263999938964844, 6.288000106811523, 6.303999900817871, 6.303999900817871, 6.320000171661377, 6.335999965667725, 6.343999862670898, 6.335999965667725, 6.303999900817871, 6.303999900817871, 6.271999835968018, 4.5920000076293945, 250, 4.480000019073486, 4.480000019073486, 4.480000019073486, 4.480000019073486, 4.4720001220703125, 4.504000186920166, 4.519999980926514, 4.5279998779296875, 4.5279998779296875, 4.5279998779296875, 4.5279998779296875, 4.567999839782715, 4.559999942779541, 250, 4.48799991607666, 4.48799991607666, 4.48799991607666, 4.48799991607666, 4.440000057220459, 4.392000198364258, 4.360000133514404, 4.360000133514404, 4.320000171661377, 4.263999938964844, 4.223999977111816, 4.191999912261963, 4.168000221252441, 4.168000221252441, 4.168000221252441, 4.176000118255615, 4.176000118255615, 4.191999912261963, 4.208000183105469, 4.208000183105469, 4.208000183105469, 4.216000080108643, 4.216000080108643, 4.216000080108643, 4.23199987411499, 4.23199987411499, 4.223999977111816, 4.184000015258789, 250, 4.136000156402588, 4.136000156402588, 4.136000156402588, 4.136000156402588, 4.111999988555908, 4.111999988555908, 4.0960001945495605, 4.064000129699707, 4.015999794006348, 4.015999794006348, 3.9839999675750732, 3.9679999351501465, 3.947999954223633, 3.9119999408721924, 3.875999927520752, 3.875999927520752, 3.8480000495910645, 3.8239998817443848, 3.7039999961853027, 3.5920000076293945, 3.259999990463257, 3.259999990463257, 3.240000009536743, 3.2160000801086426, 3.196000099182129, 3.171999931335449, 3.1519999504089355, 3.1519999504089355, 3.140000104904175, 3.124000072479248, 3.115999937057495, 2.5959999561309814, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.51200008392334, 2.51200008392334, 2.51200008392334, 2.51200008392334, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.6040000915527344, 2.6040000915527344, 2.6040000915527344, 2.6040000915527344, 250, 250, 250, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 250, 250, 250, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 2.6480000019073486, 2.635999917984009, 2.628000020980835, 2.619999885559082, 2.619999885559082, 2.6080000400543213, 2.5959999561309814, 2.5880000591278076, 2.5759999752044678, 2.563999891281128, 2.563999891281128, 2.552000045776367, 2.5399999618530273, 2.5280001163482666, 2.5199999809265137, 2.51200008392334, 2.51200008392334, 2.5, 2.48799991607666, 2.4839999675750732, 2.4719998836517334, 2.4639999866485596, 2.4639999866485596, 2.4560000896453857, 2.444000005722046, 2.436000108718872, 2.431999921798706, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4159998893737793, 2.4200000762939453, 2.431999921798706, 2.444000005722046, 2.4600000381469727, 2.4600000381469727, 2.4760000705718994, 2.48799991607666, 2.5, 2.5160000324249268, 2.5360000133514404, 2.5360000133514404, 2.5480000972747803, 2.563999891281128, 2.5840001106262207, 2.7079999446868896, 2.684000015258789, 2.684000015258789, 2.5880000591278076, 2.5880000591278076, 2.5880000591278076, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.631999969482422, 2.624000072479248, 2.615999937057495, 2.611999988555908, 2.6040000915527344, 2.6040000915527344, 2.5920000076293945, 2.5880000591278076, 2.5840001106262207, 2.5759999752044678, 2.572000026702881, 2.559999942779541, 2.559999942779541, 2.552000045776367, 2.5480000972747803, 2.5439999103546143, 2.5399999618530273, 2.5320000648498535, 2.5320000648498535, 2.5280001163482666, 2.5239999294281006, 2.5160000324249268, 2.507999897003174, 2.503999948501587, 2.503999948501587, 2.5, 2.492000102996826, 2.492000102996826, 2.4839999675750732, 2.4800000190734863, 2.4800000190734863, 2.4760000705718994, 2.4719998836517334, 2.4679999351501465, 2.4600000381469727, 2.4600000381469727, 2.4519999027252197, 2.447999954223633, 2.447999954223633, 2.447999954223633, 2.440000057220459, 2.440000057220459, 2.431999921798706, 2.427999973297119, 2.427999973297119, 2.4240000247955322, 2.4200000762939453, 2.4200000762939453, 2.4159998893737793, 2.4119999408721924, 2.4079999923706055, 2.4040000438690186, 2.4000000953674316, 2.4000000953674316, 2.4000000953674316, 2.3959999084472656, 2.3919999599456787, 2.388000011444092, 2.384000062942505, 2.384000062942505, 2.384000062942505, 2.380000114440918, 2.375999927520752, 2.371999979019165, 2.371999979019165, 2.371999979019165, 2.368000030517578, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.364000082015991, 2.359999895095825, 2.371999979019165, 2.388000011444092, 2.388000011444092, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.002000093460083, 2.002000093460083, 2.002000093460083, 2.002000093460083, 2.002000093460083, 1.9980000257492065, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.9980000257492065, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.9919999837875366, 1.99399995803833, 1.9919999837875366, 1.9919999837875366, 1.9919999837875366, 1.996000051498413, 1.996000051498413, 2.0, 2.009999990463257, 2.009999990463257, 2.0320000648498535, 2.0480000972747803, 2.0320000648498535, 2.0179998874664307, 2.007999897003174, 2.007999897003174, 1.996000051498413, 1.9919999837875366, 1.9900000095367432, 1.9880000352859497, 1.99399995803833, 1.99399995803833, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.996000051498413, 1.99399995803833, 1.99399995803833, 1.9980000257492065, 1.9980000257492065, 2.0, 2.002000093460083, 2.0, 2.002000093460083, 1.9759999513626099, 1.9759999513626099, 1.9359999895095825, 2.0360000133514404, 2.0859999656677246, 2.0439999103546143, 1.9819999933242798, 1.9819999933242798, 1.9279999732971191, 1.9559999704360962, 1.9559999704360962, 1.9600000381469727, 1.9579999446868896, 1.9559999704360962, 1.9559999704360962, 1.9579999446868896, 1.9620000123977661, 1.9639999866485596, 1.968000054359436, 1.9700000286102295, 1.9700000286102295, 1.972000002861023, 1.9739999771118164, 1.9739999771118164, 1.9739999771118164, 1.9780000448226929, 1.9780000448226929, 1.9839999675750732, 1.9859999418258667, 1.9859999418258667, 1.9900000095367432, 1.99399995803833, 1.99399995803833, 1.996000051498413, 1.99399995803833, 1.9980000257492065, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.003999948501587, 2.5999999046325684, 1.6720000505447388, 2.0480000972747803, 2.115999937057495, 2.115999937057495, 2.191999912261963, 2.196000099182129, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5160000324249268, 2.5239999294281006, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5280001163482666, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5239999294281006, 2.5239999294281006, 2.5280001163482666, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5320000648498535, 2.5360000133514404, 2.5360000133514404, 2.5360000133514404, 2.5360000133514404, 2.5399999618530273, 2.5439999103546143, 2.5439999103546143, 2.5439999103546143, 2.5439999103546143, 2.5439999103546143, 2.5480000972747803, 2.555999994277954, 2.559999942779541, 1.315999984741211, 1.315999984741211, 1.315999984741211, 1.315999984741211, 1.3040000200271606, 1.3040000200271606, 1.2979999780654907, 1.2920000553131104, 1.2920000553131104, 1.2860000133514404, 1.2899999618530273, 1.2999999523162842, 1.3020000457763672, 1.3020000457763672, 1.3020000457763672, 1.2999999523162842, 1.2960000038146973, 1.3020000457763672, 1.312000036239624, 1.315999984741211, 1.315999984741211, 1.3179999589920044, 1.3279999494552612, 1.3359999656677246, 1.3420000076293945, 1.2200000286102295, 1.2200000286102295, 250, 250, 250, 250, 250, 1.090000033378601, 1.090000033378601, 1.090000033378601, 1.090000033378601, 1.0880000591278076, 1.0880000591278076, 1.0880000591278076, 1.0920000076293945, 1.0959999561309814, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0980000495910645, 1.0959999561309814, 1.0959999561309814, 1.0959999561309814, 1.0959999561309814, 1.093999981880188, 1.093999981880188, 1.093999981880188, 1.0920000076293945, 1.0920000076293945, 1.0920000076293945, 1.0920000076293945, 1.093999981880188, 1.1019999980926514, 1.1139999628067017, 1.1239999532699585, 1.1239999532699585, 1.1299999952316284, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.888000011444092, 5.888000011444092, 5.888000011444092, 5.888000011444092, 5.863999843597412, 5.863999843597412, 5.808000087738037, 5.751999855041504, 5.751999855041504, 5.74399995803833, 5.74399995803833, 5.656000137329102, 5.656000137329102, 5.656000137329102, 5.584000110626221, 5.584000110626221, 5.584000110626221, 5.584000110626221, 5.552000045776367, 5.559999942779541, 5.599999904632568, 5.599999904632568, 6.71999979019165, 6.783999919891357, 6.831999778747559, 6.815999984741211, 6.696000099182129, 6.552000045776367, 6.552000045776367, 6.423999786376953, 6.343999862670898, 6.328000068664551, 9.232000350952148, 9.192000389099121, 9.192000389099121, 9.175999641418457, 9.144000053405762, 9.039999961853027, 8.960000038146973, 7.943999767303467, 7.943999767303467, 7.863999843597412, 7.8480000495910645, 7.791999816894531, 7.751999855041504, 7.736000061035156, 7.736000061035156, 7.71999979019165, 7.728000164031982, 7.728000164031982, 7.711999893188477, 7.696000099182129, 7.696000099182129, 7.688000202178955, 7.696000099182129, 7.696000099182129, 7.688000202178955, 7.688000202178955, 7.688000202178955, 7.736000061035156, 7.800000190734863, 8.616000175476074, 8.600000381469727, 8.600000381469727, 8.576000213623047, 8.543999671936035, 8.520000457763672, 8.51200008392334, 8.496000289916992, 8.496000289916992, 8.472000122070312, 8.456000328063965, 8.4399995803833, 8.4399995803833, 8.423999786376953, 8.423999786376953, 8.383999824523926, 8.37600040435791, 8.37600040435791, 8.368000030517578, 8.35200023651123, 8.35200023651123, 8.35200023651123, 8.312000274658203, 8.248000144958496, 8.223999977111816, 8.288000106811523, 8.288000106811523, 8.4399995803833, 8.607999801635742, 8.776000022888184, 8.895999908447266, 9.064000129699707, 9.064000129699707, 9.168000221252441, 11.84000015258789, 12.175999641418457, 12.312000274658203, 12.656000137329102, 12.656000137329102, 12.767999649047852, 13.208000183105469, 13.35200023651123, 13.656000137329102, 13.64799976348877, 13.64799976348877, 13.527999877929688, 14.015999794006348, 13.960000038146973, 14.119999885559082, 14.255999565124512, 14.255999565124512, 13.784000396728516, 15.407999992370605, 15.496000289916992, 15.53600025177002, 15.423999786376953, 15.423999786376953, 15.37600040435791, 15.416000366210938, 15.423999786376953, 15.383999824523926, 15.343999862670898, 15.343999862670898, 15.368000030517578, 15.32800006866455, 15.255999565124512, 15.272000312805176, 15.272000312805176, 15.272000312805176, 15.215999603271484, 15.272000312805176, 15.295999526977539, 15.208000183105469, 15.215999603271484, 15.215999603271484, 15.208000183105469, 15.208000183105469, 15.192000389099121, 15.168000221252441, 15.168000221252441, 16.719999313354492, 16.719999313354492, 16.639999389648438, 16.6560001373291, 16.719999313354492, 16.719999313354492, 16.639999389648438, 16.672000885009766, 16.687999725341797, 16.54400062561035, 16.576000213623047, 16.6560001373291, 16.6560001373291, 16.27199935913086, 16.583999633789062, 16.079999923706055, 16.399999618530273, 16.736000061035156, 16.736000061035156, 16.143999099731445, 250, 20.304000854492188, 20.304000854492188, 20.304000854492188, 20.304000854492188, 250, 250, 250, 20.20800018310547, 20.20800018310547, 20.20800018310547, 20.20800018310547, 250, 250, 250, 250, 250, 250}; diff --git a/test/testData2.h b/test/testData2.h deleted file mode 100644 index d0e07f0..0000000 --- a/test/testData2.h +++ /dev/null @@ -1,28 +0,0 @@ -/** - * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant - * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. - * - * This file is part of FALKOLib. - * - * FALKOLib is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * FALKOLib is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with FALKOLib. If not, see . - */ -#pragma once - -extern double testRanges1[]; - -extern double testRanges2[]; - -extern double testRangesOrtho1[]; - -extern double testRangesOrtho2[]; -- GitLab From 386c6f8f637f02998f49e78a3a852fbd392a8d0c Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 31 May 2021 16:55:04 +0200 Subject: [PATCH 089/100] wip --- deps/falkolib | 2 +- src/loop_closure_falko.h | 19 ++- test/gtest_loop_closure_falko.cpp | 243 +++++++++++++++++------------- 3 files changed, 148 insertions(+), 116 deletions(-) diff --git a/deps/falkolib b/deps/falkolib index 0017c6c..8f341e9 160000 --- a/deps/falkolib +++ b/deps/falkolib @@ -1 +1 @@ -Subproject commit 0017c6c5be963b923684bfd8eb6bd8b7641ba1b6 +Subproject commit 8f341e9f1739c445cd8352586d514e2e49e25718 diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 453cdbe..c4f94ca 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -190,15 +190,20 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract { mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; - // new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); - // std::cout << "angle_min :" << angle_min << std::endl; - // std::cout << "angle_min :" << angle_step << std::endl; - // std::cout << "new_scene->angle_rotation_[i] :" << angle_rotation[i] << std::endl; - descriptors_list_rotated[i].rotate(angle_rotation[i]); + // descriptors_list_rotated[i].rotate(angle_rotation[i]); + // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i]; + double orientation = angle_rotation[i]; + // if (orientation < 0) + // { + // orientation = 2 * M_PI + orientation; + // } + descriptors_list_rotated[i].rotate(orientation); + // std::cout << "orientation : " << angle_rotation[i] << std::endl; + // std::cout << "orientation desc : " << orientation << std::endl; new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]); } - + new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); // Compute Scene Area and Perimeter int n = 3; @@ -371,7 +376,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), // scene_2_falko->keypoints_list_.size()); - new_match->score = (double)matching_number / 10.0; + new_match->score = (double)matching_number; new_match->transform_vector.head(2) = new_match->transform.translation(); new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index c711ab9..878ac4b 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -32,7 +32,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ASSERT_EQ(firstPoint, 250); - // Test extractScene + // Test extractScene2 auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, laser_params)); auto new_scene2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); int detectedKeypoints = new_scene->keypoints_list_.size(); @@ -94,59 +94,80 @@ TEST(loop_closure_falko, TestDescriptorsRotation) int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber; int circularSectorNumber = new_scene->descriptors_list_[0].circularSectorNumber; - int acum_distance = 0; + int acum_distance = 0; + double asso_dist[key_1.size()][key_2.size()]; + double min_dist_vector[key_1.size()]; for (int i = 0; i < desc_1.size(); i++) { + double min_dist = 1000; + int asso_number; for (int j = 0; j < desc_2.size(); j++) { - if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or - key_1[i].index == key_2[j].index - 1440 + rot) - { - std::cout << "pair : " << i << " , " << j - << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl; - - acum_distance += desc_1[i].distance(desc_2[j]); - - // grid to x and y - std::vector x_pos, x_pos_rotated; - std::vector y_pos, y_pos_rotated; - - int desc_1_number = i; - int desc_2_number = j; - auto grid_1 = new_scene->descriptors_list_rotated[i].grid; - auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid; - - for (int i = 0; i < radialRingNumber; i++) - for (int j = 0; j < circularSectorNumber; j++) - { - if (grid_1[i][j] > 0) - { - x_pos.push_back(i); - y_pos.push_back(j); - } - - if (grid_2[i][j] > 0) - { - x_pos_rotated.push_back(i); - y_pos_rotated.push_back(j); - } - } - - // Plotting descriptors - plt::title("NNMatcher BSC only keypoints"); - plt::subplot(2, 1, 1); - plt::xlabel(" descriprtor not rotated"); - plt::plot(x_pos, y_pos, "ob"); - - plt::subplot(2, 1, 2); - plt::xlabel(" descriprtor rotated"); - plt::plot(x_pos_rotated, y_pos_rotated, "or"); - // plt::show(); + acum_distance += desc_1[i].distance(desc_2[j]); + asso_dist[i][j] = desc_1[i].distance(desc_2[j]); + if (asso_dist[i][j] < min_dist) + { + min_dist = asso_dist[i][j]; + asso_number = j; } + std::cout << "pair : " << i << " , " << j << " , distance : " << asso_dist[i][j] << std::endl; } + // std::cout << "pair : " << i << " , " << asso_number << " , distance : " << min_dist << std::endl; } - // std::cout << "acum_distance : " << acum_distance << std::endl; + + // for (int i = 0; i < desc_1.size(); i++) + // { + // for (int j = 0; j < desc_2.size(); j++) + // { + // if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or + // key_1[i].index == key_2[j].index - 1440 + rot) + // { + + // std::cout << "pair : " << i << " , " << j + // << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl; + + // acum_distance += desc_1[i].distance(desc_2[j]); + + // // grid to x and y + // std::vector x_pos, x_pos_rotated; + // std::vector y_pos, y_pos_rotated; + + // int desc_1_number = i; + // int desc_2_number = j; + // auto grid_1 = new_scene->descriptors_list_rotated[i].grid; + // auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid; + + // for (int i = 0; i < radialRingNumber; i++) + // for (int j = 0; j < circularSectorNumber; j++) + // { + // if (grid_1[i][j] > 0) + // { + // x_pos.push_back(i); + // y_pos.push_back(j); + // } + + // if (grid_2[i][j] > 0) + // { + // x_pos_rotated.push_back(i); + // y_pos_rotated.push_back(j); + // } + // } + + // // Plotting descriptors + // // plt::title("NNMatcher BSC only keypoints"); + // // plt::subplot(2, 1, 1); + // // plt::xlabel(" descriprtor not rotated"); + // // plt::plot(x_pos, y_pos, "ob"); + + // // plt::subplot(2, 1, 2); + // // plt::xlabel(" descriprtor rotated"); + // // plt::plot(x_pos_rotated, y_pos_rotated, "or"); + // // plt::show(); + // } + // } + // } + std::cout << "acum_distance : " << acum_distance << std::endl; ASSERT_EQ(acum_distance, 39); } @@ -154,7 +175,7 @@ TEST(loop_closure_falko, TestMatch) { // Initialization int scan_size = 1440; - LaserScan scan_1, scan_2, scan_3; + LaserScan scan_1, scan_2, scan_3, scan_target, scan_ref; LaserScanParams laser_params; laser_params.angle_min_ = 0; @@ -191,76 +212,82 @@ TEST(loop_closure_falko, TestMatch) ASSERT_EQ(asso_1_2.size(), 13); - std::cout << " n associations : " << asso_1_2.size() << std::endl; - - for (int i = 0; i < match_1_2->associations.size(); i++) + // ** TEST WITH TARGET AND REFERENCE SCENE + std::cout << "scan size : " << target_scan_1.size() << std::endl; + for (int i = 0; i < target_scan_1.size(); i++) { - std::cout << "id first : " << match_1_2->associations[i].first << std::endl; - std::cout << "id second : " << match_1_2->associations[i].second << std::endl; + scan_target.ranges_raw_.push_back(target_scan_1[i]); + scan_ref.ranges_raw_.push_back(reference_scan_1[i]); } - // auto key_1 = new_scene->keypoints_list_; - // auto key_2 = new_scene_2->keypoints_list_; - // auto desc_1 = new_scene->descriptors_list_rotated; - // auto desc_2 = new_scene_2->descriptors_list_rotated; - - // int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber; - // int circularSectorNumber = new_scene->descriptors_list_[0].circularSectorNumber; - - // int acum_distance = 0; - // for (int i = 0; i < desc_1.size(); i++) - // { - // for (int j = 0; j < desc_2.size(); j++) - // { - // if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or - // key_1[i].index == key_2[j].index - 1440 + rot) - // { - - // std::cout << "pair : " << i << " , " << j - // << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl; + param.use_descriptors_ = true; + param.matcher_distance_th_ = 100; + param.matcher_ddesc_th_ = 100; + param.nms_radius_ = 0.1; + param.neigh_b_ = 0.07; + param.b_ratio_ = 2.5; + + laser_params.angle_step_ = 0.00701248; + + LoopClosureFalko loop_cl_falko_2(param); + auto new_scene_target = + std::static_pointer_cast>(loop_cl_falko_2.extractScene(scan_target, laser_params)); + auto new_scene_reference = + std::static_pointer_cast>(loop_cl_falko_2.extractScene(scan_ref, laser_params)); + + std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl; + std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl; + + auto match_r_t = loop_cl_falko_2.matchScene(new_scene_reference, new_scene_target); + for (int i = 0; i < match_r_t->associations.size(); i++) + if (match_r_t->associations[i].second != -1) + { + std::cout << "id first : " << match_r_t->associations[i].first << std::endl; + std::cout << "id second : " << match_r_t->associations[i].second << std::endl; + } + + auto key_ref = new_scene_reference->keypoints_list_; + auto key_target = new_scene_target->keypoints_list_; + + std::vector x_ref_all, x_target_all; + std::vector y_ref_all, y_target_all; + + // Plotting keypoints scenes + for (int i = 0; i < key_ref.size(); i++) + { + x_ref_all.push_back(key_ref[i].point.x()); + y_ref_all.push_back(key_ref[i].point.y()); + } - // acum_distance += desc_1[i].distance(desc_2[j]); + for (int i = 0; i < key_target.size(); i++) + { + x_target_all.push_back(key_target[i].point.x()); + y_target_all.push_back(key_target[i].point.y()); + } - // // grid to x and y - // std::vector x_pos, x_pos_rotated; - // std::vector y_pos, y_pos_rotated; + plt::title("AHTMatcher BSC without descriptors"); + plt::plot(x_ref_all, y_ref_all, "ob"); + plt::plot(x_target_all, y_target_all, "or"); - // int desc_1_number = i; - // int desc_2_number = j; - // auto grid_1 = new_scene->descriptors_list_rotated[i].grid; - // auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid; + std::vector x_ref, x_target; + std::vector y_ref, y_target; + for (auto asso : match_r_t->associations) + if (asso.second != -1) + { + // auto a = key_ref[asso.first].point.x(); + x_ref.push_back(key_ref[asso.first].point.x()); + y_ref.push_back(key_ref[asso.first].point.y()); - // for (int i = 0; i < radialRingNumber; i++) - // for (int j = 0; j < circularSectorNumber; j++) - // { - // if (grid_1[i][j] > 0) - // { - // x_pos.push_back(i); - // y_pos.push_back(j); - // } + x_target.push_back(key_target[asso.second].point.x()); + y_target.push_back(key_target[asso.second].point.y()); + } - // if (grid_2[i][j] > 0) - // { - // x_pos_rotated.push_back(i); - // y_pos_rotated.push_back(j); - // } - // } + for (int i = 0; i < x_ref.size(); i++) + { + plt::plot({x_ref[i], x_target[i]}, {y_ref[i], y_target[i]}, "g"); + } - // // Plotting descriptors - // plt::title("NNMatcher BSC only keypoints"); - // plt::subplot(2, 1, 1); - // plt::xlabel(" descriprtor not rotated"); - // plt::plot(x_pos, y_pos, "ob"); - - // plt::subplot(2, 1, 2); - // plt::xlabel(" descriprtor rotated"); - // plt::plot(x_pos_rotated, y_pos_rotated, "or"); - // // plt::show(); - // } - // } - // } - // // std::cout << "acum_distance : " << acum_distance << std::endl; - // ASSERT_EQ(acum_distance, 39); + plt::show(); } int main(int argc, char **argv) -- GitLab From bfb166dc10ff943fcedd94285112a4574cf5ca4f Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 1 Jun 2021 11:53:50 +0200 Subject: [PATCH 090/100] wip --- src/loop_closure_falko.h | 326 ++++++++++++++++++++++++++++-- test/gtest_loop_closure_falko.cpp | 6 +- 2 files changed, 312 insertions(+), 20 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index c4f94ca..26961e5 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -63,14 +63,13 @@ struct ParameterLoopClosureFalko // matching double matcher_ddesc_th_ = 0.2; - // aht matcher - double xRes_ = 0.1; - double yRes_ = 0.1; - double thetaRes_ = 0.04; - double xAbsMax_ = 5; - double yAbsMax_ = 5; - double thetaAbsMax_ = 1.57; + double xRes_ = 0.15; + double yRes_ = 0.15; + double thetaRes_ = 0.1; + double xAbsMax_ = 3; + double yAbsMax_ = 3; + double thetaAbsMax_ = 3; }; /** \brief A class for loop closure using falko library @@ -94,7 +93,6 @@ struct ParameterLoopClosureFalko template typename M> class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor { - private: public: typedef std::shared_ptr> sceneFalkoBSCPtr; typedef std::shared_ptr laserScanPtr; @@ -127,8 +125,311 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract use_descriptors_ = _param.use_descriptors_; }; - // Template specialization + /** \brief Destructor + **/ + ~LoopClosureFalko() {} + + /** \brief Create and update the scene struct with keypoints and descriptors + **/ + sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override + { + auto new_scene = std::make_shared>(); + auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); + // Extract keypoints + std::vector keypoints_list; + extract(*scan_falko, keypoints_list); + + double angle_min = _scan_params.angle_min_; + double angle_step = _scan_params.angle_step_; + + // Compute max_dist + new_scene->max_distance_ = 0; + for (int i = 0; i < keypoints_list.size(); i++) + for (int j = 0; j < keypoints_list.size(); j++) + { + double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]); + double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]); + double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); + if (distance > new_scene->max_distance_) + new_scene->max_distance_ = distance; + } + + // discard too close by kp + for (int i = 0; i < keypoints_list.size(); i++) + { + int repeated = 0; + for (int j = i + 1; j < keypoints_list.size(); j++) + { + double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]); + double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]); + double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); + if (distance < 0.05) + { + repeated = 1; + } + } + if (repeated == 0) + { + new_scene->keypoints_list_.push_back(keypoints_list[i]); + } + } + + // Compute descriptors + extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); + std::vector descriptors_list_rotated; + extractor_.compute(*scan_falko, new_scene->keypoints_list_, descriptors_list_rotated); + + // Compute Scene mid point, angle for each keypoint and rotate descriptors + Eigen::Vector2d mid_point(0, 0); + std::vector angle_rotation; + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) + { + mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; + mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; + angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); + // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i]; + double orientation = angle_rotation[i]; + descriptors_list_rotated[i].rotate(orientation); + new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]); + } + + new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); + // Compute Scene Area and Perimeter + int n = 3; + double X[n]; + double Y[n]; + new_scene->perimeter_ = 0.0; + new_scene->area_ = 0.0; + Eigen::Vector2d dist_between_two_kp; + + if (new_scene->keypoints_list_.size() < 3) + return new_scene; + + if (new_scene->keypoints_list_.size() < 4) + { + X[0] = new_scene->keypoints_list_[0].point[0]; + Y[0] = new_scene->keypoints_list_[0].point[1]; + + X[1] = new_scene->keypoints_list_[1].point[0]; + Y[1] = new_scene->keypoints_list_[1].point[1]; + + X[2] = new_scene->keypoints_list_[2].point[0]; + Y[2] = new_scene->keypoints_list_[2].point[1]; + + new_scene->area_ = new_scene->area_ + triangleArea(X, Y, n); + return new_scene; + } + + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) + { + X[0] = new_scene->mid_point_[0]; + Y[0] = new_scene->mid_point_[1]; + + // X[0] = 0.0; + // Y[0] = 0.0; + X[1] = new_scene->keypoints_list_[i].point[0]; + Y[1] = new_scene->keypoints_list_[i].point[1]; + + if (i < new_scene->keypoints_list_.size() - 1) // Proceed until final keypoint + { + X[2] = new_scene->keypoints_list_[i + 1].point[0]; + Y[2] = new_scene->keypoints_list_[i + 1].point[1]; + + dist_between_two_kp = + new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point; + } + else // if you arrived to the final keypoint then use inital keypoint + { + X[2] = new_scene->keypoints_list_[0].point[0]; + Y[2] = new_scene->keypoints_list_[0].point[1]; + + dist_between_two_kp = new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[0].point; + } + new_scene->area_ = new_scene->area_ + (double)triangleArea(X, Y, n); + new_scene->perimeter_ = new_scene->perimeter_ + hypot(dist_between_two_kp[0], dist_between_two_kp[1]); + } + + // Compue Scene linear regresion and initial angle + double ss_xy = 0; + double ss_xx = 0; + for (int i = 0; i <= new_scene->keypoints_list_.size(); i++) + { + ss_xy += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * + (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]); + ss_xx += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * + (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]); + } + double b_1 = ss_xy / ss_xx; + // double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0]; + + // new_scene->regressor_coefs.push_back(b_0); + // new_scene->regressor_coefs.push_back(b_1); + + double initial_angle = -atan(b_1); + + // double inital_angle_inv = initial_angle - M_PI; + + // rotate keypoints + for (int i = 0; i < new_scene->keypoints_list_.size(); i++) + { + falkolib::FALKO keypoint_mid; + keypoint_mid = new_scene->keypoints_list_[i]; + + keypoint_mid.point[0] = new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]; + keypoint_mid.point[1] = new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]; + new_scene->keypoints_list_mid_point_.push_back(keypoint_mid); + + falkolib::FALKO keypoint_rotated; + keypoint_rotated = new_scene->keypoints_list_[i]; + keypoint_rotated.point[0] = new_scene->keypoints_list_[i].point[0] * cos(initial_angle) - + new_scene->keypoints_list_[i].point[1] * sin(initial_angle); + keypoint_rotated.point[1] = new_scene->keypoints_list_[i].point[0] * sin(initial_angle) + + new_scene->keypoints_list_[i].point[1] * cos(initial_angle); + new_scene->keypoints_list_rotated_.push_back(keypoint_rotated); + + falkolib::FALKO keypoint_rot_trans; + keypoint_rot_trans = new_scene->keypoints_list_[i]; + keypoint_rot_trans.point[0] = + (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * cos(initial_angle) - + (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * sin(initial_angle); + + keypoint_rot_trans.point[1] = + (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * sin(initial_angle) + + (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * cos(initial_angle); + + new_scene->keypoints_list_transl_rot_.push_back(keypoint_rot_trans); + } + + return new_scene; + } + + /** \brief Convert scans from laserscanutils::LaserScan to + *falkolib::LaserScan object + **/ + laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params) + { + auto scan_falko = std::make_shared(_scan_params.angle_min_, _scan_params.angle_max_, + _scan.ranges_raw_.size()); + std::vector 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 produced when matching two given scenes + * \param _scene_1 reference scene struct + * \param _scene_2 target scene struct + **/ + MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override + { + std::vector> asso_nn; + auto scene_1_falko = std::static_pointer_cast>(_scene_1); + auto scene_2_falko = std::static_pointer_cast>(_scene_2); + + int matching_number = 0; + + if (use_descriptors_ == 0) + { + // matching_number = matcher_.match(scene_1_falko->keypoints_list_transl_rot_, + // scene_2_falko->keypoints_list_transl_rot_, asso_nn); + + matching_number = + matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); + } + else if (use_descriptors_ == 1) + { + // matching_number = matcher_.match( + // scene_1_falko->keypoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, + // scene_2_falko->keypoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn); + + matching_number = + matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated, + scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn); + } + auto new_match = std::make_shared(); + new_match->keypoints_number_match = matching_number; + if (matching_number > keypoints_number_th_) + { + new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, + asso_nn, new_match->transform); + } + else + { + new_match->match = false; + } + + new_match->associations = asso_nn; + new_match->scene_1 = _scene_1; + new_match->scene_2 = _scene_2; + + // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), + // scene_2_falko->keypoints_list_.size()); + + new_match->score = (double)matching_number; + + new_match->transform_vector.head(2) = new_match->transform.translation(); + new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); + + return new_match; + } + + int keypoints_number_th_; + bool use_descriptors_; + + // (X[i], Y[i]) are coordinates of i'th point. + double triangleArea(double X[], double Y[], int n) + { + // Initialize area + double area = 0.0; + + // Calculate value of shoelace formula + int j = n - 1; + for (int i = 0; i < n; i++) + { + area += (X[j] + X[i]) * (Y[j] - Y[i]); + j = i; // j is previous vertex to i + } + + // Return absolute value + return fabs((double)area / 2.0); + } +}; +// Partial template specialization +template +class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor +{ + public: + typedef std::shared_ptr> sceneFalkoBSCPtr; + typedef std::shared_ptr laserScanPtr; + + Extr extractor_; + falkolib::AHTMatcher matcher_; + + /** \brief Constructor + * \param _param parameter struct with falko lib parameters + **/ + LoopClosureFalko(ParameterLoopClosureFalko _param) + : LoopClosureBase2d() + , falkolib::FALKOExtractor() + , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_) + , matcher_(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_, _param.yAbsMax_, _param.thetaAbsMax_) + + { + // FALKO Extractor Parameters + 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(_param.matcher_distance_th_); + matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_); + keypoints_number_th_ = _param.keypoints_number_th_; + use_descriptors_ = _param.use_descriptors_; + }; /** \brief Destructor **/ ~LoopClosureFalko() {} @@ -191,16 +492,9 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); - // descriptors_list_rotated[i].rotate(angle_rotation[i]); // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i]; double orientation = angle_rotation[i]; - // if (orientation < 0) - // { - // orientation = 2 * M_PI + orientation; - // } descriptors_list_rotated[i].rotate(orientation); - // std::cout << "orientation : " << angle_rotation[i] << std::endl; - // std::cout << "orientation desc : " << orientation << std::endl; new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]); } diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 878ac4b..6394926 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -88,7 +88,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation) auto new_scene_2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_2, laser_params)); auto key_1 = new_scene->keypoints_list_; auto key_2 = new_scene_2->keypoints_list_; - auto desc_1 = new_scene->descriptors_list_rotated; + auto desc_1 = new_scene->descriptors_list_; auto desc_2 = new_scene_2->descriptors_list_rotated; int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber; @@ -111,9 +111,8 @@ TEST(loop_closure_falko, TestDescriptorsRotation) min_dist = asso_dist[i][j]; asso_number = j; } - std::cout << "pair : " << i << " , " << j << " , distance : " << asso_dist[i][j] << std::endl; } - // std::cout << "pair : " << i << " , " << asso_number << " , distance : " << min_dist << std::endl; + std::cout << "pair : " << i << " , " << asso_number << " , distance : " << min_dist << std::endl; } // for (int i = 0; i < desc_1.size(); i++) @@ -167,7 +166,6 @@ TEST(loop_closure_falko, TestDescriptorsRotation) // } // } // } - std::cout << "acum_distance : " << acum_distance << std::endl; ASSERT_EQ(acum_distance, 39); } -- GitLab From 31e28c22e981c3b0509ee2ed672359877a246681 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 10 Jun 2021 15:43:34 +0200 Subject: [PATCH 091/100] wip --- src/loop_closure_falko.h | 344 +++--------------------------- test/gtest_loop_closure_falko.cpp | 8 +- 2 files changed, 36 insertions(+), 316 deletions(-) diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 26961e5..7bb20be 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -103,11 +103,11 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract /** \brief Constructor * \param _param parameter struct with falko lib parameters **/ - LoopClosureFalko(ParameterLoopClosureFalko _param) + LoopClosureFalko(ParameterLoopClosureFalko _param, const M &_matcher) : LoopClosureBase2d() , falkolib::FALKOExtractor() , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_) - , matcher_() + , matcher_(_matcher) { // FALKO Extractor Parameters setMinExtractionRange(_param.min_extraction_range_); @@ -346,17 +346,21 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated, scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn); } + + // std::cout << "laserscanutils.LoopClosureFalko.matchScene -> num of skp matched : " << matching_number << + // std::endl; + auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; - if (matching_number > keypoints_number_th_) - { - new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, - asso_nn, new_match->transform); - } - else - { - new_match->match = false; - } + // if (matching_number >= keypoints_number_th_ - 2) + // { + new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, + new_match->transform); + // } + // else + // { + // new_match->match = false; + // } new_match->associations = asso_nn; new_match->scene_1 = _scene_1; @@ -365,11 +369,11 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), // scene_2_falko->keypoints_list_.size()); - new_match->score = (double)matching_number; - new_match->transform_vector.head(2) = new_match->transform.translation(); new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); + new_match->score = (double)matching_number; + return new_match; } @@ -394,310 +398,26 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract return fabs((double)area / 2.0); } }; + // Partial template specialization -template -class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor + +template class LoopClosureFalkoAht : public LoopClosureFalko { public: - typedef std::shared_ptr> sceneFalkoBSCPtr; - typedef std::shared_ptr laserScanPtr; - - Extr extractor_; - falkolib::AHTMatcher matcher_; - - /** \brief Constructor - * \param _param parameter struct with falko lib parameters - **/ - LoopClosureFalko(ParameterLoopClosureFalko _param) - : LoopClosureBase2d() - , falkolib::FALKOExtractor() - , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_) - , matcher_(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_, _param.yAbsMax_, _param.thetaAbsMax_) - - { - // FALKO Extractor Parameters - 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(_param.matcher_distance_th_); - matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_); - keypoints_number_th_ = _param.keypoints_number_th_; - use_descriptors_ = _param.use_descriptors_; - }; - /** \brief Destructor - **/ - ~LoopClosureFalko() {} - - /** \brief Create and update the scene struct with keypoints and descriptors - **/ - sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override - { - auto new_scene = std::make_shared>(); - auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); - // Extract keypoints - std::vector keypoints_list; - extract(*scan_falko, keypoints_list); - - double angle_min = _scan_params.angle_min_; - double angle_step = _scan_params.angle_step_; - - // Compute max_dist - new_scene->max_distance_ = 0; - for (int i = 0; i < keypoints_list.size(); i++) - for (int j = 0; j < keypoints_list.size(); j++) - { - double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]); - double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]); - double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); - if (distance > new_scene->max_distance_) - new_scene->max_distance_ = distance; - } - - // discard too close by kp - for (int i = 0; i < keypoints_list.size(); i++) - { - int repeated = 0; - for (int j = i + 1; j < keypoints_list.size(); j++) - { - double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]); - double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]); - double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); - if (distance < 0.05) - { - repeated = 1; - } - } - if (repeated == 0) - { - new_scene->keypoints_list_.push_back(keypoints_list[i]); - } - } - - // Compute descriptors - extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); - std::vector descriptors_list_rotated; - extractor_.compute(*scan_falko, new_scene->keypoints_list_, descriptors_list_rotated); - - // Compute Scene mid point, angle for each keypoint and rotate descriptors - Eigen::Vector2d mid_point(0, 0); - std::vector angle_rotation; - for (int i = 0; i < new_scene->keypoints_list_.size(); i++) - { - mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; - mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; - angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); - // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i]; - double orientation = angle_rotation[i]; - descriptors_list_rotated[i].rotate(orientation); - new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]); - } - - new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); - // Compute Scene Area and Perimeter - int n = 3; - double X[n]; - double Y[n]; - new_scene->perimeter_ = 0.0; - new_scene->area_ = 0.0; - Eigen::Vector2d dist_between_two_kp; - - if (new_scene->keypoints_list_.size() < 3) - return new_scene; - - if (new_scene->keypoints_list_.size() < 4) - { - X[0] = new_scene->keypoints_list_[0].point[0]; - Y[0] = new_scene->keypoints_list_[0].point[1]; - - X[1] = new_scene->keypoints_list_[1].point[0]; - Y[1] = new_scene->keypoints_list_[1].point[1]; - - X[2] = new_scene->keypoints_list_[2].point[0]; - Y[2] = new_scene->keypoints_list_[2].point[1]; - - new_scene->area_ = new_scene->area_ + triangleArea(X, Y, n); - return new_scene; - } - - for (int i = 0; i < new_scene->keypoints_list_.size(); i++) - { - X[0] = new_scene->mid_point_[0]; - Y[0] = new_scene->mid_point_[1]; - - // X[0] = 0.0; - // Y[0] = 0.0; - - X[1] = new_scene->keypoints_list_[i].point[0]; - Y[1] = new_scene->keypoints_list_[i].point[1]; - - if (i < new_scene->keypoints_list_.size() - 1) // Proceed until final keypoint - { - X[2] = new_scene->keypoints_list_[i + 1].point[0]; - Y[2] = new_scene->keypoints_list_[i + 1].point[1]; - - dist_between_two_kp = - new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point; - } - else // if you arrived to the final keypoint then use inital keypoint - { - X[2] = new_scene->keypoints_list_[0].point[0]; - Y[2] = new_scene->keypoints_list_[0].point[1]; - - dist_between_two_kp = new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[0].point; - } - new_scene->area_ = new_scene->area_ + (double)triangleArea(X, Y, n); - new_scene->perimeter_ = new_scene->perimeter_ + hypot(dist_between_two_kp[0], dist_between_two_kp[1]); - } - - // Compue Scene linear regresion and initial angle - double ss_xy = 0; - double ss_xx = 0; - for (int i = 0; i <= new_scene->keypoints_list_.size(); i++) - { - ss_xy += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * - (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]); - ss_xx += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * - (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]); - } - double b_1 = ss_xy / ss_xx; - // double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0]; - - // new_scene->regressor_coefs.push_back(b_0); - // new_scene->regressor_coefs.push_back(b_1); - - double initial_angle = -atan(b_1); - - // double inital_angle_inv = initial_angle - M_PI; - - // rotate keypoints - for (int i = 0; i < new_scene->keypoints_list_.size(); i++) - { - falkolib::FALKO keypoint_mid; - keypoint_mid = new_scene->keypoints_list_[i]; - - keypoint_mid.point[0] = new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]; - keypoint_mid.point[1] = new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]; - new_scene->keypoints_list_mid_point_.push_back(keypoint_mid); - - falkolib::FALKO keypoint_rotated; - keypoint_rotated = new_scene->keypoints_list_[i]; - keypoint_rotated.point[0] = new_scene->keypoints_list_[i].point[0] * cos(initial_angle) - - new_scene->keypoints_list_[i].point[1] * sin(initial_angle); - keypoint_rotated.point[1] = new_scene->keypoints_list_[i].point[0] * sin(initial_angle) + - new_scene->keypoints_list_[i].point[1] * cos(initial_angle); - new_scene->keypoints_list_rotated_.push_back(keypoint_rotated); - - falkolib::FALKO keypoint_rot_trans; - keypoint_rot_trans = new_scene->keypoints_list_[i]; - keypoint_rot_trans.point[0] = - (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * cos(initial_angle) - - (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * sin(initial_angle); - - keypoint_rot_trans.point[1] = - (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * sin(initial_angle) + - (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * cos(initial_angle); - - new_scene->keypoints_list_transl_rot_.push_back(keypoint_rot_trans); - } - - return new_scene; - } - - /** \brief Convert scans from laserscanutils::LaserScan to - *falkolib::LaserScan object - **/ - laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params) - { - auto scan_falko = std::make_shared(_scan_params.angle_min_, _scan_params.angle_max_, - _scan.ranges_raw_.size()); - std::vector 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 produced when matching two given scenes - * \param _scene_1 reference scene struct - * \param _scene_2 target scene struct - **/ - MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override - { - std::vector> asso_nn; - auto scene_1_falko = std::static_pointer_cast>(_scene_1); - auto scene_2_falko = std::static_pointer_cast>(_scene_2); - - int matching_number = 0; - - if (use_descriptors_ == 0) - { - // matching_number = matcher_.match(scene_1_falko->keypoints_list_transl_rot_, - // scene_2_falko->keypoints_list_transl_rot_, asso_nn); - - matching_number = - matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); - } - else if (use_descriptors_ == 1) - { - // matching_number = matcher_.match( - // scene_1_falko->keypoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, - // scene_2_falko->keypoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn); - - matching_number = - matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated, - scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn); - } - auto new_match = std::make_shared(); - new_match->keypoints_number_match = matching_number; - if (matching_number > keypoints_number_th_) - { - new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, - asso_nn, new_match->transform); - } - else - { - new_match->match = false; - } - - new_match->associations = asso_nn; - new_match->scene_1 = _scene_1; - new_match->scene_2 = _scene_2; - - // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), - // scene_2_falko->keypoints_list_.size()); - - new_match->score = (double)matching_number; - - new_match->transform_vector.head(2) = new_match->transform.translation(); - new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); - - return new_match; - } - - int keypoints_number_th_; - bool use_descriptors_; - - // (X[i], Y[i]) are coordinates of i'th point. - double triangleArea(double X[], double Y[], int n) - { - // Initialize area - double area = 0.0; + LoopClosureFalkoAht(ParameterLoopClosureFalko _param) + : LoopClosureFalko( + _param, + falkolib::AHTMatcher(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_, + _param.yAbsMax_, _param.thetaAbsMax_)){}; +}; - // Calculate value of shoelace formula - int j = n - 1; - for (int i = 0; i < n; i++) - { - area += (X[j] + X[i]) * (Y[j] - Y[i]); - j = i; // j is previous vertex to i - } +template class LoopClosureFalkoNn : public LoopClosureFalko +{ + public: + LoopClosureFalkoNn(ParameterLoopClosureFalko _param) + : LoopClosureFalko(_param, falkolib::NNMatcher()){ - // Return absolute value - return fabs((double)area / 2.0); - } + }; }; } /* namespace laserscanutils */ diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 6394926..9c0caf4 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -24,7 +24,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ParameterLoopClosureFalko param; param.matcher_distance_th_ = 0.3; - LoopClosureFalko loop_cl_falko(param); + LoopClosureFalkoAht loop_cl_falko(param); // Test convert2LaserScanFALKO std::shared_ptr scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params); @@ -81,7 +81,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation) ParameterLoopClosureFalko param; param.matcher_distance_th_ = 0.3; - LoopClosureFalko loop_cl_falko(param); + LoopClosureFalkoAht loop_cl_falko(param); // Test extractScene auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_1, laser_params)); @@ -194,7 +194,7 @@ TEST(loop_closure_falko, TestMatch) ParameterLoopClosureFalko param; param.matcher_distance_th_ = 100; param.matcher_ddesc_th_ = 5; - LoopClosureFalko loop_cl_falko(param); + LoopClosureFalkoAht loop_cl_falko(param); // Test extractScene auto new_scene_1 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_1, laser_params)); @@ -227,7 +227,7 @@ TEST(loop_closure_falko, TestMatch) laser_params.angle_step_ = 0.00701248; - LoopClosureFalko loop_cl_falko_2(param); + LoopClosureFalkoAht loop_cl_falko_2(param); auto new_scene_target = std::static_pointer_cast>(loop_cl_falko_2.extractScene(scan_target, laser_params)); auto new_scene_reference = -- GitLab From c45c76cb2d37bda85466053397584f7f50e05578 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 17 Jun 2021 17:31:52 +0200 Subject: [PATCH 092/100] wip --- deps/falkolib | 2 +- src/loop_closure_falko.h | 2 +- test/gtest_loop_closure_falko.cpp | 36 +++++++++++++++++++++---------- 3 files changed, 27 insertions(+), 13 deletions(-) diff --git a/deps/falkolib b/deps/falkolib index 8f341e9..0017c6c 160000 --- a/deps/falkolib +++ b/deps/falkolib @@ -1 +1 @@ -Subproject commit 8f341e9f1739c445cd8352586d514e2e49e25718 +Subproject commit 0017c6c5be963b923684bfd8eb6bd8b7641ba1b6 diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 7bb20be..7a5774e 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -45,7 +45,7 @@ struct ParameterLoopClosureFalko // Keypoints extractor Default double min_extraction_range_ = 0; double max_extraction_range_ = 30; - bool enable_subbeam_ = true; + bool enable_subbeam_ = false; double nms_radius_ = 0.1; double neigh_b_ = 0.01; double b_ratio_ = 4; diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 9c0caf4..b7e4ab0 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -81,7 +81,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation) ParameterLoopClosureFalko param; param.matcher_distance_th_ = 0.3; - LoopClosureFalkoAht loop_cl_falko(param); + LoopClosureFalkoAht loop_cl_falko(param); // Test extractScene auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_1, laser_params)); @@ -91,8 +91,8 @@ TEST(loop_closure_falko, TestDescriptorsRotation) auto desc_1 = new_scene->descriptors_list_; auto desc_2 = new_scene_2->descriptors_list_rotated; - int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber; - int circularSectorNumber = new_scene->descriptors_list_[0].circularSectorNumber; + int radialRingNumber = 8; + int circularSectorNumber = 16; int acum_distance = 0; double asso_dist[key_1.size()][key_2.size()]; @@ -166,7 +166,6 @@ TEST(loop_closure_falko, TestDescriptorsRotation) // } // } // } - ASSERT_EQ(acum_distance, 39); } TEST(loop_closure_falko, TestMatch) @@ -194,7 +193,7 @@ TEST(loop_closure_falko, TestMatch) ParameterLoopClosureFalko param; param.matcher_distance_th_ = 100; param.matcher_ddesc_th_ = 5; - LoopClosureFalkoAht loop_cl_falko(param); + LoopClosureFalkoAht loop_cl_falko(param); // Test extractScene auto new_scene_1 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan_1, laser_params)); @@ -207,8 +206,19 @@ TEST(loop_closure_falko, TestMatch) for (auto asso : match_1_2->associations) if (asso.second != -1) asso_1_2.push_back(asso); +} + +TEST(loop_closure_falko, TestMatch2) +{ + + // Initialization + int scan_size = 1440; + LaserScan scan_target, scan_ref; + LaserScanParams laser_params; - ASSERT_EQ(asso_1_2.size(), 13); + laser_params.angle_min_ = 0; + laser_params.angle_max_ = 2.0 * M_PI; + laser_params.angle_step_ = laser_params.angle_max_ / 1440; // ** TEST WITH TARGET AND REFERENCE SCENE std::cout << "scan size : " << target_scan_1.size() << std::endl; @@ -218,17 +228,19 @@ TEST(loop_closure_falko, TestMatch) scan_ref.ranges_raw_.push_back(reference_scan_1[i]); } + ParameterLoopClosureFalko param; param.use_descriptors_ = true; param.matcher_distance_th_ = 100; - param.matcher_ddesc_th_ = 100; + param.matcher_ddesc_th_ = 20; param.nms_radius_ = 0.1; - param.neigh_b_ = 0.07; - param.b_ratio_ = 2.5; + param.neigh_b_ = 0.1; + param.b_ratio_ = 4; + param.enable_subbeam_ = false; laser_params.angle_step_ = 0.00701248; - LoopClosureFalkoAht loop_cl_falko_2(param); - auto new_scene_target = + LoopClosureFalkoAht loop_cl_falko_2(param); + auto new_scene_target = std::static_pointer_cast>(loop_cl_falko_2.extractScene(scan_target, laser_params)); auto new_scene_reference = std::static_pointer_cast>(loop_cl_falko_2.extractScene(scan_ref, laser_params)); @@ -244,6 +256,8 @@ TEST(loop_closure_falko, TestMatch) std::cout << "id second : " << match_r_t->associations[i].second << std::endl; } + std::cout << "transform : " << match_r_t->transform_vector.transpose() << std::endl; + auto key_ref = new_scene_reference->keypoints_list_; auto key_target = new_scene_target->keypoints_list_; -- GitLab From b8c704f8e6a832515f1a73dfcfd034f11d0e9bf5 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Mon, 28 Jun 2021 16:23:44 +0200 Subject: [PATCH 093/100] modified to input in falkolib::LaserScan input from inf to 250 --- deps/falkolib | 2 +- src/loop_closure_falko.h | 13 ++++++++++++- test/data/scan_data.h | 7 +++++-- test/gtest_loop_closure_falko.cpp | 1 + 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/deps/falkolib b/deps/falkolib index 0017c6c..8f341e9 160000 --- a/deps/falkolib +++ b/deps/falkolib @@ -1 +1 @@ -Subproject commit 0017c6c5be963b923684bfd8eb6bd8b7641ba1b6 +Subproject commit 8f341e9f1739c445cd8352586d514e2e49e25718 diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 7a5774e..a4a5343 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -309,9 +309,20 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract **/ laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params) { - auto scan_falko = std::make_shared(_scan_params.angle_min_, _scan_params.angle_max_, + double field_of_view = _scan_params.angle_max_-_scan_params.angle_min_; + auto scan_falko = std::make_shared(_scan_params.angle_min_, field_of_view, _scan.ranges_raw_.size()); + std::vector double_ranges(_scan.ranges_raw_.begin(), _scan.ranges_raw_.end()); + + for (int i = 0; i < double_ranges.size(); i++) + { + if (std::to_string(double_ranges[i]) == "inf") + { + double_ranges[i] = 250; + } + } + scan_falko->fromRanges(double_ranges); return scan_falko; } diff --git a/test/data/scan_data.h b/test/data/scan_data.h index a98a244..7728475 100644 --- a/test/data/scan_data.h +++ b/test/data/scan_data.h @@ -2899,5 +2899,8 @@ std::vector testRanges2 = {250, 250, 250}; -std::vector target_scan_1 = {2.03169,1.9677,2.02369,2.01969,1.9517,1.93171,1.93171,1.89171,1.92771,1.92771,1.89171,1.85172,1.83172,1.85972,1.85172,1.79173,1.55576,1.54376,1.53577,1.49577,1.49577,1.49577,1.45178,1.47977,1.47977,1.45578,1.59576,250,250,250,1.59576,1.59576,1.59576,1.59576,1.54776,1.55176,1.53577,1.53577,1.51977,1.50377,1.49577,1.48377,1.47977,1.44378,1.47178,1.47578,1.44378,1.42378,1.41978,1.42778,1.42378,1.41179,1.3198,1.26781,1.22781,1.24781,1.21981,1.20782,1.23581,1.19582,1.22781,1.19982,1.17982,1.20382,1.17582,1.17982,1.16782,1.16382,1.15582,1.15182,1.15982,1.14783,1.13183,1.14783,1.12383,1.15182,1.14383,1.13583,1.24781,1.24781,1.19582,1.2958,1.3118,1.3038,1.2958,1.2998,1.2998,1.3038,1.2978,1.2858,1.2818,1.27581,1.2838,1.2878,1.27981,1.3078,1.2898,1.27781,1.2878,1.27981,1.3018,1.2998,1.2938,1.2958,250,250,1.19582,1.21182,1.20382,1.18382,1.23581,1.19582,250,250,250,1.9777,1.9837,1.9777,1.9857,1.9917,2.09168,250,4.49132,4.48732,4.48332,4.48532,4.47732,4.47132,4.48532,4.47332,4.47532,4.46332,4.47332,4.47132,4.46332,4.49931,4.96124,5.04323,6.571,6.567,6.579,6.579,6.64299,7.55685,7.52685,7.56485,7.93079,250,250,8.70467,8.69668,8.69268,8.72067,8.72067,8.72067,8.73267,8.73667,8.76467,8.76267,8.76467,5.01924,5.01524,5.02723,5.02723,5.03123,5.05523,5.04323,5.07123,5.07123,5.07123,5.09722,5.01924,4.92325,4.68529,4.64729,4.66929,4.66529,4.68929,4.68729,4.70128,4.71728,4.70928,4.73928,4.74928,4.75128,4.76927,4.76327,4.79527,4.79527,4.80927,4.82926,4.83526,4.85326,4.86926,4.86726,4.88926,4.90725,4.92325,4.94325,4.95125,4.97124,5.00524,3.13952,3.11753,3.06953,3.03754,2.99154,2.96555,2.91156,2.87756,2.83757,2.22366,2.18967,2.16167,2.15367,2.15167,2.14167,2.17367,2.16767,2.16367,2.18567,2.32765,2.37964,2.44363,2.43163,2.41763,2.38564,2.37364,2.34164,2.30165,2.27565,2.26965,2.30565,2.31765,2.33764,2.34364,2.36164,2.37364,2.38364,2.40563,2.41163,2.41963,2.44163,2.45363,2.47362,2.48362,2.49162,2.51162,2.52961,2.55961,2.56561,2.57361,2.5996,2.6136,2.6396,2.6576,2.66959,2.69559,250,2.71959,7.15891,7.17491,7.2469,7.27889,7.37888,9.08662,9.06062,8.99063,8.90664,8.84865,8.76266,8.68668,8.67868,8.66668,8.66268,8.73067,8.81066,8.91064,9.01063,9.11461,9.2106,9.28259,9.43456,9.49056,9.62653,9.73252,9.8625,9.96848,10.0825,10.2244,10.3484,10.4724,10.6244,10.7524,10.9003,11.0623,11.2203,11.4623,11.6362,11.8122,11.9922,3.79342,3.78142,3.77343,3.76743,3.75143,3.73943,3.73743,3.71743,3.71143,3.70544,3.69144,3.68544,3.67344,3.67544,3.66144,3.63945,3.64145,3.62945,3.63345,3.61745,3.61345,3.61145,3.60345,3.60345,3.58545,3.58545,3.57746,3.55746,3.56946,3.56946,3.54746,3.55546,3.53346,3.54146,3.53346,3.52746,3.52146,3.50947,3.51547,3.50547,3.48947,3.50347,3.49947,3.50147,3.48547,3.47747,3.49147,3.47947,1.90571,1.90571,2.38764,2.37764,2.36764,3.47747,3.47747,3.45947,3.46147,3.46547,3.47747,3.45747,3.46747,3.48147,3.46747,3.47947,3.48147,3.46147,3.47947,3.47347,3.48147,3.47347,3.47147,2.38764,2.36364,1.90171,1.88771,1.88771,3.48547,3.59145,5.19121,4.08338,3.88341,3.69544,3.58345,4.18136,4.17536,3.2915,2.89156,2.83157,2.77958,2.79157,2.82357,2.81957,2.82557,2.84357,250,250,1.9677,2.0037,250,250,2.87956,2.86156,2.85956,2.87756,2.87356,2.89956,2.89956,2.89356,2.91356,2.90756,2.92955,4.41933,4.42333,4.44332,4.44332,4.47132,4.47532,1.43178,1.42778,1.37579,1.35979,1.3418,1.3298,1.3098,1.2858,1.2938,1.2818,1.25581,1.34779,1.44978,1.46778,1.47178,1.48177,1.49377,1.49177,1.51777,1.51377,1.49977,1.51977,1.52377,1.54376,1.54976,1.54376,1.55976,1.55976,1.57176,1.57576,1.57576,1.59176,1.59376,1.60975,1.61575,1.61975,1.62775,1.63775,1.65375,1.65175,1.66575,1.68174,1.68174,1.69774,1.70574,1.70574,1.73174,1.72774,1.75173,1.74973,1.76573,1.77573,1.78573,1.80573,1.81372,1.81372,1.83372,1.84572,1.86772,1.87571,1.88771,1.91171,1.92571,1.93371,1.9597,1.9637,1.9917,1.9857,2.01969,2.03169,2.03569,2.05969,2.07168,2.09368,2.11768,2.13168,2.15967,2.16967,2.19967,2.21566,2.23166,2.25966,2.28365,2.27765,2.25766,2.23566,2.23166,2.21166,2.21966,2.20766,2.19767,2.21566,2.24766,4.41933,4.39133,4.38333,4.37533,4.35134,4.32334,4.30334,4.29935,4.28335,4.29135,4.25935,4.24335,4.21536,4.21136,4.19536,4.19536,4.17137,4.15137,4.15137,4.11537,4.12737,4.09538,3.79542,3.79142,3.87541,3.9314,4.03139,4.45332,4.44332,3.04754,3.03554,3.05353,3.08753,2.98755,2.98755,3.21951,3.2715,3.07753,3.03954,3.03554,2.85357,2.86556,2.91156,2.90956,3.01554,3.03754,3.01754,3.00754,2.89556,2.90156,2.88556,3.02154,3.37349,3.33949,2.95155,2.92156,2.91956,2.91156,2.91156,2.91956,2.95155,2.99954,3.2595,3.65944,4.18736,4.18336,4.19536,4.19536,2.79557,2.93355,2.97955,3.01954,3.03554,3.05554,3.08753,3.08753,3.12552,3.12552,3.12952,3.15352,3.13952,3.17352,3.69944,3.82142,3.89141,250,2.19167,2.17967,2.13567,2.08368,2.04369,2.04369,1.9837,1.9677,1.92371,1.69174,1.64975,1.63375,1.62775,1.62775,1.62375,1.63375,1.63775,1.64175,1.65775,1.67774,1.68774,1.90371,1.9477,1.9837,1.9797,1.9877,1.9757,1.93171,1.9557,1.9437,1.9637,1.89171,1.78173,1.76173,1.75173,1.76573,1.74573,1.73974,1.76373,1.75973,1.77973,1.77373,1.75373,1.79573,4.64729,4.6193,4.63129,4.63929,4.65529,4.67529,4.70328,4.71928,4.70328,4.33934,250,4.11537,4.09938,4.07538,4.01739,3.98539,3.98339,3.9614,3.97539,4.01539,1.70574,1.67774,1.65975,1.66575,1.64775,1.63975,1.62175,1.59976,1.65175,1.68974,1.69974,1.69774,1.63975,1.64575,1.63175,1.63575,1.64975,1.62775,1.63975,1.64575,1.64375,1.65375,1.64975,1.65175,1.65575,1.65775,1.67574,1.67175,1.68974,1.68974,1.69974,1.70574,1.71774,1.72974,1.73574,1.76173,1.76973,1.78373,1.81972,1.89171,250,250,250,250,5.05923,5.16521,5.15122,5.10922,5.09722,5.05923,5.03123,5.00924,4.97724,4.95525,4.92925,4.90325,4.89126,4.85326,4.85126,4.81727,4.52731,4.51531,4.49132,4.48332,4.45932,4.43532,4.42333,4.39533,4.39333,4.37533,4.35734,4.33134,4.31334,4.30534,4.28735,4.32134,4.38733,4.43732,4.47332,4.44732,4.43332,4.42533,4.26935,3.82742,3.81542,3.79942,3.80142,3.77942,3.77543,3.76943,3.74143,3.73943,3.72143,3.71543,3.69544,3.67544,3.65944,3.64345,3.63945,3.65744,3.63945,3.63345,3.61545,3.62345,3.60345,3.60545,3.58145,3.57146,2.75158,2.77758,2.77158,2.74758,2.71159,2.70559,2.68359,2.69559,2.71359,2.73358,2.75958,2.75758,2.81357,2.82957,3.00354,3.18152,3.2755,3.2735,3.2635,3.2615,3.2595,3.23951,3.24951,3.24351,3.23151,3.23951,3.22351,2.99754,2.98355,2.98555,2.99154,2.97555,2.99954,2.99154,2.98155,2.99354,2.97155,2.97755,2.97355,2.97755,2.98355,2.98155,2.98954,2.98755,2.97555,2.98555,2.96555,2.99154,4.01739,4.02139,4.02339,3.72943,3.57946,3.44348,3.2695,3.2555,3.2695,3.2755,3.2695,3.2735,3.2915,3.2815,3.3015,3.3075,3.3095,3.31949,3.3155,3.33749,3.32749,3.32749,4.04538,4.04738,4.12537,3.9674,3.82942,3.69744,3.51147,3.46547,3.48147,3.41148,3.46947,4.21136,4.21936,4.21936,4.23136,4.24935,4.25135,4.26135,3.56346,3.50347,3.45147,3.40548,3.40348,3.38348,3.38748,3.38149,3.37749,3.39948,3.39948,3.43748,3.47347,3.49947,3.59145,3.85141,3.84541,4.47132,4.52331,4.54331,3.9334,4.5813,4.17736,4.5953,4.62929,2.25366,2.19966,2.18567,2.17967,2.19967,2.23166,250,3.41948,2.06369,2.09168,2.07168,2.06369,2.07168,1.9917,1.9917}; -std::vector reference_scan_1 = {3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144}; \ No newline at end of file +// std::vector target_scan_1 = {2.03169,1.9677,2.02369,2.01969,1.9517,1.93171,1.93171,1.89171,1.92771,1.92771,1.89171,1.85172,1.83172,1.85972,1.85172,1.79173,1.55576,1.54376,1.53577,1.49577,1.49577,1.49577,1.45178,1.47977,1.47977,1.45578,1.59576,250,250,250,1.59576,1.59576,1.59576,1.59576,1.54776,1.55176,1.53577,1.53577,1.51977,1.50377,1.49577,1.48377,1.47977,1.44378,1.47178,1.47578,1.44378,1.42378,1.41978,1.42778,1.42378,1.41179,1.3198,1.26781,1.22781,1.24781,1.21981,1.20782,1.23581,1.19582,1.22781,1.19982,1.17982,1.20382,1.17582,1.17982,1.16782,1.16382,1.15582,1.15182,1.15982,1.14783,1.13183,1.14783,1.12383,1.15182,1.14383,1.13583,1.24781,1.24781,1.19582,1.2958,1.3118,1.3038,1.2958,1.2998,1.2998,1.3038,1.2978,1.2858,1.2818,1.27581,1.2838,1.2878,1.27981,1.3078,1.2898,1.27781,1.2878,1.27981,1.3018,1.2998,1.2938,1.2958,250,250,1.19582,1.21182,1.20382,1.18382,1.23581,1.19582,250,250,250,1.9777,1.9837,1.9777,1.9857,1.9917,2.09168,250,4.49132,4.48732,4.48332,4.48532,4.47732,4.47132,4.48532,4.47332,4.47532,4.46332,4.47332,4.47132,4.46332,4.49931,4.96124,5.04323,6.571,6.567,6.579,6.579,6.64299,7.55685,7.52685,7.56485,7.93079,250,250,8.70467,8.69668,8.69268,8.72067,8.72067,8.72067,8.73267,8.73667,8.76467,8.76267,8.76467,5.01924,5.01524,5.02723,5.02723,5.03123,5.05523,5.04323,5.07123,5.07123,5.07123,5.09722,5.01924,4.92325,4.68529,4.64729,4.66929,4.66529,4.68929,4.68729,4.70128,4.71728,4.70928,4.73928,4.74928,4.75128,4.76927,4.76327,4.79527,4.79527,4.80927,4.82926,4.83526,4.85326,4.86926,4.86726,4.88926,4.90725,4.92325,4.94325,4.95125,4.97124,5.00524,3.13952,3.11753,3.06953,3.03754,2.99154,2.96555,2.91156,2.87756,2.83757,2.22366,2.18967,2.16167,2.15367,2.15167,2.14167,2.17367,2.16767,2.16367,2.18567,2.32765,2.37964,2.44363,2.43163,2.41763,2.38564,2.37364,2.34164,2.30165,2.27565,2.26965,2.30565,2.31765,2.33764,2.34364,2.36164,2.37364,2.38364,2.40563,2.41163,2.41963,2.44163,2.45363,2.47362,2.48362,2.49162,2.51162,2.52961,2.55961,2.56561,2.57361,2.5996,2.6136,2.6396,2.6576,2.66959,2.69559,250,2.71959,7.15891,7.17491,7.2469,7.27889,7.37888,9.08662,9.06062,8.99063,8.90664,8.84865,8.76266,8.68668,8.67868,8.66668,8.66268,8.73067,8.81066,8.91064,9.01063,9.11461,9.2106,9.28259,9.43456,9.49056,9.62653,9.73252,9.8625,9.96848,10.0825,10.2244,10.3484,10.4724,10.6244,10.7524,10.9003,11.0623,11.2203,11.4623,11.6362,11.8122,11.9922,3.79342,3.78142,3.77343,3.76743,3.75143,3.73943,3.73743,3.71743,3.71143,3.70544,3.69144,3.68544,3.67344,3.67544,3.66144,3.63945,3.64145,3.62945,3.63345,3.61745,3.61345,3.61145,3.60345,3.60345,3.58545,3.58545,3.57746,3.55746,3.56946,3.56946,3.54746,3.55546,3.53346,3.54146,3.53346,3.52746,3.52146,3.50947,3.51547,3.50547,3.48947,3.50347,3.49947,3.50147,3.48547,3.47747,3.49147,3.47947,1.90571,1.90571,2.38764,2.37764,2.36764,3.47747,3.47747,3.45947,3.46147,3.46547,3.47747,3.45747,3.46747,3.48147,3.46747,3.47947,3.48147,3.46147,3.47947,3.47347,3.48147,3.47347,3.47147,2.38764,2.36364,1.90171,1.88771,1.88771,3.48547,3.59145,5.19121,4.08338,3.88341,3.69544,3.58345,4.18136,4.17536,3.2915,2.89156,2.83157,2.77958,2.79157,2.82357,2.81957,2.82557,2.84357,250,250,1.9677,2.0037,250,250,2.87956,2.86156,2.85956,2.87756,2.87356,2.89956,2.89956,2.89356,2.91356,2.90756,2.92955,4.41933,4.42333,4.44332,4.44332,4.47132,4.47532,1.43178,1.42778,1.37579,1.35979,1.3418,1.3298,1.3098,1.2858,1.2938,1.2818,1.25581,1.34779,1.44978,1.46778,1.47178,1.48177,1.49377,1.49177,1.51777,1.51377,1.49977,1.51977,1.52377,1.54376,1.54976,1.54376,1.55976,1.55976,1.57176,1.57576,1.57576,1.59176,1.59376,1.60975,1.61575,1.61975,1.62775,1.63775,1.65375,1.65175,1.66575,1.68174,1.68174,1.69774,1.70574,1.70574,1.73174,1.72774,1.75173,1.74973,1.76573,1.77573,1.78573,1.80573,1.81372,1.81372,1.83372,1.84572,1.86772,1.87571,1.88771,1.91171,1.92571,1.93371,1.9597,1.9637,1.9917,1.9857,2.01969,2.03169,2.03569,2.05969,2.07168,2.09368,2.11768,2.13168,2.15967,2.16967,2.19967,2.21566,2.23166,2.25966,2.28365,2.27765,2.25766,2.23566,2.23166,2.21166,2.21966,2.20766,2.19767,2.21566,2.24766,4.41933,4.39133,4.38333,4.37533,4.35134,4.32334,4.30334,4.29935,4.28335,4.29135,4.25935,4.24335,4.21536,4.21136,4.19536,4.19536,4.17137,4.15137,4.15137,4.11537,4.12737,4.09538,3.79542,3.79142,3.87541,3.9314,4.03139,4.45332,4.44332,3.04754,3.03554,3.05353,3.08753,2.98755,2.98755,3.21951,3.2715,3.07753,3.03954,3.03554,2.85357,2.86556,2.91156,2.90956,3.01554,3.03754,3.01754,3.00754,2.89556,2.90156,2.88556,3.02154,3.37349,3.33949,2.95155,2.92156,2.91956,2.91156,2.91156,2.91956,2.95155,2.99954,3.2595,3.65944,4.18736,4.18336,4.19536,4.19536,2.79557,2.93355,2.97955,3.01954,3.03554,3.05554,3.08753,3.08753,3.12552,3.12552,3.12952,3.15352,3.13952,3.17352,3.69944,3.82142,3.89141,250,2.19167,2.17967,2.13567,2.08368,2.04369,2.04369,1.9837,1.9677,1.92371,1.69174,1.64975,1.63375,1.62775,1.62775,1.62375,1.63375,1.63775,1.64175,1.65775,1.67774,1.68774,1.90371,1.9477,1.9837,1.9797,1.9877,1.9757,1.93171,1.9557,1.9437,1.9637,1.89171,1.78173,1.76173,1.75173,1.76573,1.74573,1.73974,1.76373,1.75973,1.77973,1.77373,1.75373,1.79573,4.64729,4.6193,4.63129,4.63929,4.65529,4.67529,4.70328,4.71928,4.70328,4.33934,250,4.11537,4.09938,4.07538,4.01739,3.98539,3.98339,3.9614,3.97539,4.01539,1.70574,1.67774,1.65975,1.66575,1.64775,1.63975,1.62175,1.59976,1.65175,1.68974,1.69974,1.69774,1.63975,1.64575,1.63175,1.63575,1.64975,1.62775,1.63975,1.64575,1.64375,1.65375,1.64975,1.65175,1.65575,1.65775,1.67574,1.67175,1.68974,1.68974,1.69974,1.70574,1.71774,1.72974,1.73574,1.76173,1.76973,1.78373,1.81972,1.89171,250,250,250,250,5.05923,5.16521,5.15122,5.10922,5.09722,5.05923,5.03123,5.00924,4.97724,4.95525,4.92925,4.90325,4.89126,4.85326,4.85126,4.81727,4.52731,4.51531,4.49132,4.48332,4.45932,4.43532,4.42333,4.39533,4.39333,4.37533,4.35734,4.33134,4.31334,4.30534,4.28735,4.32134,4.38733,4.43732,4.47332,4.44732,4.43332,4.42533,4.26935,3.82742,3.81542,3.79942,3.80142,3.77942,3.77543,3.76943,3.74143,3.73943,3.72143,3.71543,3.69544,3.67544,3.65944,3.64345,3.63945,3.65744,3.63945,3.63345,3.61545,3.62345,3.60345,3.60545,3.58145,3.57146,2.75158,2.77758,2.77158,2.74758,2.71159,2.70559,2.68359,2.69559,2.71359,2.73358,2.75958,2.75758,2.81357,2.82957,3.00354,3.18152,3.2755,3.2735,3.2635,3.2615,3.2595,3.23951,3.24951,3.24351,3.23151,3.23951,3.22351,2.99754,2.98355,2.98555,2.99154,2.97555,2.99954,2.99154,2.98155,2.99354,2.97155,2.97755,2.97355,2.97755,2.98355,2.98155,2.98954,2.98755,2.97555,2.98555,2.96555,2.99154,4.01739,4.02139,4.02339,3.72943,3.57946,3.44348,3.2695,3.2555,3.2695,3.2755,3.2695,3.2735,3.2915,3.2815,3.3015,3.3075,3.3095,3.31949,3.3155,3.33749,3.32749,3.32749,4.04538,4.04738,4.12537,3.9674,3.82942,3.69744,3.51147,3.46547,3.48147,3.41148,3.46947,4.21136,4.21936,4.21936,4.23136,4.24935,4.25135,4.26135,3.56346,3.50347,3.45147,3.40548,3.40348,3.38348,3.38748,3.38149,3.37749,3.39948,3.39948,3.43748,3.47347,3.49947,3.59145,3.85141,3.84541,4.47132,4.52331,4.54331,3.9334,4.5813,4.17736,4.5953,4.62929,2.25366,2.19966,2.18567,2.17967,2.19967,2.23166,250,3.41948,2.06369,2.09168,2.07168,2.06369,2.07168,1.9917,1.9917}; +// std::vector reference_scan_1 = {3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144}; + +std::vector reference_scan_1 ={3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144}; +std::vector target_scan_1 ={1.93771,1.93171,1.9437,1.9597,1.9537,1.9737,1.9777,1.9757,1.9937,1.9617,1.89771,1.87371,1.85772,1.86372,1.85572,1.86372,1.86172,1.86372,1.88371,1.89171,1.91971,4.73128,4.74128,4.76927,4.76927,4.78927,4.80327,4.81927,4.83526,4.85126,4.31534,4.23136,4.19536,4.16337,4.14137,4.13537,1.82572,1.79173,1.78373,1.75373,1.76173,1.74773,1.73974,1.72774,1.71574,1.71574,1.70974,1.70374,1.70974,1.70374,1.71174,1.70774,1.70374,1.71174,1.69974,1.71574,1.71374,1.70774,1.71774,1.71374,1.72374,1.72574,1.74773,1.75773,1.75373,1.78173,1.77773,1.77173,1.79573,1.79773,1.82972,1.83772,1.85572,1.87571,1.92571,1.9737,250,250,250,250,250,250,250,5.14722,5.2752,5.2692,5.2252,5.18921,5.17521,5.13322,5.12122,5.08523,5.04923,5.03123,5.00524,4.98324,4.95525,4.92525,4.91325,4.86926,4.86126,4.83926,4.81727,4.80527,4.77527,4.75528,4.67729,4.53931,4.54331,4.65929,4.65529,4.63529,4.6153,4.6033,4.5773,4.5633,4.55131,4.52131,4.52131,4.48732,3.9194,3.89541,3.88741,3.87941,3.85941,3.85941,3.83542,3.81542,3.81142,3.78742,3.78142,3.76343,3.74943,3.73343,3.70744,3.72343,3.72143,3.70344,3.70544,3.68344,3.68344,3.66744,3.65144,3.65544,2.85157,2.83557,2.81357,2.78358,2.77158,2.76358,2.76358,2.76358,2.79357,2.82157,2.79557,2.85557,2.89356,3.14552,3.2775,3.32349,3.32949,3.3075,3.2995,3.3055,3.2975,3.3055,3.2855,3.2795,3.2855,3.2675,3.04954,3.02354,3.01954,3.03354,3.01754,3.02554,3.01354,3.00354,3.02154,3.00954,3.01354,3.00354,3.00354,3.01954,2.99954,3.01154,3.00754,3.01354,3.00754,2.99154,3.01354,4.03339,4.02939,4.03539,3.82342,3.62345,3.74343,3.2695,3.2815,3.2735,3.2875,3.2895,3.2875,3.2935,3.2955,3.3115,3.3135,3.3135,3.32949,3.32149,3.34349,3.33749,4.12537,4.10737,4.07138,4.05538,4.03738,3.85941,3.44947,3.42548,3.53146,3.44348,3.43548,3.48547,4.19136,4.20336,4.21336,3.59145,3.59145,3.59145,3.59145,3.59145,3.47147,3.45147,3.42348,3.45147,3.42348,3.34749,3.34749,3.44348,250,250,250,2.95955,2.93355,2.93555,2.92955,2.91556,2.88756,2.88756,2.92355,4.50331,2.19167,2.16767,2.13368,2.04369,1.91971,1.85972,1.83172,1.85972,1.79173,1.76773,1.73174,1.79173,1.84772,1.79173,1.79173,1.86372,1.79173,1.86372,1.89171,1.84772,1.89171,1.87571,1.89171,1.84372,1.85572,1.66775,1.51577,1.45978,1.47977,1.47178,1.42778,1.43178,1.41179,1.40779,1.37979,1.39579,1.39579,1.37579,1.35579,1.35979,1.3078,1.3398,1.3278,1.39579,1.39579,1.39579,250,1.39579,1.43578,1.44778,1.39579,1.45978,1.41578,1.42778,1.43978,1.41179,1.39579,1.36779,1.37979,1.37179,1.3158,1.3318,1.21182,1.17582,1.15582,1.14783,1.11183,1.12383,1.11183,1.12183,1.09783,1.10783,1.10183,1.08783,1.08783,1.08384,1.08384,1.07384,1.06784,1.06984,1.05784,1.06184,1.05584,1.04784,1.05784,1.03784,1.05184,1.04184,1.03584,1.05584,1.01984,1.02384,1.03584,1.01585,1.00785,1.00785,1.09583,250,250,1.19582,250,1.19582,1.17982,1.19582,1.17782,1.17582,1.18782,1.17582,1.17182,1.17382,1.16582,1.17982,1.16782,1.13583,1.12783,1.09583,1.12783,1.08384,1.09583,1.08384,1.07584,1.06384,1.05184,1.03384,1.05384,1.05184,1.04784,1.04184,1.05584,1.05584,1.05984,1.06784,1.12383,1.84772,1.83972,1.85172,1.85972,1.84772,1.85572,1.85972,1.89171,250,4.36734,4.36334,4.37333,4.35534,4.36534,4.36933,4.35934,4.36733,4.35134,4.34734,4.36134,4.33734,4.34934,4.33734,4.35334,4.34134,4.77927,4.85926,4.90325,6.45502,6.45902,6.44702,6.45102,8.5347,250,7.61484,7.59484,8.5287,8.5347,8.5427,8.5447,8.5527,8.5627,8.5627,8.5727,8.57869,8.58269,8.60069,8.59469,8.61669,8.62469,4.86726,4.88326,4.88926,4.89725,4.89925,4.90725,4.92325,4.91525,4.94125,4.93525,4.94725,4.90125,4.79127,4.54931,4.52131,4.51731,4.54331,4.53731,4.55331,4.5633,4.5693,4.6033,4.6073,4.6193,4.6233,4.64729,4.65329,4.66729,4.68129,4.69528,4.72328,4.72128,4.73528,4.75728,4.75728,4.79327,4.79927,4.80727,4.82527,4.83126,4.85726,4.87526,4.89126,3.02154,2.99954,2.98155,2.91556,2.87756,2.85357,2.80757,2.78958,2.75158,2.70359,2.67559,2.6176,2.06569,2.04769,2.03569,2.04369,2.03369,2.03969,2.04969,2.06169,2.10168,2.22166,2.30765,2.32165,2.31165,2.29765,2.25766,2.24566,2.21766,2.17367,2.17767,2.16567,2.19967,2.21966,2.22566,2.24366,2.23966,2.27165,2.27965,2.29365,2.31965,2.32765,2.35564,2.35764,2.36564,2.39164,2.40163,2.41763,2.43363,2.44563,2.47162,2.47962,2.51762,2.52162,2.54161,2.56561,2.57961,2.6056,2.59161,2.6076,7.2149,7.2469,7.30689,9.00663,8.91864,8.85865,8.77066,8.71467,8.63868,8.57469,8.69468,8.5327,8.62469,8.70867,8.80066,8.90464,8.99663,9.10061,9.2046,9.29259,9.43456,9.51655,9.66053,9.75451,9.88649,10.0145,10.1425,10.2784,10.4224,10.5424,10.7024,10.8423,11.0103,11.1783,11.4323,11.6122,11.7702,11.9642,12.0702,12.0462,3.74743,3.70744,3.70944,3.70744,3.69144,3.68144,3.66544,3.66944,3.64944,3.64544,3.64744,3.62345,3.62745,3.61145,3.60545,3.60545,3.58945,3.59345,3.57346,3.57346,3.56346,3.54146,3.55546,3.53746,3.53746,3.54146,3.53346,3.52746,3.51746,3.50947,3.51147,3.49547,3.50347,3.49547,3.48747,3.48947,3.47547,3.47947,3.47347,3.46147,3.46947,3.46147,3.46947,3.46347,3.45147,3.46347,3.44548,3.45347,3.45347,3.44748,3.45147,250,1.86972,1.87172,1.86772,3.43548,3.43348,3.46547,3.44548,3.43548,3.44947,3.44348,3.46747,3.44947,3.43748,3.44747,3.44947,3.45547,3.45547,3.45547,3.46347,3.45747,3.47147,2.36764,2.37164,2.35964,3.47547,1.89771,1.88971,1.87971,1.91171,3.68144,3.59345,4.16936,4.18736,4.20936,3.2695,250,2.79158,2.77758,2.78958,2.81557,2.82357,2.81957,2.82357,2.85357,2.84357,250,250,250,2.09168,2.04369,250,250,2.89356,2.89956,2.89956,2.91556,2.91956,2.92156,2.93755,2.93555,4.46932,4.47532,4.48332,4.50331,4.51331,4.53331,4.54331,4.55131,4.5813,4.54931,4.52131,1.49577,1.45978,1.43978,1.40379,1.38379,1.37979,1.35579,1.3318,1.3218,1.3318,1.3138,1.3018,1.3118,1.49977,1.52577,1.52377,1.52777,1.53977,1.54376,1.56376,1.57176,1.56376,1.58776,1.58376,1.60376,1.60376,1.60975,1.62575,1.62975,1.64575,1.64575,1.65375,1.66775,1.66575,1.68774,1.70174,1.70374,1.71374,1.72574,1.74373,1.74573,1.75573,1.78573,1.78973,1.80173,1.81772,1.81772,1.84172,1.84972,1.86772,1.87371,1.88971,1.90971,1.91371,1.9457,1.9617,1.9737,1.9897,1.9997,2.02369,2.03169,2.04569,2.07968,2.08768,2.12168,2.13368,2.13967,2.17967,2.18767,2.21966,2.23966,2.24366,2.27965,2.29965,2.34364,2.36164,2.37564,2.34364,2.32765,2.33564,2.31165,2.29165,2.29165,2.29565,2.29165,2.29165,4.49132,4.46332,4.43532,4.43133,4.41533,4.39933,4.38333,4.36334,4.33534,4.34734,4.30334,4.29535,4.27935,4.28335,4.29135,4.24735,4.26735,4.22736,4.19936,4.08738,3.87141,3.9594,4.03139,4.23136,4.55131,4.54531,4.52931,4.51731,4.52131,4.50531,4.50131,4.48932,4.47532,4.46932,3.2755,3.21951,3.21151,3.20751,3.24351,3.24151,3.11153,3.11553,3.12552,3.13152,3.09153,3.01954,2.94755,2.96355,3.03554,3.04354,3.09753,3.09953,3.09953,3.07753,3.08953,3.07553,3.01954,3.09353,3.14352,3.09153,3.46547,3.13152,3.09953,3.09153,3.09353,3.13152,3.17352,3.76143,4.31734,4.31934,4.30934,4.31734,4.30934,4.32534,4.31334,4.30934,4.30934,3.9354,3.9174,3.61945,3.9314,3.9394,3.9374,3.9494,3.9454,3.9394,3.9654,3.9454,3.99139,2.09168,2.17167,2.19167,2.08768,2.08768,2.03169,1.88771,1.87971,1.85372,1.83372,1.78973,1.74973,1.74773,1.73774,1.75173,1.74573,1.74773,1.76173,1.75773,1.78173,1.79173}; diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index b7e4ab0..5732438 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -236,6 +236,7 @@ TEST(loop_closure_falko, TestMatch2) param.neigh_b_ = 0.1; param.b_ratio_ = 4; param.enable_subbeam_ = false; + // laser_params.angle_step_ = 0.00701248; -- GitLab From 04945cc99200e55bfe7f2d7f22ab43d18639be40 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 27 Jul 2021 00:07:15 +0200 Subject: [PATCH 094/100] added scan area and covariance extraction. Solved bug in LaserScan::getXY --- src/laser_scan.cpp | 3 +- src/loop_closure_falko.h | 131 ++++++++++++++++++++++++------ src/scene_base.h | 17 ++-- test/gtest_loop_closure_falko.cpp | 30 ++++--- 4 files changed, 136 insertions(+), 45 deletions(-) diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp index 3747de3..398e859 100644 --- a/src/laser_scan.cpp +++ b/src/laser_scan.cpp @@ -102,14 +102,13 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _ { //check raw range integrity //invalid range - if (!isValidRange(ranges_raw_[ii], _scan_params)) + if (isValidRange(ranges_raw_[ii], _scan_params)) { //set as valid range ranges_[ii] = ranges_raw_[ii]; //transform the laser hit from polar to 3D euclidean homogeneous point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1; - //apply device mounting point calibration (p_r = T * p_l) point_ref = _device_T * point_laser; diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index a4a5343..127b392 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -135,14 +135,14 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract { auto new_scene = std::make_shared>(); auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); - // Extract keypoints + // Extract KEYPOINTS std::vector keypoints_list; extract(*scan_falko, keypoints_list); double angle_min = _scan_params.angle_min_; double angle_step = _scan_params.angle_step_; - // Compute max_dist + // Compute KEYPOINTS max_dist new_scene->max_distance_ = 0; for (int i = 0; i < keypoints_list.size(); i++) for (int j = 0; j < keypoints_list.size(); j++) @@ -154,7 +154,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract new_scene->max_distance_ = distance; } - // discard too close by kp + // discard too close by KEYPOINTS for (int i = 0; i < keypoints_list.size(); i++) { int repeated = 0; @@ -163,7 +163,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]); double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]); double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist)); - if (distance < 0.05) + if (distance < 0.03) { repeated = 1; } @@ -174,12 +174,12 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract } } - // Compute descriptors + // Compute DESCRIPTORS extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); std::vector descriptors_list_rotated; extractor_.compute(*scan_falko, new_scene->keypoints_list_, descriptors_list_rotated); - // Compute Scene mid point, angle for each keypoint and rotate descriptors + // Compute KEYPOINTS Scene mid point, angle for each keypoint and rotate descriptors Eigen::Vector2d mid_point(0, 0); std::vector angle_rotation; for (int i = 0; i < new_scene->keypoints_list_.size(); i++) @@ -187,14 +187,13 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1]; angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index); - // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i]; double orientation = angle_rotation[i]; descriptors_list_rotated[i].rotate(orientation); new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]); } new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); - // Compute Scene Area and Perimeter + // Compute KEYPOINTS Area and Perimeter int n = 3; double X[n]; double Y[n]; @@ -225,9 +224,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract X[0] = new_scene->mid_point_[0]; Y[0] = new_scene->mid_point_[1]; - // X[0] = 0.0; - // Y[0] = 0.0; - X[1] = new_scene->keypoints_list_[i].point[0]; Y[1] = new_scene->keypoints_list_[i].point[1]; @@ -250,7 +246,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract new_scene->perimeter_ = new_scene->perimeter_ + hypot(dist_between_two_kp[0], dist_between_two_kp[1]); } - // Compue Scene linear regresion and initial angle + // Compue KEYPOINTS Scene linear regresion and initial angle double ss_xy = 0; double ss_xx = 0; for (int i = 0; i <= new_scene->keypoints_list_.size(); i++) @@ -261,16 +257,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]); } double b_1 = ss_xy / ss_xx; - // double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0]; - - // new_scene->regressor_coefs.push_back(b_0); - // new_scene->regressor_coefs.push_back(b_1); double initial_angle = -atan(b_1); - // double inital_angle_inv = initial_angle - M_PI; - - // rotate keypoints + // rotate KEYPOINTS for (int i = 0; i < new_scene->keypoints_list_.size(); i++) { falkolib::FALKO keypoint_mid; @@ -301,6 +291,101 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract new_scene->keypoints_list_transl_rot_.push_back(keypoint_rot_trans); } + // Compute KEYPOINTS covariance matrix + + float sumXX = 0; + float sumXY = 0; + float sumYY = 0; + + n = new_scene->keypoints_list_.size(); + auto mean = new_scene->mid_point_; + for (int i = 0; i < n; i++) + { + sumXX += (new_scene->keypoints_list_[i].point[0] - mean[0]) * + (new_scene->keypoints_list_[i].point[0] - mean[0]); + sumXY += (new_scene->keypoints_list_[i].point[0] - mean[0]) * + (new_scene->keypoints_list_[i].point[1] - mean[1]); + sumYY += (new_scene->keypoints_list_[i].point[1] - mean[1]) * + (new_scene->keypoints_list_[i].point[1] - mean[1]); + } + double covXX = sumXX / n; + double covXY = sumXY / n; + double covYY = sumYY / n; + + Eigen::Matrix A; + A << covXX, covXY, covXY, covYY; + + // Compute KEYPOINTS eigenvalues + Eigen::EigenSolver> s(A); + double eig1 = s.eigenvalues()(0).real(); + double eig2 = s.eigenvalues()(1).real(); + new_scene->eigenvalues_kp_.push_back(eig1); + new_scene->eigenvalues_kp_.push_back(eig2); + + // Compute SCAN mid point + LaserScan scan = _scan; + scan.ranges2xy(_scan_params); + Eigen::Vector2d mid_point_scan(0, 0); + + for (int i = 0; i < scan.points_.size() / 3; i++) + { + mid_point_scan[0] = mid_point_scan[0] + scan.points_(0, i); + mid_point_scan[1] = mid_point_scan[1] + scan.points_(1, i); + } + new_scene->mid_point_scan_ = mid_point_scan / (double)scan.points_.size() / 3; + + // Compute SCAN Area + n = 3; + new_scene->area_scan_ = 0.0; + + int points_size = scan.points_.size() / 3; + for (int i = 0; i < points_size; i++) + { + X[0] = 0.0; + Y[0] = 0.0; + + X[1] = scan.points_(0, i); + Y[1] = scan.points_(1, i); + + if (i < points_size - 1) // Proceed until final keypoint + { + X[2] = scan.points_(0, i + 1); + Y[2] = scan.points_(1, i + 1); + } + else // if you arrived to the final keypoint then use inital keypoint + { + X[2] = scan.points_(0, 0); + Y[2] = scan.points_(1, 0); + } + new_scene->area_scan_ = new_scene->area_scan_ + (double)triangleArea(X, Y, n); + } + // Compute SCAN covariance matrix + + sumXX = 0; + sumXY = 0; + sumYY = 0; + + mean = new_scene->mid_point_scan_; + for (int i = 0; i < points_size; i++) + { + sumXX += (scan.points_(0, i) - mean[0]) * (scan.points_(0, i) - mean[0]); + sumXY += (scan.points_(0, i) - mean[0]) * (scan.points_(1, i) - mean[1]); + sumYY += (scan.points_(1, i) - mean[1]) * (scan.points_(1, i) - mean[1]); + } + covXX = sumXX / points_size; + covXY = sumXY / points_size; + covYY = sumYY / points_size; + + Eigen::Matrix A_scan; + A_scan << covXX, covXY, covXY, covYY; + + // Compute SCAN eigenvalues + Eigen::EigenSolver> s_scan(A_scan); + eig1 = s_scan.eigenvalues()(0).real(); + eig2 = s_scan.eigenvalues()(1).real(); + new_scene->eigenvalues_scan_.push_back(eig1); + new_scene->eigenvalues_scan_.push_back(eig2); + return new_scene; } @@ -309,10 +394,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract **/ laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params) { - double field_of_view = _scan_params.angle_max_-_scan_params.angle_min_; - auto scan_falko = std::make_shared(_scan_params.angle_min_, field_of_view, - _scan.ranges_raw_.size()); - + double field_of_view = _scan_params.angle_max_ - _scan_params.angle_min_; + auto scan_falko = + std::make_shared(_scan_params.angle_min_, field_of_view, _scan.ranges_raw_.size()); + std::vector double_ranges(_scan.ranges_raw_.begin(), _scan.ranges_raw_.end()); for (int i = 0; i < double_ranges.size(); i++) diff --git a/src/scene_base.h b/src/scene_base.h index 740abf3..40651e4 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -15,14 +15,17 @@ namespace laserscanutils { struct SceneBase { - int id; - double area_; - double perimeter_; - double max_distance_; - double mean_distance_; - Eigen::Vector2d mid_point_; + int id; + double area_; + double area_scan_; + double perimeter_; + double max_distance_; + double mean_distance_; + Eigen::Vector2d mid_point_; + Eigen::Vector2d mid_point_scan_; std::vector regressor_coefs; - + std::vector eigenvalues_kp_; + std::vector eigenvalues_scan_; }; typedef std::shared_ptr sceneBasePtr; diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 5732438..eb0f9cf 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -16,6 +16,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) LaserScanParams laser_params; laser_params.angle_min_ = 0; laser_params.angle_max_ = 2.0 * M_PI; + laser_params.angle_step_ = 0.00701248; + laser_params.range_max_ = 50; for (int i = 0; i < scan_size; i++) { scan.ranges_raw_.push_back(testRanges1[i]); @@ -33,10 +35,10 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ASSERT_EQ(firstPoint, 250); // Test extractScene2 - auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, laser_params)); - auto new_scene2 = std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); - int detectedKeypoints = new_scene->keypoints_list_.size(); - int detectedDescriptors = new_scene->descriptors_list_.size(); + auto new_scene = std::static_pointer_cast>(loop_cl_falko.extractScene(scan, + laser_params)); auto new_scene2 = + std::static_pointer_cast>(loop_cl_falko.extractScene(scan2, laser_params)); int detectedKeypoints + = new_scene->keypoints_list_.size(); int detectedDescriptors = new_scene->descriptors_list_.size(); ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedDescriptors, 18); @@ -68,6 +70,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation) laser_params.angle_min_ = 0; laser_params.angle_max_ = 2.0 * M_PI; laser_params.angle_step_ = laser_params.angle_max_ / 1440; + laser_params.range_max_ = 50; for (int i = 0; i < scan_size; i++) { @@ -112,7 +115,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation) asso_number = j; } } - std::cout << "pair : " << i << " , " << asso_number << " , distance : " << min_dist << std::endl; + // std::cout << "pair : " << i << " , " << asso_number << " , distance : " << min_dist << std::endl; } // for (int i = 0; i < desc_1.size(); i++) @@ -178,6 +181,7 @@ TEST(loop_closure_falko, TestMatch) laser_params.angle_min_ = 0; laser_params.angle_max_ = 2.0 * M_PI; laser_params.angle_step_ = laser_params.angle_max_ / 1440; + laser_params.range_max_ = 50; for (int i = 0; i < scan_size; i++) { @@ -219,9 +223,10 @@ TEST(loop_closure_falko, TestMatch2) laser_params.angle_min_ = 0; laser_params.angle_max_ = 2.0 * M_PI; laser_params.angle_step_ = laser_params.angle_max_ / 1440; + laser_params.range_max_ = 100; // ** TEST WITH TARGET AND REFERENCE SCENE - std::cout << "scan size : " << target_scan_1.size() << std::endl; + // std::cout << "scan size : " << target_scan_1.size() << std::endl; for (int i = 0; i < target_scan_1.size(); i++) { scan_target.ranges_raw_.push_back(target_scan_1[i]); @@ -236,7 +241,6 @@ TEST(loop_closure_falko, TestMatch2) param.neigh_b_ = 0.1; param.b_ratio_ = 4; param.enable_subbeam_ = false; - // laser_params.angle_step_ = 0.00701248; @@ -246,18 +250,18 @@ TEST(loop_closure_falko, TestMatch2) auto new_scene_reference = std::static_pointer_cast>(loop_cl_falko_2.extractScene(scan_ref, laser_params)); - std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl; - std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl; + // std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl; + // std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl; auto match_r_t = loop_cl_falko_2.matchScene(new_scene_reference, new_scene_target); for (int i = 0; i < match_r_t->associations.size(); i++) if (match_r_t->associations[i].second != -1) { - std::cout << "id first : " << match_r_t->associations[i].first << std::endl; - std::cout << "id second : " << match_r_t->associations[i].second << std::endl; + // std::cout << "id first : " << match_r_t->associations[i].first << std::endl; + // std::cout << "id second : " << match_r_t->associations[i].second << std::endl; } - std::cout << "transform : " << match_r_t->transform_vector.transpose() << std::endl; + // std::cout << "transform : " << match_r_t->transform_vector.transpose() << std::endl; auto key_ref = new_scene_reference->keypoints_list_; auto key_target = new_scene_target->keypoints_list_; @@ -300,7 +304,7 @@ TEST(loop_closure_falko, TestMatch2) plt::plot({x_ref[i], x_target[i]}, {y_ref[i], y_target[i]}, "g"); } - plt::show(); + // plt::show(); } int main(int argc, char **argv) -- GitLab From b5a2a1c61cd7f7c9de7c4f2347682cf6259a2ba4 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Thu, 2 Sep 2021 23:20:16 +0200 Subject: [PATCH 095/100] solved bug for repeated scores --- src/loop_closure_base.h | 4 ++++ src/loop_closure_falko.h | 52 +++++++++++++++++++++------------------- 2 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index b6a815f..b9fcd99 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -54,6 +54,10 @@ class LoopClosureBase2d for (auto ref_scene : _l_scenes) { auto match = matchScene(ref_scene, _new_scene); + + while (matchings.find(match->score)!=matchings.end()) + match->score+=0.001; + matchings.emplace(match->score, match); } return matchings; diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 127b392..23a5def 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -319,11 +319,20 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract Eigen::EigenSolver> s(A); double eig1 = s.eigenvalues()(0).real(); double eig2 = s.eigenvalues()(1).real(); - new_scene->eigenvalues_kp_.push_back(eig1); - new_scene->eigenvalues_kp_.push_back(eig2); + // The greater eigenvalue will be the first one + if (eig1 > eig2) + { + new_scene->eigenvalues_kp_.push_back(eig1); + new_scene->eigenvalues_kp_.push_back(eig2); + } + else + { + new_scene->eigenvalues_kp_.push_back(eig2); + new_scene->eigenvalues_kp_.push_back(eig1); + } // Compute SCAN mid point - LaserScan scan = _scan; + LaserScan scan = _scan; scan.ranges2xy(_scan_params); Eigen::Vector2d mid_point_scan(0, 0); @@ -383,8 +392,18 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract Eigen::EigenSolver> s_scan(A_scan); eig1 = s_scan.eigenvalues()(0).real(); eig2 = s_scan.eigenvalues()(1).real(); - new_scene->eigenvalues_scan_.push_back(eig1); - new_scene->eigenvalues_scan_.push_back(eig2); + + // The greater eigenvalue will be the first one + if (eig1 > eig2) + { + new_scene->eigenvalues_scan_.push_back(eig1); + new_scene->eigenvalues_scan_.push_back(eig2); + } + else + { + new_scene->eigenvalues_scan_.push_back(eig2); + new_scene->eigenvalues_scan_.push_back(eig1); + } return new_scene; } @@ -426,48 +445,31 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract if (use_descriptors_ == 0) { - // matching_number = matcher_.match(scene_1_falko->keypoints_list_transl_rot_, - // scene_2_falko->keypoints_list_transl_rot_, asso_nn); - matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn); } else if (use_descriptors_ == 1) { - // matching_number = matcher_.match( - // scene_1_falko->keypoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, - // scene_2_falko->keypoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn); - matching_number = matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated, scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn); } - // std::cout << "laserscanutils.LoopClosureFalko.matchScene -> num of skp matched : " << matching_number << - // std::endl; - auto new_match = std::make_shared(); new_match->keypoints_number_match = matching_number; - // if (matching_number >= keypoints_number_th_ - 2) - // { + new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform); - // } - // else - // { - // new_match->match = false; - // } new_match->associations = asso_nn; new_match->scene_1 = _scene_1; new_match->scene_2 = _scene_2; - // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), - // scene_2_falko->keypoints_list_.size()); - new_match->transform_vector.head(2) = new_match->transform.translation(); new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); + std::cout << "laserscanutils::matching_number : " << matching_number << std::endl; + new_match->score = (double)matching_number; return new_match; -- GitLab From b30827b646b5131dfc8fe65f141886ee1afbc4d5 Mon Sep 17 00:00:00 2001 From: Sergi Pujol Date: Tue, 19 Oct 2021 09:50:24 +0200 Subject: [PATCH 096/100] assert modified --- test/gtest_loop_closure_falko.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index eb0f9cf..be5197e 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -57,7 +57,6 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ASSERT_EQ(best_match->match, true); ASSERT_EQ(best_match->scene_1, new_scene); ASSERT_EQ(best_match->scene_2, new_scene); - ASSERT_EQ(best_match->score, 1.8); } TEST(loop_closure_falko, TestDescriptorsRotation) -- GitLab From 9941d4fc37026cb55b8edf9863c8607ef42b70dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Tue, 19 Oct 2021 15:39:17 +0200 Subject: [PATCH 097/100] removed falko as git module --- .gitmodules | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 .gitmodules diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 3d61e7a..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "deps/falkolib"] - path = deps/falkolib - url = https://gitlab.iri.upc.edu/mobile_robotics/falkolib -- GitLab From d1ed6e849c6f518e926d9635f143c19f279cce65 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Tue, 19 Oct 2021 16:04:45 +0200 Subject: [PATCH 098/100] Update README.md --- README.md | 53 +++++++++++++++++------------------------------------ 1 file changed, 17 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index d929a07..f9e8baa 100644 --- a/README.md +++ b/README.md @@ -4,22 +4,22 @@ A C++ Toolbox with utilities for 2D laser scan processing, made at IRI (www.iri. Clone the repository in your chosen directory. In a terminal: -``` +```sh cd git clone ``` Create a directory to build all the source files: -``` -$ mkdir build -$ cd build +```sh +mkdir build +cd build ``` Compile the source files: -``` -$ cmake .. -$ make +```sh +cmake .. +make ``` ## Optional dependencies @@ -27,7 +27,7 @@ $ make **Canonical Scan Matching - CSM** To enable the compilation of iterative closest point algorithms, [CSM](https://github.com/AndreaCensi/csm) should be installed. Clone, compile and install: -``` +```sh cd git clone https://github.com/AndreaCensi/csm.git cd csm @@ -37,46 +37,27 @@ sudo make install ``` CSM depends on GSL that you probably have installed already. To install it: -``` +```sh sudo apt install libgsl-dev ``` **FALKO Library - falkolib** -To enbale scan matching for loop closure purposes. The library is added as submodule in the deps folder. In order to initialize the submodule use the following commands: -``` +Loop closure search is implemented using FALKO Library. In order to download, compile and install it: +```sh cd -git submodule init -git submodule update -``` -In order to compile and install the repository: -``` -cd -cd deps/openslam_falkolib/ +git clone https://gitlab.iri.upc.edu/mobile_robotics/falkolib.git +cd falkolib mkdir build cd build cmake .. make +sudo make install ``` -You can also install the library into a system directory. To change the install directory you must set cmake environment -variable ${CMAKE_INSTALL_PREFIX} (e.g. using command "ccmake .." -before calling "cmake .."). +variable `${CMAKE_INSTALL_PREFIX}` (e.g. using command `ccmake ..` +before calling `cmake ..`). Its default value on UNIX-like/Linux systems is "/usr/local". -After compiling library falkolib, run the command: -``` -sudo make install -``` -The command "sudo" is required only if ${CMAKE_INSTALL_PREFIX} +The `sudo` is required only if `${CMAKE_INSTALL_PREFIX}` is a system diretory managed by administrator user root. -Such command copies: -- header files of ${falkolib_ROOT}/include/falkolib to - ${CMAKE_INSTALL_PREFIX}/include/falkolib/ -- library files ${falkolib_ROOT}/lib/libfalkolib.a to - ${CMAKE_INSTALL_PREFIX}/lib/ -- cmake script ${falkolib_ROOT}/cmake_modules/falkolibConfig.cmake to - ${CMAKE_INSTALL_PREFIX}/share/falkolib/ - - - -- GitLab From e98599c6285251f173b34d2acae7023063485093 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Tue, 19 Oct 2021 16:47:10 +0200 Subject: [PATCH 099/100] removed git submodules and find falko quiet --- deps/falkolib | 1 - src/CMakeLists.txt | 23 +++++++++-------------- 2 files changed, 9 insertions(+), 15 deletions(-) delete mode 160000 deps/falkolib diff --git a/deps/falkolib b/deps/falkolib deleted file mode 160000 index 8f341e9..0000000 --- a/deps/falkolib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8f341e9f1739c445cd8352586d514e2e49e25718 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fee4770..d7d87c3 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -12,13 +12,7 @@ ENDIF(faramotics_FOUND) FIND_PACKAGE(Eigen3 3.3 REQUIRED) FIND_PACKAGE(csm QUIET) - -find_package(falkolib REQUIRED) -message(STATUS "falkolib_FOUND ${falkolib_FOUND}") -message(STATUS "falkolib_INCLUDE_DIRS ${falkolib_INCLUDE_DIRS}") -message(STATUS "falkolib_LIBRARY_DIRS ${falkolib_LIBRARY_DIRS}") -message(STATUS "falkolib_LIBRARIES ${falkolib_LIBRARIES}") - +FIND_PACKAGE(falkolib QUIET) #include directories INCLUDE_DIRECTORIES(. /usr/local/include) @@ -29,12 +23,15 @@ ENDIF(csm_FOUND) IF(faramotics_FOUND) INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS}) ENDIF(faramotics_FOUND) - -if(${falkolib_FOUND}) +IF(falkolib_FOUND) + MESSAGE(STATUS "FALKO lib found:") + MESSAGE(STATUS " falkolib_INCLUDE_DIRS ${falkolib_INCLUDE_DIRS}") + MESSAGE(STATUS " falkolib_LIBRARY_DIRS ${falkolib_LIBRARY_DIRS}") + MESSAGE(STATUS " falkolib_LIBRARIES ${falkolib_LIBRARIES}") add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) - include_directories(${falkolib_INCLUDE_DIRS}) - link_directories(${falkolib_LIBRARY_DIRS}) -endif() + INCLUDE_DIRECTORIES(${falkolib_INCLUDE_DIRS}) + LINK_DIRECTORIES(${falkolib_LIBRARY_DIRS}) +ENDIF(falkolib_FOUND) #headers SET(HDRS_BASE @@ -113,8 +110,6 @@ IF(falkolib_FOUND) target_link_libraries(${PROJECT_NAME} ${falkolib_LIBRARY}) ENDIF(falkolib_FOUND) - - # Examples IF(BUILD_DEMOS) MESSAGE("Building examples.") -- GitLab From e74959dcc22fd3996867621b8c6a8f3ebfc043d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Tue, 19 Oct 2021 16:48:50 +0200 Subject: [PATCH 100/100] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f9e8baa..b9bebbc 100644 --- a/README.md +++ b/README.md @@ -43,7 +43,7 @@ sudo apt install libgsl-dev **FALKO Library - falkolib** -Loop closure search is implemented using FALKO Library. In order to download, compile and install it: +Loop closure search is implemented using [FALKO Library](https://gitlab.iri.upc.edu/mobile_robotics/falkolib). In order to download, compile and install it: ```sh cd git clone https://gitlab.iri.upc.edu/mobile_robotics/falkolib.git -- GitLab