From 31e28c22e981c3b0509ee2ed672359877a246681 Mon Sep 17 00:00:00 2001
From: Sergi Pujol <spujol@iri.upc.edu>
Date: Thu, 10 Jun 2021 15:43:34 +0200
Subject: [PATCH] wip

---
 src/loop_closure_falko.h          | 344 +++---------------------------
 test/gtest_loop_closure_falko.cpp |   8 +-
 2 files changed, 36 insertions(+), 316 deletions(-)

diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index 26961e5..7bb20be 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -103,11 +103,11 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
     /** \brief Constructor
      * \param _param parameter struct with falko lib parameters
      **/
-    LoopClosureFalko(ParameterLoopClosureFalko _param)
+    LoopClosureFalko(ParameterLoopClosureFalko _param, const M<falkolib::FALKO, D> &_matcher)
         : LoopClosureBase2d()
         , falkolib::FALKOExtractor()
         , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
-        , matcher_()
+        , matcher_(_matcher)
     {
         // FALKO Extractor Parameters
         setMinExtractionRange(_param.min_extraction_range_);
@@ -346,17 +346,21 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
                     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_)
-            {
-                new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_,
-                                                    asso_nn, new_match->transform);
-            }
-        else
-            {
-                new_match->match = false;
-            }
+        // 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;
@@ -365,11 +369,11 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
         // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(),
         //                                                               scene_2_falko->keypoints_list_.size());
 
-        new_match->score = (double)matching_number;
-
         new_match->transform_vector.head(2) = new_match->transform.translation();
         new_match->transform_vector(2)      = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();
 
+        new_match->score = (double)matching_number;
+
         return new_match;
     }
 
@@ -394,310 +398,26 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
         return fabs((double)area / 2.0);
     }
 };
+
 // Partial template specialization
