diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index ba3c7f94bd2bd9b6b8e7a1033854ad7642380238..64b4b5a8dcb243a469196fdd90534d2826a47ba6 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<double> 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<double> doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end()); + scanFALKO.fromRanges(doubleRanges); + //Compute descriptors std::vector<falkolib::FALKO> keypointSet2; - extract(scan, keypointSet2); + extract(scanFALKO, keypointSet2); //Compute descriptors std::vector <falkolib::BSC> 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 0e355c1a4a319b0efc967bfaedb5d5f7e10678eb..ffcff43f9cfe4f9365ad06589ec65c50d7786828 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -11,6 +11,11 @@ #include <iostream> #include <fstream> +/************************** + * 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 8ebac1ea71f0e2c592459bf24e5d83318514c4f1..4a969bd798a33c0370acd0202ebb1663123913eb 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<scanSize; i++) { + scan.ranges_raw_.push_back(testRanges1[i]); + } CornerFalko2d cornerMatching; - cornerMatching.storeCorners(scan1, scanInterval); + cornerMatching.storeCorners(scan, laserParams, scanInterval); std::cout << "num keypoints1 extracted: " << cornerMatching.lastKeypointSet.size() << std::endl; - cornerMatching.findLoopClosure(scan1); + cornerMatching.findLoopClosure(scan, laserParams); std::cout << "matching number : " << cornerMatching.matchingNumber << std::endl; - }