solved bug for repeated scores

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;

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;