adapted to add matcher aht

src/loop_closure_falko.h

@@ -36,6 +36,8 @@
 #include <falkolib/Matching/AHTMatcher.h>
 #include <falkolib/Matching/NNMatcher.h>
 
+using namespace falkolib;
+
 namespace laserscanutils {
 
 typedef falkolib::BSC bsc;
@@ -66,6 +68,14 @@ struct ParameterLoopClosureFalko {
   int match_type = 1; // match_type=1-> uses keypoints and descriptors for
                       // matching. match_type=2-> uses only keypoints for
                       // matching
+
+  // aht matcher
+  double xRes_=0.1;
+  double yRes_=0.1;
+  double thetaRes_=0.04;
+  double xAbsMax_=5;
+  double yAbsMax_=5; 
+  double thetaAbsMax_=1.57;
 };
 
 /** \brief A base class for loop closure using falko library
@@ -81,14 +91,14 @@ public:
 
   Extr extractor_;
   M<falkolib::FALKO, D> matcher_;
-  M<D, D> matcher_desc_;
+  //M<D, D> matcher_desc_;
 
-  /** \brief Constructor
+  /** \brief Constructor for nn_matcher
   **/
   LoopClosureFalko(ParameterLoopClosureFalko _param)
     : LoopClosureBase2d(), falkolib::FALKOExtractor(),
       extractor_(_param.circularSectorNumber_, _param.radialRingNumber_),
-      matcher_(), matcher_desc_() {
+      matcher_() {
     // FALKO Extractor Parameters
     setMinExtractionRange(_param.min_extraction_range_);
     setMaxExtractionRange(_param.max_extraction_range_);
@@ -100,7 +110,7 @@ public:
 
     // Matcher Extractor Parameters
     matcher_.setDistanceThreshold(_param.matcher_distance_th_);
-    matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_);
+    //matcher_desc_.setDistanceThreshold(_param.matcher_distance_th_);
     keypoints_number_th_ = _param.keypoints_number_th_;
   };
 
@@ -149,23 +159,22 @@ public:
     if (match_type = 1) {
       matching_number = matcher_.match(scene_1_falko->keypoints_list_,
                                        scene_2_falko->keypoints_list_, asso_nn);
-      // std::cout << "scene_1_falko->descriptors_list_ : "
-      //         << scene_1_falko->descriptors_list_[0] << std::endl;
 
     } else if (match_type = 2) {
       matching_number = matcher_.match(
         scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_,
         scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_,
         asso_nn);
-    } else if (match_type = 3) {
+        
+    } /*else if (match_type = 3) {
                matching_number = matcher_desc_.match(scene_1_falko->descriptors_list_,
                               scene_2_falko->descriptors_list_, asso_nn);
     }
-
+*/
     auto new_match = std::make_shared<MatchLoopClosure>();
     new_match->keypoints_number_match = matching_number;
     if (matching_number > keypoints_number_th_) {
-      new_match->match = true;
+      new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform);
     } else {
       new_match->match = false;
     }
@@ -176,8 +185,6 @@ public:
                        (double)std::min(scene_1_falko->keypoints_list_.size(),
                                         scene_2_falko->keypoints_list_.size());
 
-    computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform);
-    
     return new_match;
   }