Skip to content
GitLab
Commit 59b9181d authored by ydepledt's avatar ydepledt
Browse files

Refactor code

parent 6d97be52
Hide whitespace changes
Inline Side-by-side
src/processor/processor_tracker_landmark_object.cpp
View file @ 59b9181d
......@@ -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;
}
......
test/gtest_processor_tracker_landmark_object.cpp
View file @ 59b9181d
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment