From fff5cc4de0c36fdb46b1ccf1a7590faff6958f1c Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Tue, 28 Jun 2022 17:04:41 +0200
Subject: [PATCH] Change type of outliers/inliers_idx and correct
 matchingRansac

---
 .../processor_tracker_landmark_object.h       |  22 +-
 .../processor_tracker_landmark_object.cpp     | 125 +++++++---
 ...test_processor_tracker_landmark_object.cpp | 229 +++++++++++++++---
 3 files changed, 298 insertions(+), 78 deletions(-)

diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 700f66a..7dadcb5 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -157,8 +157,8 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
                                            FeatureBasePtrList& _features_out_incoming);
 
 
-        void allMatchesSameType(const FeatureBasePtrList& _features_out_last,
-                                const FeatureBasePtrList& _features_out_incoming);
+        static std::vector<std::pair<int,int> > allMatchesSameType(const FeatureBasePtrList& _features_out_last,
+                                                                   const FeatureBasePtrList& _features_out_incoming);
 
 
         /** \brief Method for matching feature from last to incoming
@@ -181,8 +181,8 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
                                    const std::vector<Eigen::Isometry3d>& _ci_M_o_vec, 
                                    const std::vector<std::pair<int,int> >& _matches,
                                    const double _ratio_outliers_inliers,
-                                   std::vector<int>& _inliers_idx,
-                                   std::vector<int>& _outliers_idx,
+                                   std::vector<std::pair<int, int> >& _inliers_idx,
+                                   std::vector<std::pair<int, int> >& _outliers_idx,
                                    Eigen::Isometry3d& _best_model);
         
         /** \brief Count the number of different matches
@@ -215,9 +215,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
          * \param _trackMatrix matrix of all tracks of features along captures              
          *  
          */
-        static void processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered, 
-                                    const FeatureBasePtrList& _features_out_last,
-                                    const FeatureBasePtrList& _features_out_incoming,
+        static void processFeatures(const std::vector<std::pair<int,int> >& _matches, 
+                                    FeatureBasePtrList detections_last,
+                                    FeatureBasePtrList detections_incoming,
                                     TrackMatrix& _trackMatrix);
         
         /** \brief Filter matches thanks to outliers vector
@@ -227,7 +227,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
          * A function which remove from matches the outliers
          * 
          */
-        static void filterMatchesOutliers(const std::vector<int>& _outliers_idx, 
+        static void filterMatchesOutliers(const std::vector<std::pair<int, int> >& _outliers_idx, 
                                           std::vector<std::pair<int,int> >& _matches);
 
         /** \brief Filter matches thanks to inliers vector
@@ -237,10 +237,10 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
          * A function which create a vector based on _inliers_idx
          * 
          */
-        static void filterMatchesInliers(const std::vector<int>& _inliers_idx,
+        static void filterMatchesInliers(const std::vector<std::pair<int, int> >& _inliers_idx,
                                          std::vector<std::pair<int,int> >& _matches);
 
-        const TrackMatrix& getTrackMatrix() const {return track_matrix_;}
+        TrackMatrix& getTrackMatrix()  {return track_matrix_;}
 
         /** \brief Vote for KeyFrame generation
          *
@@ -318,4 +318,6 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
 } // namespace wolf
 
 
+
+
 #endif /* _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_ */
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index ef70a59..92f1b74 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -479,8 +479,8 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
         index_last++;
     }
 
-    std::vector<int> inliers_idx;
-    std::vector<int> outliers_idx;
+    std::vector<std::pair<int,int> > inliers_idx;
+    std::vector<std::pair<int,int> > outliers_idx;
     Eigen::Isometry3d best_model = Eigen::Isometry3d::Identity();
     bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, ratio_inliers_outliers_, inliers_idx, outliers_idx, best_model);
 
@@ -496,16 +496,37 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
     return 1;
 }
 
