Merge branch 'devel' into 3-adapt-to-state-derived-in-core

include/objectslam/processor/processor_tracker_landmark_object.h

@@ -142,8 +142,26 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
                                            FeatureBasePtrList& _features_out_incoming);
 
 
-        static std::vector<std::pair<int,int> > allMatchesSameType(const FeatureBasePtrList& _features_out_last,
-                                                                   const FeatureBasePtrList& _features_out_incoming);
+        static void keepInliers(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
+                                const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
+                                std::vector<std::pair<int,int> >& matches,
+                                Eigen::Isometry3d& _best_model);
+
+
+        /** \brief Method for create all pair of features (same type) from last and incoming
+         * \param _features_out_last returned list of features  found in \b _capture corresponding to a landmark of _landmarks_in
+         * \param _features_out_incoming returned list of features  found in incoming capture corresponding to a landmark of _landmarks_in
+         * \param _cl_M_o_vec vector of camera to object isometry for last capture
+         * \param _ci_M_o_vec vector of camera to object isometry for incoming capture
+         *              
+
+         * 
+         * \return All matches available for those two lists
+         */
+        static std::vector<std::pair<int,int> > createAllMatchesSameType(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);
 
 
         /** \brief Method for matching feature from last to incoming

src/processor/processor_tracker_landmark_object.cpp

@@ -511,14 +511,39 @@ std::ostream& operator<<(std::ostream &flux, std::vector<int> vector)
     return flux;
 }
 
-std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList& _features_out_last,
-                                                                                    const FeatureBasePtrList& _features_out_incoming)
+void ProcessorTrackerLandmarkObject::keepInliers(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
+                                                 const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
+                                                 std::vector<std::pair<int,int> >& matches,
+                                                 Eigen::Isometry3d& _best_model)
+{   
+    //Vector that will store inliers/outliers pairs
+    std::vector<std::pair<int,int> > inliers_idx;
+    std::vector<std::pair<int,int> > outliers_idx;
+
+    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::filterMatchesOutliers(outliers_idx, matches);
+        
+    }
+}
+
+std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::createAllMatchesSameType(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<Eigen::Isometry3d> cl_M_o_vec;
-    std::vector<Eigen::Isometry3d> ci_M_o_vec;
-    std::vector<std::pair<int,int> > matches;
+    //Vector that store all matches available
+    std::vector<std::pair<int,int> > all_matches;
 
     int index_last = 0;
+
+    //First pass throught the loop
     bool pushBack = true;
 
     for (auto feat_last : _features_out_last)
@@ -528,60 +553,43 @@ std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameT
         //camera to feat (last)
         Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
         Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
-
         Eigen::Isometry3d cl_M_o = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
-        cl_M_o_vec.push_back(cl_M_o);
+        _cl_M_o_vec.push_back(cl_M_o);
 
         int index_incoming = 0;
 
         for (auto feat_incoming : _features_out_incoming)
         {
             std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
-
+            
+            //Create a pair only if features have same types
             if (object_type_last == object_type_incoming)
             {
                 std::pair<int, int> pair_indexes = std::make_pair(index_last, index_incoming);
-                matches.push_back(pair_indexes);
+                all_matches.push_back(pair_indexes);
             }
             
-            //camera to feat
+            //if first pass throught the loop
             if (pushBack)
             {
-
+        
+                //camera to feat (incoming)  
                 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);
+                _ci_M_o_vec.push_back(ci_M_o);
             }
 
             index_incoming++;
         }
 
         index_last++;
-        pushBack = false;
 
+        //Not first pass throught the loop
+        pushBack = false;
     }
 
-    // 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, 0.10, inliers_idx, outliers_idx, best_model);
-
-    if (RANSACWorks)
-    {
-        std::cout << "RANSAC has worked" << std::endl;
-
-        //Keep only inliers
-        ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
-        
-    }
-
-    return matches;
+    return all_matches;
 }
 
                                                       
@@ -766,8 +774,10 @@ 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 > _ratio_outliers_inliers)
+    if ((double)nb_outliers/nb_inliers > _ratio_outliers_inliers) {
         return true;
+    }
+
 
     return true;
 }