-template <typename D, typename Extr>
-class LoopClosureFalko<D, Extr, falkolib::AHTMatcher> : public LoopClosureBase2d, public falkolib::FALKOExtractor
+
+template <typename D, typename Extr> class LoopClosureFalkoAht : public LoopClosureFalko<D, Extr, falkolib::AHTMatcher>
 {
   public:
-    typedef std::shared_ptr<SceneFalko<D>>       sceneFalkoBSCPtr;
-    typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr;
-
-    Extr                                     extractor_;
-    falkolib::AHTMatcher<falkolib::FALKO, D> matcher_;
-
-    /** \brief Constructor
-     * \param _param parameter struct with falko lib parameters
-     **/
-    LoopClosureFalko(ParameterLoopClosureFalko _param)
-        : LoopClosureBase2d()
-        , falkolib::FALKOExtractor()
-        , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
-        , matcher_(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_, _param.yAbsMax_, _param.thetaAbsMax_)
-
-    {
-        // FALKO Extractor Parameters
-        setMinExtractionRange(_param.min_extraction_range_);
-        setMaxExtractionRange(_param.max_extraction_range_);
-        enableSubbeam(_param.enable_subbeam_);
-        setNMSRadius(_param.nms_radius_);
-        setNeighB(_param.neigh_b_);
-        setBRatio(_param.b_ratio_);
-        setGridSectors(_param.grid_sectors_);
-
-        // Matcher Extractor Parameters
-        matcher_.setDistanceThreshold(_param.matcher_distance_th_);
-        matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_);
-        keypoints_number_th_ = _param.keypoints_number_th_;
-        use_descriptors_     = _param.use_descriptors_;
-    };
-    /** \brief Destructor
-     **/
-    ~LoopClosureFalko() {}
-
-    /** \brief Create and update the scene struct with keypoints and descriptors
-     **/
-    sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override
-    {
-        auto new_scene  = std::make_shared<SceneFalko<D>>();
-        auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
-        // Extract keypoints
-        std::vector<falkolib::FALKO> keypoints_list;
-        extract(*scan_falko, keypoints_list);
-
-        double angle_min  = _scan_params.angle_min_;
-        double angle_step = _scan_params.angle_step_;
-
-        // Compute max_dist
-        new_scene->max_distance_ = 0;
-        for (int i = 0; i < keypoints_list.size(); i++)
-            for (int j = 0; j < keypoints_list.size(); j++)
-                {
-                    double X_dist   = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]);
-                    double Y_dist   = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]);
-                    double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist));
-                    if (distance > new_scene->max_distance_)
-                        new_scene->max_distance_ = distance;
-                }
-
-        // discard too close by kp
-        for (int i = 0; i < keypoints_list.size(); i++)
-            {
-                int repeated = 0;
-                for (int j = i + 1; j < keypoints_list.size(); j++)
-                    {
-                        double X_dist   = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]);
-                        double Y_dist   = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]);
-                        double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist));
-                        if (distance < 0.05)
-                            {
-                                repeated = 1;
-                            }
-                    }
-                if (repeated == 0)
-                    {
-                        new_scene->keypoints_list_.push_back(keypoints_list[i]);
-                    }
-            }
-
-        // Compute descriptors
-        extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_);
-        std::vector<D> descriptors_list_rotated;
-        extractor_.compute(*scan_falko, new_scene->keypoints_list_, descriptors_list_rotated);
-
-        // Compute Scene mid point, angle for each keypoint and rotate descriptors
-        Eigen::Vector2d     mid_point(0, 0);
-        std::vector<double> angle_rotation;
-        for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
-            {
-                mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0];
-                mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1];
-                angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
-                // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i];
-                double orientation = angle_rotation[i];
-                descriptors_list_rotated[i].rotate(orientation);
-                new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]);
-            }
-
-        new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size();
-        // Compute Scene Area and Perimeter
-        int    n = 3;
-        double X[n];
-        double Y[n];
-        new_scene->perimeter_ = 0.0;
-        new_scene->area_      = 0.0;
-        Eigen::Vector2d dist_between_two_kp;
-
-        if (new_scene->keypoints_list_.size() < 3)
-            return new_scene;
-
-        if (new_scene->keypoints_list_.size() < 4)
-            {
-                X[0] = new_scene->keypoints_list_[0].point[0];
-                Y[0] = new_scene->keypoints_list_[0].point[1];
-
-                X[1] = new_scene->keypoints_list_[1].point[0];
-                Y[1] = new_scene->keypoints_list_[1].point[1];
-
-                X[2] = new_scene->keypoints_list_[2].point[0];
-                Y[2] = new_scene->keypoints_list_[2].point[1];
-
-                new_scene->area_ = new_scene->area_ + triangleArea(X, Y, n);
-                return new_scene;
-            }
-
-        for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
-            {
-                X[0] = new_scene->mid_point_[0];
-                Y[0] = new_scene->mid_point_[1];
-
-                // X[0] = 0.0;
-                // Y[0] = 0.0;
-
-                X[1] = new_scene->keypoints_list_[i].point[0];
-                Y[1] = new_scene->keypoints_list_[i].point[1];
-
-                if (i < new_scene->keypoints_list_.size() - 1) // Proceed until final keypoint
-                    {
-                        X[2] = new_scene->keypoints_list_[i + 1].point[0];
-                        Y[2] = new_scene->keypoints_list_[i + 1].point[1];
-
-                        dist_between_two_kp =
-                            new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point;
-                    }
-                else // if you arrived to the final keypoint then use inital keypoint
-                    {
-                        X[2] = new_scene->keypoints_list_[0].point[0];
-                        Y[2] = new_scene->keypoints_list_[0].point[1];
-
-                        dist_between_two_kp = new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[0].point;
-                    }
-                new_scene->area_      = new_scene->area_ + (double)triangleArea(X, Y, n);
-                new_scene->perimeter_ = new_scene->perimeter_ + hypot(dist_between_two_kp[0], dist_between_two_kp[1]);
-            }
-
-        // Compue Scene linear regresion and initial angle
-        double ss_xy = 0;
-        double ss_xx = 0;
-        for (int i = 0; i <= new_scene->keypoints_list_.size(); i++)
-            {
-                ss_xy += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) *
-                         (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]);
-                ss_xx += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) *
-                         (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]);
-            }
-        double b_1 = ss_xy / ss_xx;
-        // double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0];
-
-        // new_scene->regressor_coefs.push_back(b_0);
-        // new_scene->regressor_coefs.push_back(b_1);
-
-        double initial_angle = -atan(b_1);
-
-        // double inital_angle_inv = initial_angle - M_PI;
-
-        // rotate keypoints
-        for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
-            {
-                falkolib::FALKO keypoint_mid;
-                keypoint_mid = new_scene->keypoints_list_[i];
-
-                keypoint_mid.point[0] = new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0];
-                keypoint_mid.point[1] = new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1];
-                new_scene->keypoints_list_mid_point_.push_back(keypoint_mid);
-
-                falkolib::FALKO keypoint_rotated;
-                keypoint_rotated          = new_scene->keypoints_list_[i];
-                keypoint_rotated.point[0] = new_scene->keypoints_list_[i].point[0] * cos(initial_angle) -
-                                            new_scene->keypoints_list_[i].point[1] * sin(initial_angle);
-                keypoint_rotated.point[1] = new_scene->keypoints_list_[i].point[0] * sin(initial_angle) +
-                                            new_scene->keypoints_list_[i].point[1] * cos(initial_angle);
-                new_scene->keypoints_list_rotated_.push_back(keypoint_rotated);
-
-                falkolib::FALKO keypoint_rot_trans;
-                keypoint_rot_trans = new_scene->keypoints_list_[i];
-                keypoint_rot_trans.point[0] =
-                    (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * cos(initial_angle) -
-                    (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * sin(initial_angle);
-
-                keypoint_rot_trans.point[1] =
-                    (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * sin(initial_angle) +
-                    (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * cos(initial_angle);
-
-                new_scene->keypoints_list_transl_rot_.push_back(keypoint_rot_trans);
-            }
-
-        return new_scene;
-    }
-
-    /** \brief Convert scans from laserscanutils::LaserScan to
-     *falkolib::LaserScan object
-     **/
-    laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params)
-    {
-        auto scan_falko = std::make_shared<falkolib::LaserScan>(_scan_params.angle_min_, _scan_params.angle_max_,
-                                                                _scan.ranges_raw_.size());
-        std::vector<double> double_ranges(_scan.ranges_raw_.begin(), _scan.ranges_raw_.end());
-        scan_falko->fromRanges(double_ranges);
-        return scan_falko;
-    }
-
-    /** \brief Create and update a matchLoopClosure struct with the info that is produced when matching two given scenes
-     * \param _scene_1 reference scene struct
-     * \param _scene_2 target scene struct
-     **/
-    MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override
-    {
-        std::vector<std::pair<int, int>> asso_nn;
-        auto                             scene_1_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_1);
-        auto                             scene_2_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_2);
-
-        int matching_number = 0;
-
-        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);
-            }
-        auto new_match                    = std::make_shared<MatchLoopClosureScene>();
-        new_match->keypoints_number_match = matching_number;
-        if (matching_number > keypoints_number_th_)
-            {
-                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->score = (double)matching_number;
-
-        new_match->transform_vector.head(2) = new_match->transform.translation();
-        new_match->transform_vector(2)      = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();
-
-        return new_match;
-    }
-
-    int  keypoints_number_th_;
-    bool use_descriptors_;
-
-    // (X[i], Y[i]) are coordinates of i'th point.
-    double triangleArea(double X[], double Y[], int n)
-    {
-        // Initialize area
-        double area = 0.0;
+    LoopClosureFalkoAht(ParameterLoopClosureFalko _param)
+        : LoopClosureFalko<D, Extr, falkolib::AHTMatcher>(
+              _param,
+              falkolib::AHTMatcher<falkolib::FALKO, D>(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_,
+                                                       _param.yAbsMax_, _param.thetaAbsMax_)){};
+};
 
