From b5a2a1c61cd7f7c9de7c4f2347682cf6259a2ba4 Mon Sep 17 00:00:00 2001
From: Sergi Pujol <spujol@iri.upc.edu>
Date: Thu, 2 Sep 2021 23:20:16 +0200
Subject: [PATCH] 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<Eigen::Matrix<double, 2, 2>> 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<Eigen::Matrix<double, 2, 2>> 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<MatchLoopClosureScene>();
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