diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 1aaf437194c4e1cd88c563a55d1de9412e97600f..6bfd5139fa3b9cb10b7c258410e728fe04bfede7 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -179,7 +179,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
const FeatureBasePtrList& _features_out_last,
const FeatureBasePtrList& _features_out_incoming);
- static void filterMatches(std::vector<std::pair<int,int> >& matches, const std::vector<int>& outliers_idx);
+ static void filterMatchesOutliers(std::vector<std::pair<int,int> >& matches, const std::vector<int>& outliers_idx);
+
+ static void filterMatchesInliers(std::vector<std::pair<int,int> >& _matches, const std::vector<int>& _inliers_idx);
const TrackMatrix& getTrackMatrix() const {return track_matrix_;}
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 76477e711f6470409b692e60bbdfa226dd5a101e..d1345bf6eb043123f5f079b1928472a6e10405d0 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -488,7 +488,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
std::cout << "RANSAC has worked" << std::endl;
//Keep only inliers
- ProcessorTrackerLandmarkObject::filterMatches(matches, inliers_idx);
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(matches, inliers_idx);
}
@@ -538,39 +538,34 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
}
}
-std::ostream &operator<<(std::ostream &flux, std::vector<int> vect)
+void ProcessorTrackerLandmarkObject::filterMatchesOutliers(std::vector<std::pair<int,int> >& _matches, const std::vector<int>& _outliers_idx)
{
- for (int element : vect)
- {
- flux << element << " ";
- }
+ auto size = _matches.size();
- flux << '\n';
+ for (int i = 0; i < size; i++)
+ {
+ int index_outlier_occurrence = std::count(_outliers_idx.begin(), _outliers_idx.end(), _matches[i].first);
- return flux;
-}
-std::ostream &operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vect)
-{
- for (auto element : vect)
- {
- flux << element.first << "," << element.second << " ";
+ if (index_outlier_occurrence != 0)
+ {
+ _matches.erase(std::remove(_matches.begin(), _matches.end(), _matches[i]));
+ size--;
+ i--;
+ }
}
- flux << '\n';
-
- return flux;
}
-void ProcessorTrackerLandmarkObject::filterMatches(std::vector<std::pair<int,int> >& _matches,
+void ProcessorTrackerLandmarkObject::filterMatchesInliers(std::vector<std::pair<int,int> >& _matches,
const std::vector<int>& _inliers_idx)
{
std::vector<std::pair<int,int> > matches_temp;
for (auto pair_indexes : _matches)
{
- int index_outlier_occurrence = std::count(_inliers_idx.begin(), _inliers_idx.end(), pair_indexes.first);
+ int index_inlier_occurrence = std::count(_inliers_idx.begin(), _inliers_idx.end(), pair_indexes.first);
- if (index_outlier_occurrence != 0)
+ if (index_inlier_occurrence != 0)
{
matches_temp.push_back(pair_indexes);
}
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index ffd71a617fad4aa6c730303ee6d53ba8009a1a54..95b4277a4f2daf0de68f573dcb52ec7456293209 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -409,7 +409,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Vector3d pos_cam_RANSAC = best_model.translation();
ASSERT_TRUE(matches.size() == 5);
- ProcessorTrackerLandmarkObject::filterMatches(matches, inliers_idx);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches, outliers_idx);
ASSERT_TRUE(matches.size() == 3);
ASSERT_MATRIX_APPROX(pos_cam, pos_cam_RANSAC, 1e-6)
@@ -502,6 +502,83 @@ TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
ASSERT_TRUE(ProcessorTrackerLandmarkObject::nbOfDifferentMatches(matches) == 7);
}
+TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
+{
+ std::vector<std::pair<int,int> > matches1;
+ std::vector<std::pair<int,int> > matches2;
+ std::vector<std::pair<int,int> > matches3;
+ std::vector<std::pair<int,int> > matches1I;
+ std::vector<std::pair<int,int> > matches2I;
+ std::vector<std::pair<int,int> > matches3I;
+
+ std::vector<int> outliers_idx1;
+ std::vector<int> outliers_idx2;
+ std::vector<int> outliers_idx3;
+
+ outliers_idx1.push_back(2);
+ outliers_idx1.push_back(5);
+ outliers_idx1.push_back(9);
+ outliers_idx2.push_back(0);
+ outliers_idx2.push_back(9);
+ outliers_idx2.push_back(10);
+ outliers_idx2.push_back(11);
+ outliers_idx3.push_back(4);
+ outliers_idx3.push_back(5);
+ outliers_idx3.push_back(6);
+ outliers_idx3.push_back(7);
+ outliers_idx3.push_back(8);
+
+ //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(5, 5);
+ auto pair_o7 = std::make_pair(6, 6);
+ auto pair_o8 = std::make_pair(7, 7);
+ auto pair_o9 = std::make_pair(8, 8);
+ auto pair_o10 = std::make_pair(9, 9);
+ auto pair_o11 = std::make_pair(10, 10);
+ auto pair_o12 = std::make_pair(11, 11);
+
+ //Append pairs in matches object
+ matches1.push_back(pair_o1);
+ matches1.push_back(pair_o2);
+ matches1.push_back(pair_o3);
+ matches1.push_back(pair_o4);
+ matches1.push_back(pair_o5);
+ matches1.push_back(pair_o6);
+ matches1.push_back(pair_o7);
+ matches1.push_back(pair_o8);
+ matches1.push_back(pair_o9);
+ matches1.push_back(pair_o10);
+ matches1.push_back(pair_o11);
+ matches1.push_back(pair_o12);
+
+ matches2 = matches1;
+ matches3 = matches1;
+ matches1I = matches1;
+ matches2I = matches1;
+ matches3I = matches1;
+
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches1, outliers_idx1);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches2, outliers_idx2);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches3, outliers_idx3);
+
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(matches1I, outliers_idx1);
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(matches2I, outliers_idx2);
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(matches3I, outliers_idx3);
+
+ ASSERT_TRUE(matches1.size() == 9);
+ ASSERT_TRUE(matches2.size() == 8);
+ ASSERT_TRUE(matches3.size() == 7);
+ ASSERT_TRUE(matches1I.size() == 3);
+ ASSERT_TRUE(matches2I.size() == 4);
+ ASSERT_TRUE(matches3I.size() == 5);
+
+}
+
int main(int argc, char **argv)