-        // Calculate value of shoelace formula
-        int j = n - 1;
-        for (int i = 0; i < n; i++)
-            {
-                area += (X[j] + X[i]) * (Y[j] - Y[i]);
-                j = i; // j is previous vertex to i
-            }
+template <typename D, typename Extr> class LoopClosureFalkoNn : public LoopClosureFalko<D, Extr, falkolib::NNMatcher>
+{
+  public:
+    LoopClosureFalkoNn(ParameterLoopClosureFalko _param)
+        : LoopClosureFalko<D, Extr, falkolib::NNMatcher>(_param, falkolib::NNMatcher<falkolib::FALKO, D>()){
 
-        // Return absolute value
-        return fabs((double)area / 2.0);
-    }
+          };
 };
 
 } /* namespace laserscanutils */
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index 6394926..9c0caf4 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -24,7 +24,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
 
     ParameterLoopClosureFalko param;
     param.matcher_distance_th_ = 0.3;
-    LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
+    LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko(param);
 
     // Test convert2LaserScanFALKO
     std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);
@@ -81,7 +81,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
 
     ParameterLoopClosureFalko param;
     param.matcher_distance_th_ = 0.3;
-    LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
+    LoopClosureFalkoAht<bsc, bscExtractor>  loop_cl_falko(param);
 
     // Test extractScene
     auto new_scene   = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
@@ -194,7 +194,7 @@ TEST(loop_closure_falko, TestMatch)
     ParameterLoopClosureFalko param;
     param.matcher_distance_th_ = 100;
     param.matcher_ddesc_th_    = 5;
-    LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
+    LoopClosureFalkoAht<bsc, bscExtractor>  loop_cl_falko(param);
 
     // Test extractScene
     auto new_scene_1 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
@@ -227,7 +227,7 @@ TEST(loop_closure_falko, TestMatch)
 
     laser_params.angle_step_ = 0.00701248;
 
-    LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko_2(param);
+    LoopClosureFalkoAht<bsc, bscExtractor>  loop_cl_falko_2(param);
     auto                                             new_scene_target =
         std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2.extractScene(scan_target, laser_params));
     auto new_scene_reference =
-- 
GitLab