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