wip

src/loop_closure_falko.h

@@ -127,6 +127,8 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
         use_descriptors_     = _param.use_descriptors_;
     };
 
+    // Template specialization
+
     /** \brief Destructor
      **/
     ~LoopClosureFalko() {}
@@ -138,34 +140,61 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
         auto new_scene  = std::make_shared<SceneFalko<D>>();
         auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
         // Extract keypoints
-        extract(*scan_falko, (new_scene->keypoints_list_));
+        std::vector<falkolib::FALKO> keypoints_list;
+        extract(*scan_falko, keypoints_list);
+
+        // Compute angle vector for keypoints
+        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_);
 
-        // Compute Scene mid point
+        // Compute Scene mid point, angle for each keypoint and rotate descriptors
         Eigen::Vector2d mid_point(0, 0);
+        new_scene->descriptors_list_rotated = new_scene->descriptors_list_;
+
         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];
+                new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
+                new_scene->descriptors_list_rotated[i].rotate(-new_scene->angle_rotation_[i]);
             }
         new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size();
 
-        // Compute max_dist
-        new_scene->max_distance_ = 0;
-        for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
-            for (int j = 0; j < new_scene->keypoints_list_.size(); j++)
-                {
-                    double X_dist =
-                        fabs(new_scene->keypoints_list_[i].point[0] - new_scene->keypoints_list_[j].point[0]);
-                    double Y_dist =
-                        fabs(new_scene->keypoints_list_[i].point[1] - new_scene->keypoints_list_[j].point[1]);
-                    double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist));
-                    std::cout << "distance : " << distance << std::endl;
-                    if (distance > new_scene->max_distance_)
-                        new_scene->max_distance_ = distance;
-                }
-
         // Compute Scene Area and Perimeter
         int    n = 3;
         double X[n];
@@ -211,7 +240,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
                         dist_between_two_kp =
                             new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point;
                     }
-                else // then use inital keypoint
+                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];
@@ -222,6 +251,58 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
                 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->keipoints_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->keipoints_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->keipoints_list_transl_rot_.push_back(keypoint_rot_trans);
+            }
+
         return new_scene;
     }
 
@@ -243,7 +324,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
      **/
     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);
@@ -252,14 +332,14 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
 
         if (use_descriptors_ == 0)
             {
-                matching_number =
-                    matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn);
+                matching_number = matcher_.match(scene_1_falko->keipoints_list_transl_rot_,
+                                                 scene_2_falko->keipoints_list_transl_rot_, asso_nn);
             }
         else if (use_descriptors_ == 1)
             {
-                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);
+                matching_number = matcher_.match(
+                    scene_1_falko->keipoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated,
+                    scene_2_falko->keipoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn);
             }
         auto new_match                    = std::make_shared<MatchLoopClosureScene>();
         new_match->keypoints_number_match = matching_number;
@@ -272,11 +352,15 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
             {
                 new_match->match = false;
             }
-        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->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 / 10.0;
 
         new_match->transform_vector.head(2) = new_match->transform.translation();
         new_match->transform_vector(2)      = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();

src/match_loop_closure_scene.h

@@ -24,6 +24,7 @@ struct MatchLoopClosureScene {
   bool match;
   int keypoints_number_match;
   double score;
+  std::vector<std::pair<int, int>> associations;
   Eigen::Affine2d transform;
   Eigen::Vector3d transform_vector;
 };

src/scene_base.h

@@ -21,6 +21,7 @@ struct SceneBase
     double max_distance_;
     double mean_distance_;
     Eigen::Vector2d mid_point_;
+    std::vector<double> regressor_coefs;
 
 };
 typedef std::shared_ptr<SceneBase> sceneBasePtr;

src/scene_falko.h

@@ -35,7 +35,13 @@ typedef falkolib::CGH cgh;
 template <typename D> struct SceneFalko : public SceneBase
 {
     std::vector<falkolib::FALKO> keypoints_list_;
+    std::vector<falkolib::FALKO> keipoints_list_mid_point_;
+    std::vector<falkolib::FALKO> keipoints_list_transl_rot_;
+    std::vector<falkolib::FALKO> keipoints_list_rotated_;
+    std::vector<falkolib::FALKO> keipoints_list_rotated_reverse_;
     std::vector<D>               descriptors_list_;
+    std::vector<D>               descriptors_list_rotated;
+    std::vector<double>          angle_rotation_;
 };
 
 } /* namespace laserscanutils */

test/gtest_loop_closure_falko.cpp

@@ -20,7 +20,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
         }
 
     ParameterLoopClosureFalko                       param;
-    LoopClosureFalko<bsc, bscExtractor, nn_matcher> loop_cl_falko(param);
+    param.matcher_distance_th_= 0.3;
+    LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
 
     // Test convert2LaserScanFALKO
     std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);