-void ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList& _features_out_last,
-                                                        const FeatureBasePtrList& _features_out_incoming)
+// 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;
+// }
+
+std::ostream& operator<<(std::ostream &flux, std::vector<int> vector)
+{
+    for (auto element : vector)
+    {
+        flux << element << "\n";
+    }
+
+    return flux;
+}
+
+std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList& _features_out_last,
+                                                                                    const FeatureBasePtrList& _features_out_incoming)
 {
     std::vector<Eigen::Isometry3d> cl_M_o_vec;
     std::vector<Eigen::Isometry3d> ci_M_o_vec;
     std::vector<std::pair<int,int> > matches;
 
     int index_last = 0;
+    bool pushBack = true;
 
-    for (auto feat_last : detections_last_)
+    for (auto feat_last : _features_out_last)
     {
         std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
 
@@ -518,52 +539,66 @@ void ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList
 
         int index_incoming = 0;
 
-        for (auto feat_incoming : detections_incoming_)
+        for (auto feat_incoming : _features_out_incoming)
         {
             std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
 
             if (object_type_last == object_type_incoming)
             {
-                //camera to feat
+                std::pair<int, int> pair_indexes = std::make_pair(index_last, index_incoming);
+                matches.push_back(pair_indexes);
+            }
+            
+            //camera to feat
+            if (pushBack)
+            {
+
                 Vector3d pos_obj_incoming = feat_incoming->getMeasurement().head(3);
                 Quaterniond quat_obj_incoming (feat_incoming->getMeasurement().tail(4).data());
 
                 Eigen::Isometry3d ci_M_o = Eigen::Translation<double,3>(pos_obj_incoming) * quat_obj_incoming;
-                ci_M_o_vec.push_back(ci_M_o);
-
-                std::pair<int, int> pair_indexes = std::make_pair(index_last, index_incoming);
-                matches.push_back(pair_indexes);
 
+                ci_M_o_vec.push_back(ci_M_o);
             }
+
             index_incoming++;
         }
+
+        index_last++;
+        pushBack = false;
+
     }
 
-    std::vector<int> inliers_idx;
-    std::vector<int> outliers_idx;
+    // std::cout << matches << "\n";
+
+    
+    std::vector<std::pair<int,int> > inliers_idx;
+    std::vector<std::pair<int,int> > outliers_idx;
     Eigen::Isometry3d best_model = Eigen::Isometry3d::Identity();
-    bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, ratio_inliers_outliers_, inliers_idx, outliers_idx, best_model);
+    bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, 0.10, inliers_idx, outliers_idx, best_model);
 
     if (RANSACWorks)
     {
         std::cout << "RANSAC has worked" << std::endl;
 
         //Keep only inliers
-        ProcessorTrackerLandmarkObject::filterMatchesInliers(inliers_idx, matches);
+        ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
         
     }
+
+    return matches;
 }
 
                                                       
