Comment and add new function allMatchesSameType

include/objectslam/processor/processor_tracker_landmark_object.h

@@ -141,11 +141,42 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
         std::pair<double,double> errorsComputations(const Eigen::Isometry3d& _w_M_f, 
                                                     std::shared_ptr<wolf::LandmarkObject> _lmk_obj);
         
-
+        /** \brief Method for matching feature from last to incoming
+         * \param _capture the capture in which the _landmarks_in should be searched
+         * \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
+         * 
+                              
+         *  
+         * Match the best(smallest error) feature of the list that respect the given threshold
+         * 
+         * \return the number of landmarks found
+         */
         unsigned int multiviewTypeMatching(const CaptureBasePtr& _capture,
                                            FeatureBasePtrList& _features_out_last,
                                            FeatureBasePtrList& _features_out_incoming);
 
+
+        void allMatchesSameType(const FeatureBasePtrList& _features_out_last,
+                                const FeatureBasePtrList& _features_out_incoming);
+
+
+        /** \brief Method for matching feature from last to incoming
+         * \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
+         * \param _matches vector of indexes of matching feature from last to incoming
+         * \param _ratio_outliers_inliers threshold
+         * \param _inliers_idx vector of inliers 
+         * \param _outliers_idx vector of outliers
+         * \param _best_model isometry of best model
+         * 
+         * 
+                              
+         *  
+         * Match the best(smallest error) features between last and incoming of the list that respect the given threshold
+         * 
+         * \return the number of landmarks found
+         */
         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,
@@ -176,14 +207,36 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
                               const Eigen::Isometry3d& _ci_M_oi, 
                               const Eigen::Isometry3d& _cl_M_ci);
 
+
+        /** \brief Method for matching feature from last to incoming
+         * \param _matches_filtered vector of indexes of matching feature from last to incoming "without" outliers
+         * \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 _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,
                                     TrackMatrix& _trackMatrix);
         
-        static void filterMatchesOutliers(const std::vector<int>& outliers_idx, 
-                                          std::vector<std::pair<int,int> >& matches);
+        /** \brief Filter matches thanks to outliers vector
+         *  \param _outliers_idx vector of outliers
+         *  \param _matches vector of indexes of matching feature from last to incoming                            
+         *  
+         * A function which remove from matches the outliers
+         * 
+         */
+        static void filterMatchesOutliers(const std::vector<int>& _outliers_idx, 
+                                          std::vector<std::pair<int,int> >& _matches);
 
+        /** \brief Filter matches thanks to inliers vector
+         *  \param _inliers_idx vector of inliers
+         *  \param matches vector of indexes of matching feature from last to incoming                            
+         *  
+         * A function which create a vector based on _inliers_idx
+         * 
+         */
         static void filterMatchesInliers(const std::vector<int>& _inliers_idx,
                                          std::vector<std::pair<int,int> >& _matches);
 

src/processor/processor_tracker_landmark_object.cpp

@@ -496,6 +496,50 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
     return 1;
 }
 
+void 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;
+
+    for (auto feat_last : detections_last_)
+    {
+        std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
+
+        //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);
+
+        int index_incoming = 0;
+
+        for (auto feat_incoming : detections_incoming_)
+        {
+            std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
+
+            if (object_type_last == object_type_incoming)
+            {
+                //camera to feat
+                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);
+
+            }
+            index_incoming++;
+        }
+    }
+}
+
                                                       
 void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered, 
                                                      const FeatureBasePtrList& _features_out_last,
@@ -543,16 +587,21 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
 void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int>& _outliers_idx,
                                                            std::vector<std::pair<int,int> >& _matches)
 {
-    auto size = _matches.size();
+    //Number of matches
+    auto nbMatches = _matches.size();
 
-    for (int i = 0; i < size; i++)
+    for (int i = 0; i < nbMatches; i++)
     {   
         int index_outlier_occurrence = std::count(_outliers_idx.begin(), _outliers_idx.end(), _matches[i].first);
 
+        //Check if the outlier is in matches
         if (index_outlier_occurrence != 0)
-        {
+        {   
+            //Delete outiler from matches
             _matches.erase(std::remove(_matches.begin(), _matches.end(), _matches[i]));
-            size--;
+            //Matches has lost an element
+            nbMatches--;
+            //Decrement counter to avoid skipping elements
             i--;
         }
     }
@@ -562,19 +611,23 @@ void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int
 void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<int>& _inliers_idx,
                                                           std::vector<std::pair<int,int> >& _matches)
 {
+    //Matches that will store only inliers
     std::vector<std::pair<int,int> > matches_temp;
 
     for (auto pair_indexes : _matches)
     {
         int index_inlier_occurrence = std::count(_inliers_idx.begin(), _inliers_idx.end(), pair_indexes.first);
 
+        //Check if the inlier is in matches
         if (index_inlier_occurrence != 0)
-        {
+        {   
+            //Append inlier to matches
             matches_temp.push_back(pair_indexes);
         }
     }
 
     _matches.clear();
+    //Modify matches given in parameters
     _matches = matches_temp;
 }
 
@@ -639,6 +692,7 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
             }
         }
 
+        //Feature which provide the model is an inlier
         inliers_idx_buff.push_back(index_feat_last);
 
         //If the model gives better results

test/gtest_processor_tracker_landmark_object.cpp

@@ -478,11 +478,11 @@ TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
     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_o7     = std::make_pair(1, 4); //Not unique
+    auto pair_o8     = std::make_pair(3, 2); //Not unique
+    auto pair_o9     = std::make_pair(1, 6); //Not unique
+    auto pair_o10    = std::make_pair(4, 1); //Not unique
+    auto pair_o11    = std::make_pair(3, 7); //Not unique
     auto pair_o12    = std::make_pair(6, 7);
 
     //Append pairs in matches object
@@ -573,6 +573,7 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
     ASSERT_TRUE(matches1.size() == 9);
     ASSERT_TRUE(matches2.size() == 8);  
     ASSERT_TRUE(matches3.size() == 7);  
+
     ASSERT_TRUE(matches1I.size() == 3);
     ASSERT_TRUE(matches2I.size() == 4);
     ASSERT_TRUE(matches3I.size() == 5);