From 59b9181d6db00e49dfd5430bccf006ba57606f47 Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Mon, 4 Jul 2022 12:51:40 +0200
Subject: [PATCH] Refactor code

---
 .../processor_tracker_landmark_object.cpp     | 10 +--
 ...test_processor_tracker_landmark_object.cpp | 86 +++++++++++++------
 2 files changed, 62 insertions(+), 34 deletions(-)

diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index cf2ea9e..01173b3 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -697,11 +697,6 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
                                                     Eigen::Isometry3d& _best_model)
                                                     
 {
-
-    //Check if the dataset has a sufficient size
-    // if (nbOfDifferentMatches(_matches) < 3)
-    //     return false;
-
     // Vector that will contains index of inliers/outliers for each iteration
     std::vector<std::pair<int, int> > inliers_idx_buff;
     std::vector<std::pair<int, int> > outliers_idx_buff;
@@ -749,8 +744,6 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
                     outliers_idx_buff.push_back(p);
                 }
             }
-
-            // std::cout << index_feat_last << " " << index_feat_incoming << "--->" << index_feat_last_other << " " << index_feat_incomming_other << " ---> " << outliers_idx_buff << "\n";
         }
 
         //Feature which provide the model is an inlier
@@ -778,6 +771,9 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
         return true;
     }
 
+    if (best_nb_inliers < 2) {
+        return false;
+    }
 
     return true;
 }
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 7a0f8c2..a1b61f8 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -465,6 +465,7 @@ std::ostream &operator<<(std::ostream &flux, std::vector<std::pair<int, int>> ve
 
 TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
 {
+    //Matches with two outliers
     std::vector<Eigen::Isometry3d> cl_M_o_vec;
     std::vector<Eigen::Isometry3d> ci_M_o_vec;
     std::vector<std::pair<int, int>> matches;
@@ -472,6 +473,14 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     std::vector<std::pair<int, int>> outliers_idx;
     Eigen::Isometry3d best_model;
 
+    //Matches with too many outliers
+    std::vector<Eigen::Isometry3d> cl_M_o_vec_2;
+    std::vector<Eigen::Isometry3d> ci_M_o_vec_2;
+    std::vector<std::pair<int, int>> matches_2;
+    std::vector<std::pair<int, int>> inliers_idx_2;
+    std::vector<std::pair<int, int>> outliers_idx_2;
+    Eigen::Isometry3d best_model_2;
+
     // Transform pose into isometry
     Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last_);
     Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming_);
@@ -484,9 +493,11 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
 
     Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
     Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
-    Eigen::Isometry3d cl_M_o3 = w_M_o3;
+    Eigen::Isometry3d cl_M_o3 = w_M_o3; //outliers
     Eigen::Isometry3d cl_M_o4 = w_M_cl.inverse() * w_M_o4;
-    Eigen::Isometry3d cl_M_o5 = w_M_o5;
+    Eigen::Isometry3d cl_M_o5 = w_M_o5; //outliers
+    Eigen::Isometry3d cl_M_o6 = w_M_o1; //outliers
+    Eigen::Isometry3d cl_M_o7 = w_M_o2; //outliers
 
     cl_M_o_vec.push_back(cl_M_o1);
     cl_M_o_vec.push_back(cl_M_o2);
@@ -494,11 +505,19 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     cl_M_o_vec.push_back(cl_M_o4);
     cl_M_o_vec.push_back(cl_M_o5);
 
+    cl_M_o_vec_2.push_back(cl_M_o1);
+    cl_M_o_vec_2.push_back(cl_M_o3);
+    cl_M_o_vec_2.push_back(cl_M_o5);
+    cl_M_o_vec_2.push_back(cl_M_o6);
+    cl_M_o_vec_2.push_back(cl_M_o7);
+
     Eigen::Isometry3d ci_M_o1 = w_M_ci.inverse() * w_M_o1;
     Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2;
     Eigen::Isometry3d ci_M_o3 = w_M_o5; // outliers
     Eigen::Isometry3d ci_M_o4 = w_M_ci.inverse() * w_M_o4;
     Eigen::Isometry3d ci_M_o5 = w_M_o2; // outliers
+    Eigen::Isometry3d ci_M_o6 = w_M_o1; // outliers
+    Eigen::Isometry3d ci_M_o7 = w_M_o3; // outliers
 
     ci_M_o_vec.push_back(ci_M_o1);
     ci_M_o_vec.push_back(ci_M_o2);
@@ -506,6 +525,12 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     ci_M_o_vec.push_back(ci_M_o4);
     ci_M_o_vec.push_back(ci_M_o5);
 
+    ci_M_o_vec_2.push_back(ci_M_o1);
+    ci_M_o_vec_2.push_back(ci_M_o3);
+    ci_M_o_vec_2.push_back(ci_M_o5);
+    ci_M_o_vec_2.push_back(ci_M_o6);
+    ci_M_o_vec_2.push_back(ci_M_o7);
+
     Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
 
     // Create 5 matches
