diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 065de2c5dab9f512e1484df8ae86bb98680f2396..c054322b5d14dee0f7be4b2e8b08ce721661a66a 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -174,6 +174,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
*/
static bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
+ static void filterMatches(std::vector<std::pair<int,int> >& matches, const std::vector<int>& outliers_idx);
/** \brief Vote for KeyFrame generation
*
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index b57729c50322dd12bd7f69df014beb498ae29d91..1c30a796b201d3abfbe120fd5409c718274549a2 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -486,6 +486,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
if (RANSACWorks)
{
std::cout << "RANSAC has worked" << std::endl;
+ ProcessorTrackerLandmarkObject::filterMatches(matches, outliers_idx);
// int nb_outliers = outliers_idx.size();
// Eigen::Isometry3d cf_M_ci = best_model;
}
@@ -493,6 +494,48 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
return 1;
}
+std::ostream &operator<<(std::ostream &flux, std::vector<int> vect)
+{
+ for (int element : vect)
+ {
+ flux << element << " ";
+ }
+
+ flux << '\n';
+
+ return flux;
+}
+std::ostream &operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vect)
+{
+ for (auto element : vect)
+ {
+ flux << element.first << "," << element.second << " ";
+ }
+
+ flux << '\n';
+
+ return flux;
+}
+
+void ProcessorTrackerLandmarkObject::filterMatches(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);
+
+ if (index_outlier_occurrence != 0)
+ {
+ matches_temp.push_back(pair_indexes);
+ }
+ }
+
+ matches.clear();
+ matches = matches_temp;
+}
+
bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Isometry3d>& cl_M_o_vec,
const std::vector<Eigen::Isometry3d>& ci_M_o_vec,
const std::vector<std::pair<int,int> >& matches,
@@ -530,8 +573,7 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
Eigen::Isometry3d cl_M_ci = cl_M_o * o_M_ci;
for (auto other_match : matches)
- {
-
+ {
int index_feat_last_other = other_match.first;
int index_feat_incomming_other = other_match.second;
@@ -554,6 +596,8 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
}
}
+ inliers_idx_buff.push_back(index_feat_last);
+
//If the model gives better results
if (nb_inliers > best_nb_inliers)
{
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 9c4e83eebaca448afd804bf49a47bf8e81cfb6ef..0c781bb034c64db3c2dc8749c6b393a918931b4f 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -405,6 +405,10 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Quaterniond quat_cam_RANSAC(best_model.linear());
Vector3d pos_cam_RANSAC = best_model.translation();
+ ASSERT_TRUE(matches.size() == 5);
+ ProcessorTrackerLandmarkObject::filterMatches(matches, inliers_idx);
+ ASSERT_TRUE(matches.size() == 3);
+
ASSERT_MATRIX_APPROX(pos_cam, pos_cam_RANSAC, 1e-6)
ASSERT_MATRIX_APPROX(quat_cam.coeffs(), quat_cam_RANSAC.coeffs(), 1e-6)
@@ -415,6 +419,8 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
TEST(ProcessorTrackerLandmarkObject, isInliers)
{
+ std::cout << "test11" << "\n\n";
+
//Camera poses
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
@@ -458,6 +464,45 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
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
+
+ std::cout << "test1" << "\n\n";
+}
+
+TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
+{
+ std::cout << "test" << "\n\n";
+ std::vector<std::pair<int,int> > 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);
+ auto pair_o8 = std::make_pair(3, 2);
+ auto pair_o9 = std::make_pair(1, 6);
+ auto pair_o10 = std::make_pair(4, 1);
+ auto pair_o11 = std::make_pair(3, 7);
+ auto pair_o12 = std::make_pair(6, 7);
+
+ matches.push_back(pair_o1);
+ matches.push_back(pair_o2);
+ matches.push_back(pair_o3);
+ matches.push_back(pair_o4);
+ matches.push_back(pair_o5);
+ matches.push_back(pair_o6);
+ matches.push_back(pair_o7);
+ matches.push_back(pair_o8);
+ matches.push_back(pair_o9);
+ matches.push_back(pair_o10);
+ matches.push_back(pair_o11);
+ matches.push_back(pair_o12);
+
+ std::cout << "test" << "\n\n";
+ std::cout << ProcessorTrackerLandmarkObject::nbOfDifferentMatches(matches) << "\n\n\n";
+
+ ASSERT_TRUE(ProcessorTrackerLandmarkObject::nbOfDifferentMatches(matches) == 6);
}