-void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered, 
-                                                     const FeatureBasePtrList& _features_out_last,
-                                                     const FeatureBasePtrList& _features_out_incoming,
+void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches, 
+                                                     FeatureBasePtrList detections_last,
+                                                     FeatureBasePtrList detections_incoming,
                                                      TrackMatrix& _trackMatrix)
 {
-    for (auto match : _matches_filtered)
+    for (auto match : _matches)
     {   
-        auto feat_last_itr = _features_out_last.begin();
-        auto feat_incoming_itr = _features_out_incoming.begin();
+        auto feat_last_itr = detections_last.begin();
+        auto feat_incoming_itr = detections_incoming.begin();
 
         std::advance(feat_last_itr, match.first);
         std::advance(feat_incoming_itr, match.second);
@@ -590,6 +625,11 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
         //New feature or track lost
         else 
         {
+
+            _trackMatrix.newTrack(feat_last);
+            _trackMatrix.add(feat_last, feat_incoming);
+
+
             //Check if the feature can be linked to a previous defined lmk
             
             //else we create a new track : _trackMatrix.newTrack(match.first) && _trackMatrix.add(match.first, match.second);
@@ -598,7 +638,7 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
     }
 }
 
-void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int>& _outliers_idx,
+void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<std::pair<int, int> >& _outliers_idx,
                                                            std::vector<std::pair<int,int> >& _matches)
 {
     //Number of matches
@@ -606,7 +646,7 @@ void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int
 
     for (int i = 0; i < nbMatches; i++)
     {   
-        int index_outlier_occurrence = std::count(_outliers_idx.begin(), _outliers_idx.end(), _matches[i].first);
+        int index_outlier_occurrence = std::count(_outliers_idx.begin(), _outliers_idx.end(), _matches[i]);
 
         //Check if the outlier is in matches
         if (index_outlier_occurrence != 0)
@@ -622,7 +662,7 @@ void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int
 
 }
 
-void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<int>& _inliers_idx,
+void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<std::pair<int, int> >& _inliers_idx,
                                                           std::vector<std::pair<int,int> >& _matches)
 {
     //Matches that will store only inliers
@@ -630,7 +670,7 @@ void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<int>
 
     for (auto pair_indexes : _matches)
     {
-        int index_inlier_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);
 
         //Check if the inlier is in matches
         if (index_inlier_occurrence != 0)
@@ -649,19 +689,19 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
                                                     const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
                                                     const std::vector<std::pair<int,int> >& _matches,
                                                     const double _ratio_outliers_inliers,
-                                                    std::vector<int>& _inliers_idx,
-                                                    std::vector<int>& _outliers_idx,
+                                                    std::vector<std::pair<int, int> >& _inliers_idx,
+                                                    std::vector<std::pair<int, int> >& _outliers_idx,
                                                     Eigen::Isometry3d& _best_model)
                                                     
 {
 
     //Check if the dataset has a sufficient size
-    if (nbOfDifferentMatches(_matches) < 3)
-        return false;
+    // if (nbOfDifferentMatches(_matches) < 3)
+    //     return false;
 
     // Vector that will contains index of inliers/outliers for each iteration
-    std::vector<int> inliers_idx_buff;
-    std::vector<int> outliers_idx_buff;
+    std::vector<std::pair<int, int> > inliers_idx_buff;
+    std::vector<std::pair<int, int> > outliers_idx_buff;
 
     //Number of inliers for the best model
     int best_nb_inliers = 0;
@@ -688,26 +728,31 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
             int index_feat_incomming_other = other_match.second;
 
             //Not necessary to test same pair of matches
-            if (index_feat_last != index_feat_last_other && index_feat_incoming != index_feat_incomming_other)
-            {
                 //Transformation between camera and object in last/incoming frame for the other match
-                Eigen::Isometry3d cl_M_ol = _cl_M_o_vec[index_feat_last_other];
-                Eigen::Isometry3d ci_M_oi = _ci_M_o_vec[index_feat_incomming_other];
-
+            Eigen::Isometry3d cl_M_ol = _cl_M_o_vec[index_feat_last_other];
+            Eigen::Isometry3d ci_M_oi = _ci_M_o_vec[index_feat_incomming_other];
+            
+            if (index_feat_last != index_feat_last_other || index_feat_incoming != index_feat_incomming_other)
+            {   
                 if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
                 {
                     nb_inliers++;
-                    inliers_idx_buff.push_back(index_feat_last_other);
+                    auto p = std::make_pair(index_feat_last_other, index_feat_incomming_other);
+                    inliers_idx_buff.push_back(p);
                 }
                 else
                 {
-                    outliers_idx_buff.push_back(index_feat_last_other);
+                    auto p = std::make_pair(index_feat_last_other, index_feat_incomming_other);
+                    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
-        inliers_idx_buff.push_back(index_feat_last);
+        auto p = std::make_pair(index_feat_last, index_feat_incoming);
+        inliers_idx_buff.push_back(p);
 
         //If the model gives better results
         if (nb_inliers > best_nb_inliers)
@@ -729,7 +774,7 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
     if ((double)nb_outliers/nb_inliers > _ratio_outliers_inliers)
         return true;
 
-    return false;
+    return true;
 }
 
 int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::pair<int,int> >& _matches)
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 743ac27..2b2b04c 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -310,13 +310,24 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceFactor)
     ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics");
 }
 
+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, matchingRANSAC)
 {
-    std::vector<Eigen::Isometry3d> cl_M_o_vec; 
-    std::vector<Eigen::Isometry3d> ci_M_o_vec; 
+    std::vector<Eigen::Isometry3d> cl_M_o_vec;
+    std::vector<Eigen::Isometry3d> ci_M_o_vec;
     std::vector<std::pair<int,int> > matches;
-    std::vector<int> inliers_idx;
-    std::vector<int> outliers_idx;
+    std::vector<std::pair<int, int> > inliers_idx;
+    std::vector<std::pair<int, int> > outliers_idx;
     Eigen::Isometry3d best_model;
 
     //Camera poses
@@ -357,7 +368,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     Eigen::Isometry3d w_M_o3 = posevec_to_isometry(pose_object_3);
     Eigen::Isometry3d w_M_o4 = posevec_to_isometry(pose_object_4);
     Eigen::Isometry3d w_M_o5 = posevec_to_isometry(pose_object_5);
-    
+
 
     Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
     Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
@@ -416,8 +427,8 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     ASSERT_MATRIX_APPROX(quat_cam.coeffs(), quat_cam_RANSAC.coeffs(), 1e-6)
 
     ASSERT_TRUE(outliers_idx.size() == 2);
-    ASSERT_TRUE(outliers_idx[0] == 2);
-    ASSERT_TRUE(outliers_idx[1] == 4);
+    ASSERT_TRUE(outliers_idx[0].first == 2);
+    ASSERT_TRUE(outliers_idx[1].first == 4);
 }
 
 TEST(ProcessorTrackerLandmarkObject, isInliers)
@@ -429,7 +440,7 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
     //Create 2 objects
     Vector7d pose_object_1;
     Vector7d pose_object_2;