@@ -515,6 +540,12 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     auto pair_o4 = std::make_pair(3, 3);
     auto pair_o5 = std::make_pair(4, 4);
 
+    auto pair_o1_2 = std::make_pair(0, 0);
+    auto pair_o2_2 = std::make_pair(1, 1);
+    auto pair_o3_2 = std::make_pair(2, 2);
+    auto pair_o4_2 = std::make_pair(3, 3);
+    auto pair_o5_2 = std::make_pair(4, 4);
+
     // Append pairs in matches object
     matches.push_back(pair_o1);
     matches.push_back(pair_o2);
@@ -522,8 +553,15 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     matches.push_back(pair_o4);
     matches.push_back(pair_o5);
 
+    matches_2.push_back(pair_o1_2);
+    matches_2.push_back(pair_o2_2);
+    matches_2.push_back(pair_o3_2);
+    matches_2.push_back(pair_o4_2);
+    matches_2.push_back(pair_o5_2);
+
     // Detect all outliers of our batch
-    ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, 0.55, inliers_idx, outliers_idx, best_model);
+    bool firstMatchRansacWorked  = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, 0.55, inliers_idx, outliers_idx, best_model);
+    bool secondMatchRansacWorked = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec_2, ci_M_o_vec_2, matches_2, 0.55, inliers_idx_2, outliers_idx_2, best_model_2);
 
     Quaterniond quat_cam(cl_M_ci.linear());
     Vector3d pos_cam = cl_M_ci.translation();
@@ -531,6 +569,10 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     Quaterniond quat_cam_RANSAC(best_model.linear());
     Vector3d pos_cam_RANSAC = best_model.translation();
 
+    ASSERT_TRUE(firstMatchRansacWorked);
+    ASSERT_FALSE(secondMatchRansacWorked);
+
+
     ASSERT_TRUE(matches.size() == 5);
     ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
     ASSERT_TRUE(matches.size() == 3);
@@ -562,8 +604,8 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, isInliers)
     ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o1, cl_M_ci));
     ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o2, cl_M_ci));
     ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o3, ci_M_o3, cl_M_ci));
-    ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o2, cl_M_ci))); // outliers
-    ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, cl_M_ci))); // outliers
+    ASSERT_FALSE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o2, cl_M_ci)); // outliers
+    ASSERT_FALSE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, cl_M_ci)); // outliers
 }
 
 TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
@@ -571,15 +613,15 @@ TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
     std::vector<std::pair<int, int>> matches;
 
     // Create 12 matches
-    auto pair_o1 = std::make_pair(0, 0);
-    auto pair_o2 = std::make_pair(1, 1);
-    auto pair_o3 = std::make_pair(2, 2);
-    auto pair_o4 = std::make_pair(3, 3);
-    auto pair_o5 = std::make_pair(4, 4);
-    auto pair_o6 = std::make_pair(8, 9);
-    auto pair_o7 = std::make_pair(1, 4);  // Not unique
-    auto pair_o8 = std::make_pair(3, 2);  // Not unique
-    auto pair_o9 = std::make_pair(1, 6);  // Not unique
+    auto pair_o1  = std::make_pair(0, 0);
+    auto pair_o2  = std::make_pair(1, 1);
+    auto pair_o3  = std::make_pair(2, 2);
+    auto pair_o4  = std::make_pair(3, 3);
+    auto pair_o5  = std::make_pair(4, 4);
+    auto pair_o6  = std::make_pair(8, 9);
+    auto pair_o7  = std::make_pair(1, 4);  // Not unique
+    auto pair_o8  = std::make_pair(3, 2);  // Not unique
+    auto pair_o9  = std::make_pair(1, 6);  // Not unique
     auto pair_o10 = std::make_pair(4, 1); // Not unique
     auto pair_o11 = std::make_pair(3, 7); // Not unique
     auto pair_o12 = std::make_pair(6, 7);
@@ -655,8 +697,8 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
     matches1.push_back(pair_o11);
     matches1.push_back(pair_o12);
 
-    matches2 = matches1;
-    matches3 = matches1;
+    matches2  = matches1;
+    matches3  = matches1;
     matches1I = matches1;
     matches2I = matches1;
     matches3I = matches1;
@@ -678,16 +720,6 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
     ASSERT_TRUE(matches3I.size() == 5);
 }
 
-// std::ostream& operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vector)
-// {
-//     for (auto pair : vector)
-//     {
-//         flux << pair.first << " " << pair.second << "\n";
-//     }
-
-//     return flux;
-// }
-
 TEST_F(ProcessorTrackerLandmarkObject_fixture, allMatchesSameType)
 {
     FeatureBasePtrList features_last;
@@ -864,7 +896,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, processFeatures)
 
     
     //Test if the trackMatrix is the one above
-    
+
     ASSERT_TRUE(sizeTrackFeat1 == 3);
     ASSERT_TRUE(sizeTrackFeat2 == 3);
     ASSERT_TRUE(sizeTrackFeat3 == 3);
-- 
GitLab