Add ratio(outliers/inliers) parameter, function that count unique pair of matches and comments

include/objectslam/processor/processor_tracker_landmark_object.h

@@ -146,15 +146,32 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
                                                  FeatureBasePtrList& _features_out_last,
                                                  FeatureBasePtrList& _features_out_incoming);
 
-        // static std::pair<std::vector<int>, Eigen::Isometry3d> matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming);
-
         static bool 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,
                     std::vector<int>& inliers_idx,
                     std::vector<int>& outliers_idx,
-                    Eigen::Isometry3d& best_model);
+                    Eigen::Isometry3d& best_model,
+                    double ratio_outliers_inliers);
+        
+        /** \brief Count the number of different matches
+         *  \param matches A vector of pair of matches between last and incoming                           
+         *  
+         * A function which count number of unique pair (not the same index for last or for incoming)
+         * 
+         * \return an int
+         */
+        static int nbOfDifferentMatches(const std::vector<std::pair<int,int> >& matches);
 
+        /** \brief Check if three transformations correspond to an inlier
+         *  \param ci_M_oi camera pose isometry last to incoming
+         *  \param cl_M_ol an isometry camera to object (last frame)
+         *  \param ci_M_oi an isometry camera to object (incoming frame)                              
+         *  
+         * A function which check if ol_M_cl * cl_M_ci * ci_M_oi is similar to identity (if the object is an inlier)
+         * 
+         * \return A bool
+         */
         static bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
         
 

src/processor/processor_tracker_landmark_object.cpp

@@ -568,8 +568,14 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
                                                     const std::vector<std::pair<int,int> >& matches,
                                                     std::vector<int>& inliers_idx,
                                                     std::vector<int>& outliers_idx,
-                                                    Eigen::Isometry3d& best_model)
+                                                    Eigen::Isometry3d& best_model,
+                                                    double ratio_outliers_inliers)
 {   
+
+    //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<int> inliers_idx_buff;
     std::vector<int> outliers_idx_buff;
@@ -635,12 +641,34 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
     int nb_inliers = inliers_idx.size();  
     int nb_outliers = outliers_idx.size();  
 
-    if ((double)nb_outliers/nb_inliers > 0.55)
+    if ((double)nb_outliers/nb_inliers > ratio_outliers_inliers)
         return true;
 
     return false;
 }
 
+int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::pair<int,int> >& matches)
+{
+    int nb_of_diff_matches = 0;
+
+    //Vectors that will store new indexes
+    std::vector<int> index_last_feat;
+    std::vector<int> index_incoming_feat;
+
+    for (auto match : matches)
+    {   
+        //Check if the pair has unique indexes
+        if(!(std::count(index_last_feat.begin(), index_last_feat.end(), match.first)) && !(std::count(index_incoming_feat.begin(), index_incoming_feat.end(), match.second)))
+        {
+            nb_of_diff_matches++;
+            index_last_feat.push_back(match.first);
+            index_incoming_feat.push_back(match.second);
+        }
+    }
+
+    return nb_of_diff_matches;
+}
+
 
 bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci)
 {
@@ -661,10 +689,7 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen:
     Eigen::Matrix3d wRf_i = identity.linear();
     quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
     Vector3d pos_feat_identity = identity.translation();
-
-    std::cout << "\n\n\n\n\n" << quat_feat.coeffs().transpose() << " " << quat_feat_identity.coeffs().transpose() << std::endl;
-    std::cout << pos_feat << "\n" << pos_feat_identity << "\n\n\n\n\n" << std::endl;
-
+    
     // Error between identity and ol_M_oi
     double e_pos = (pos_feat_identity - pos_feat).norm();
     double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();

test/gtest_processor_tracker_landmark_object.cpp

@@ -397,7 +397,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
     matches.push_back(pair_o5);
 
     //Detect all outliers of our batch
-    ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model);
+    ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model, 0.55);
 
     Quaterniond quat_cam(cl_M_ci.linear());
     Vector3d pos_cam = cl_M_ci.translation();