-    
+
     //Give random values
     pose_cam_last     << 4.1,3.5,0,0.8,   2.7,1.3,1;
     pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
@@ -449,9 +460,9 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
     Eigen::Isometry3d w_M_o1 = posevec_to_isometry(pose_object_1);
     Eigen::Isometry3d w_M_o2 = posevec_to_isometry(pose_object_2);
     Eigen::Isometry3d w_M_o3 = Eigen::Isometry3d::Identity();
-    
+
     Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
-    
+
     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_cl.inverse() * w_M_o3;
@@ -511,22 +522,22 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
     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);
+    std::vector<std::pair<int,int> > outliers_idx1;
+    std::vector<std::pair<int,int> > outliers_idx2;
+    std::vector<std::pair<int,int> > outliers_idx3;
+
+    outliers_idx1.push_back(std::make_pair(2,2));
+    outliers_idx1.push_back(std::make_pair(5,5));
+    outliers_idx1.push_back(std::make_pair(9,9));
+    outliers_idx2.push_back(std::make_pair(0,0));
+    outliers_idx2.push_back(std::make_pair(9,9));
+    outliers_idx2.push_back(std::make_pair(10,10));
+    outliers_idx2.push_back(std::make_pair(11,11));
+    outliers_idx3.push_back(std::make_pair(4,4));
+    outliers_idx3.push_back(std::make_pair(5,5));
+    outliers_idx3.push_back(std::make_pair(6,6));
+    outliers_idx3.push_back(std::make_pair(7,7));
+    outliers_idx3.push_back(std::make_pair(8,8));
 
     //Create 12 matches
     auto pair_o1     = std::make_pair(0, 0);
@@ -571,8 +582,8 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
     ProcessorTrackerLandmarkObject::filterMatchesInliers(outliers_idx3, matches3I);
 
     ASSERT_TRUE(matches1.size() == 9);
-    ASSERT_TRUE(matches2.size() == 8);  
-    ASSERT_TRUE(matches3.size() == 7);  
+    ASSERT_TRUE(matches2.size() == 8);
+    ASSERT_TRUE(matches3.size() == 7);
 
     ASSERT_TRUE(matches1I.size() == 3);
     ASSERT_TRUE(matches2I.size() == 4);
@@ -580,6 +591,168 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
 
 }
 
