Skip to content
Snippets Groups Projects
Commit 59b9181d authored by ydepledt's avatar ydepledt
Browse files

Refactor code

parent 6d97be52
No related branches found
No related tags found
No related merge requests found
......@@ -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;
}
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment