diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index 8f1e6b7e41075bd153c4e64c230f0448f2400b09..deebc2d5ffce8adef760fc9175a2745dc69182f7 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<keypointSets.size();i++){ + for (int i=0; i<rows;i++){ + setDistanceThreshold(0.1); std::vector<std::pair<int, int> > 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 079db34fee63338d95291af3be3ae278024e96c9..02c0474081b9625db69a07453f5cd44e35d01fde 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -77,6 +77,8 @@ public: std::vector<std::vector<falkolib::BSC>> descriptorSets; std::vector<falkolib::BSC>lastDescriptorSet; + int matchingNumber; + int scanNumber=0; }; diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index cda359cc0584768cef03e5ad1ad09d35d18d78a4..2b94b984ff5055dd53a605463bd410cfeaba4803 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; + }