+// 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(ProcessorTrackerLandmarkObject, allMatchesSameType)
+{
+    FeatureBasePtrList features_last;
+    FeatureBasePtrList features_incoming;
+
+    Eigen::Vector7d pose;
+    Eigen::Vector3d pos;
+    Eigen::Matrix6d meas_cov = Matrix6d::Identity();
+    std::string object_type;
+
+    // feature 0
+    pose << 0.8,2.5,2.3,2.5, 1.2,2.0,1.1;
+    pose.tail<4>() = pose.tail<4>().normalized();
+    Eigen::Isometry3d w_M_f0 = posevec_to_isometry(pose);
+    object_type = "type0";
+    FeatureBasePtr f0 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 1
+    pose << 0.5,3.5,3.2,1.5, 0.2,2.1,1.0;
+    pose.tail<4>() = pose.tail<4>().normalized();
+    Eigen::Isometry3d w_M_f1 = posevec_to_isometry(pose);
+    object_type = "type0";
+    FeatureBasePtr f1 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 2
+    pose << 1.5,3.2,1.2,2.0, 2.2,1.0,1.0;
+    pose.tail<4>() = pose.tail<4>().normalized();
+    Eigen::Isometry3d w_M_f2 = posevec_to_isometry(pose);
+    object_type = "type2";
+    FeatureBasePtr f2 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 3
+    pose << 2.1,1.1,2.1,1.0, 1.2,1.3,1.1;
+    pose.tail<4>() = pose.tail<4>().normalized();
+    Eigen::Isometry3d w_M_f3 = posevec_to_isometry(pose);
+    object_type = "type1";
+    FeatureBasePtr f3 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 4
+    pose << 1.9,4.2,8.2,3.0, 4.2,8.0,1.6;
+    pose.tail<4>() = pose.tail<4>().normalized();
+    Eigen::Isometry3d w_M_f4 = posevec_to_isometry(pose);
+    object_type = "type1";
+    FeatureBasePtr f4 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+    
+
+    Eigen::Vector7d pose_cam_last;
+    Eigen::Vector7d pose_cam_incoming;
+    pose_cam_last << 4.1,3.5,0,0.8,   2.7,1.3,1;
+    pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
+    pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
+    pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
+
+
+    Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
+
+
+    // feature 0
+    Eigen::Isometry3d ci_M_f0 = w_M_ci.inverse() * w_M_f0;
+    Eigen::Isometry3d new_w_M_f0  = w_M_ci * ci_M_f0;
+    Eigen::Quaterniond quat0(new_w_M_f0.linear());
+    pos = new_w_M_f0.translation();
+    pose << pos, quat0.coeffs();
+    pose.tail<4>() = pose.tail<4>().normalized();
+    object_type = "type0";
+    FeatureBasePtr new_f0 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 1
+    Eigen::Isometry3d ci_M_f1 = w_M_ci.inverse() * w_M_f1;
+    Eigen::Isometry3d new_w_M_f1  = w_M_ci * ci_M_f1;
+    Eigen::Quaterniond quat1(new_w_M_f1.linear());
+    pos = new_w_M_f1.translation();
+    pose << pos, quat1.coeffs();
+    pose.tail<4>() = pose.tail<4>().normalized();
+    object_type = "type0";
+    FeatureBasePtr new_f1 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 2
+    Eigen::Isometry3d ci_M_f2 = w_M_ci.inverse() * w_M_f2;
+    Eigen::Isometry3d new_w_M_f2  = w_M_ci * ci_M_f2;
+    Eigen::Quaterniond quat2(new_w_M_f2.linear());
+    pos = new_w_M_f2.translation();
+    pose << pos, quat2.coeffs();
+    pose.tail<4>() = pose.tail<4>().normalized();
+    object_type = "type2";
+    FeatureBasePtr new_f2 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 3
+    Eigen::Isometry3d new_w_M_f3  = Eigen::Isometry3d::Identity(); //outliers
+    Eigen::Quaterniond quat3(new_w_M_f3.linear());
+    pos = new_w_M_f3.translation();
+    pose << pos, quat3.coeffs();
+    pose.tail<4>() = pose.tail<4>().normalized();
+    object_type = "type0";
+    FeatureBasePtr new_f3 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 4
+    Eigen::Isometry3d new_w_M_f4  = Eigen::Isometry3d::Identity(); //outliers 
+    Eigen::Quaterniond quat4(new_w_M_f4.linear());
+    pos = new_w_M_f4.translation();
+    pose << pos, quat4.coeffs();
+    pose.tail<4>() = pose.tail<4>().normalized();
+    object_type = "type1";
+    FeatureBasePtr new_f4 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 5
+    Eigen::Isometry3d ci_M_f5 = w_M_ci.inverse() * w_M_f3;
+    Eigen::Isometry3d new_w_M_f5  = w_M_ci * ci_M_f5;
+    Eigen::Quaterniond quat5(new_w_M_f5.linear());
+    pos = new_w_M_f5.translation();
+    pose << pos, quat5.coeffs();
+    pose.tail<4>() = pose.tail<4>().normalized();
+    object_type = "type1";
+    FeatureBasePtr new_f5 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 6
+    Eigen::Isometry3d ci_M_f6 = w_M_ci.inverse() * w_M_f4;
+    Eigen::Isometry3d new_w_M_f6  = w_M_ci * ci_M_f6;
+    Eigen::Quaterniond quat6(new_w_M_f6.linear());
+    pos = new_w_M_f6.translation();
+    pose << pos, quat6.coeffs();
+    pose.tail<4>() = pose.tail<4>().normalized();
+    object_type = "type1";
+    FeatureBasePtr new_f6 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    features_last.push_back(f0); 
+    features_last.push_back(f1);
+    features_last.push_back(f2);
+    features_last.push_back(f3);
+    features_last.push_back(f4);
+
+    features_incoming.push_back(new_f0);  //go with f0
+    features_incoming.push_back(new_f1);  //go with f1
+    features_incoming.push_back(new_f2);  //go with f2
+    features_incoming.push_back(new_f3);  //outliers
+    features_incoming.push_back(new_f4);  //outliers
+    features_incoming.push_back(new_f5);  //go with f3
+    features_incoming.push_back(new_f6);  //go with f4
+
+
+    auto matches = ProcessorTrackerLandmarkObject::allMatchesSameType(features_last, features_incoming);
+    
+    //Matches has only inliers
+    ASSERT_TRUE(matches.size() == 5);
+    ASSERT_TRUE(matches[0] == std::make_pair(0,0));
+    ASSERT_TRUE(matches[1] == std::make_pair(1,1));
+    ASSERT_TRUE(matches[2] == std::make_pair(2,2));
+    ASSERT_TRUE(matches[3] == std::make_pair(3,5));
+    ASSERT_TRUE(matches[4] == std::make_pair(4,6));
+
+}
+
 
 
 int main(int argc, char **argv)
-- 
GitLab