From e6ef7245738c8246ad6814b287207f6f427d5b66 Mon Sep 17 00:00:00 2001
From: Sergi Pujol <sergi.pujol.badell@estudiantat.upc.edu>
Date: Mon, 1 Feb 2021 15:45:10 +0100
Subject: [PATCH] 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<rows;i++){
std::vector<std::pair<int, int> > 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