Debug gtest

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
          *

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)
         {

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);
 }