diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 4e3103540572024874edde73b63ee42582f129df..827226fba38e862722445282e091681e922c0f8e 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -23,7 +23,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
     double e_rot_thr_;
     int fps_;
     Matrix3d intrinsic_;
-    bool   first_lmk_method_;
+    bool   first_lmk_matching_;
 
     ParamsProcessorTrackerLandmarkObject() = default;
     ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
@@ -31,7 +31,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
     {
         min_time_vote_              = _server.getParam<double>(prefix + _unique_name                    + "/keyframe_vote/min_time_vote");
         max_time_vote_              = _server.getParam<double>(prefix + _unique_name                    + "/keyframe_vote/max_time_vote");
-        nb_vote_for_every_first_    = _server.getParam<int>(prefix + _unique_name                       + "/keyframe_vote/nb_vote_for_every_first");
+        nb_vote_for_every_first_    = _server.getParam<int>   (prefix + _unique_name                    + "/keyframe_vote/nb_vote_for_every_first");
         lmk_score_thr_              = _server.getParam<double>(prefix + _unique_name                    + "/lmk_score_thr");
         e_pos_thr_                  = _server.getParam<double>(prefix + _unique_name                    + "/e_pos_thr");
         e_rot_thr_                  = _server.getParam<double>(prefix + _unique_name                    + "/e_rot_thr");
@@ -40,7 +40,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
         double fy                   = _server.getParam<double>(prefix + _unique_name                    + "/intrinsic/fy");
         double cx                   = _server.getParam<double>(prefix + _unique_name                    + "/intrinsic/cx");
         double cy                   = _server.getParam<double>(prefix + _unique_name                    + "/intrinsic/cy");
-        first_lmk_method_           = _server.getParam<bool>(prefix + _unique_name                    + "/first_lmk_method");
+        first_lmk_matching_         = _server.getParam<bool>  (prefix + _unique_name                    + "/first_lmk_matching");
         intrinsic_ << fx, 0, cx,
                  0, fy, cy,
                  0, 0, 1;
@@ -96,7 +96,12 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
                                            FeatureBasePtrList& _features_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences) override;
 
-        unsigned int findLandmarksBis(const LandmarkBasePtrList& _landmarks_in,
+        unsigned int firstLmkMatching(const LandmarkBasePtrList& _landmarks_in,
+                                           const CaptureBasePtr& _capture,
+                                           FeatureBasePtrList& _features_out,
+                                           LandmarkMatchMap& _feature_landmark_correspondences);
+
+        unsigned int bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
                                            const CaptureBasePtr& _capture,
                                            FeatureBasePtrList& _features_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
@@ -168,7 +173,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
         int             nb_vote_;
         int             fps_;
         Matrix3d        intrinsic_;
-        bool            first_lmk_method_;
+        bool            first_lmk_matching_;
 
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 04845b2bc2e899ccd9725ed744cd34f9a9cd2832..f65503802d6e53328cc542bfa1d0a4af6b86f37a 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -23,7 +23,7 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT
         nb_vote_(0),
         fps_(_params_tracker_landmark_object->fps_),
         intrinsic_(_params_tracker_landmark_object->intrinsic_),
-        first_lmk_method_(_params_tracker_landmark_object->first_lmk_method_)
+        first_lmk_matching_(_params_tracker_landmark_object->first_lmk_matching_)
 
 {
     std::cout << _params_tracker_landmark_object->print() << std::endl;
@@ -214,180 +214,82 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
     return false;
 }
 
-unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
-                                                             const CaptureBasePtr& _capture,
-                                                             FeatureBasePtrList& _features_out,
-                                                             LandmarkMatchMap& _feature_landmark_correspondences)
+unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBasePtrList& _landmarks_in,
+                                                              const CaptureBasePtr&      _capture,
+                                                              FeatureBasePtrList&        _features_out,
+                                                              LandmarkMatchMap&          _feature_landmark_correspondences)
 {
-    if (first_lmk_method_)
-    {
-        for (auto feat : detections_incoming_)
-        {
-            std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
-
-            if (feat->getCapture() != nullptr){
-                break;
-            }
-
-            for (auto lmk : _landmarks_in)
-            {   
-                auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
-
-                if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
-                {
-                    
-                    // world to rob
-                    Vector3d pos = getLast()->getFrame()->getP()->getState();
-                    Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
-                    Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
-
-                    // rob to sensor
-                    pos = getSensor()->getP()->getState();
-                    quat.coeffs() = getSensor()->getO()->getState();
-                    Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
-
-                    // camera to feat
-                    pos = feat->getMeasurement().head(3);
-                    quat.coeffs() = feat->getMeasurement().tail(4);
-                    Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
-
-                    // world to feat
-                    Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
-                    Quaterniond quat_feat;
-                    Eigen::Matrix3d wRf = w_M_f.linear();
-                    quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
-                    Vector3d pos_feat = w_M_f.translation();
-
-                    // world to lmk
-                    Vector3d pos_lmk = lmk_obj->getP()->getState();
-                    Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
-
-                    // Error computation
-                    double e_pos = (pos_lmk - pos_feat).norm();
-                    double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
-
-                    if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_){
-                        _features_out.push_back(feat);
-                        double score(1.0);
-                        LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
-                        _feature_landmark_correspondences.emplace (feat, matched_landmark);
-                        feat->link(_capture); // since all features are created in preProcess() are unlinked 
-                        break;  
-                    }
-                }
-            }
-        }
-    
-    }
-    else
-    {
-        if (!last_ptr_){
+    // This is the right thing to test but SEGFAULTS!
+    if (!last_ptr_){
         return 0;
-        }
-
-        // world to rob
-        Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
-        Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
-        Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob;
-
-        // rob to sensor
-        Vector3d pos_sen = getSensor()->getP()->getState();
-        Quaterniond quat_sen (getSensor()->getO()->getState().data());
-        Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
-
-        if (_landmarks_in.size() == 0)
-                return _features_out.size();
-
-        for (auto feat : detections_incoming_)
-        {
-            std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
-
-            // camera to feat
-            Vector3d pos_obj = feat->getMeasurement().head(3);
-            Quaterniond quat_obj (feat->getMeasurement().tail(4).data());
-            Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj;
+    }
 
-            if (feat->getCapture() != nullptr){
-                break;
-            }
+    // world to rob
+    Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
+    Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
+    Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob;
 
-            std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
-            int i = 0;
-            int index_lmks = 0;
-            int index_min = -1;
-            bool match = false;
+    // rob to sensor
+    Vector3d pos_sen = getSensor()->getP()->getState();
+    Quaterniond quat_sen (getSensor()->getO()->getState().data());
+    Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
 
-            double e_pos_min = 100;
-            double e_rot_min = 100;
+    for (auto feat : detections_incoming_)
+    {
+        std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
 
-            for (auto lmk : _landmarks_in)
-            {   
-                auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+        // camera to feat
+        Vector3d pos_obj = feat->getMeasurement().head(3);
+        Quaterniond quat_obj (feat->getMeasurement().tail(4).data());
+        Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj;
 
-                if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
-                {
-                    // world to feat
-                    Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
-                    Quaterniond quat_feat;
-                    Eigen::Matrix3d wRf = w_M_f.linear();
-                    quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
-                    Vector3d pos_feat = w_M_f.translation();
+        if (feat->getCapture() != nullptr){
+            break;
+        }
 
-                    // world to lmk
-                    Vector3d pos_lmk = lmk_obj->getP()->getState();
-                    Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
+        for (auto lmk : _landmarks_in)
+        {   
+            auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
 
-                    auto t = std::make_tuple(i, pos_lmk, quat_lmk);
-                    lmks.push_back(t);
+            if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
+            {
+                // world to feat
+                Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
+                Quaterniond quat_feat;
+                Eigen::Matrix3d wRf = w_M_f.linear();
+                quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
+                Vector3d pos_feat = w_M_f.translation();
 
-                    // Error computation
-                    double e_pos = (pos_lmk - pos_feat).norm();
-                    double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
+                // world to lmk
+                Vector3d pos_lmk = lmk_obj->getP()->getState();
+                Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
 
-                    if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_)
-                    {
+                // Error computation
+                double e_pos = (pos_lmk - pos_feat).norm();
+                double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
 
-                        if (e_pos < e_pos_min && e_rot < e_rot_min)
-                        {
-                            e_pos_min = e_pos;
-                            e_rot_min = e_rot;
-                            index_min = std::get<0>(lmks[index_lmks]); 
-                            match = true;
-                        } 
-                    }
-                    index_lmks++;
+                if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_){
+                    _features_out.push_back(feat);
+                    double score(1.0);  // not used
+                    LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
+                    _feature_landmark_correspondences.emplace (feat, matched_landmark);
+                    feat->link(_capture); // since all features are created in preProcess() are unlinked 
+                    break;  
                 }
-                i++;
-            }
-
-            if (match)
-            {
-                auto itr_lmk = _landmarks_in.begin(); 
-                std::advance(itr_lmk, index_min);
-
-                _features_out.push_back(feat);
-                double score(1.0);  // not used
-                LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(*itr_lmk, score);
-                _feature_landmark_correspondences.emplace (feat, matched_landmark);
-                feat->link(_capture); // since all features are created in preProcess() are unlinked 
             }
         }
-
     }
-    std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
     return _features_out.size();
 }
 
 
-unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBasePtrList& _landmarks_in,
-                                                             const CaptureBasePtr& _capture,
-                                                             FeatureBasePtrList& _features_out,
-                                                             LandmarkMatchMap& _feature_landmark_correspondences)
+unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
+                                                             const CaptureBasePtr&      _capture,
+                                                             FeatureBasePtrList&        _features_out,
+                                                             LandmarkMatchMap&          _feature_landmark_correspondences)
 {
-    // This is the right thing to test but SEGFAULTS!
-    if (!last_ptr_){
+    if (!last_ptr_)
         return 0;
-    }
 
     // world to rob
     Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
@@ -476,127 +378,31 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarksBis(const LandmarkBase
             feat->link(_capture); // since all features are created in preProcess() are unlinked 
         }
     }
-    
-    std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
+
     return _features_out.size();
 }
+                                                         
 
-// unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
-//                                                              const CaptureBasePtr& _capture,
-//                                                              FeatureBasePtrList& _features_out,
-//                                                              LandmarkMatchMap& _feature_landmark_correspondences)
-// {
-//     std::cout << std::endl << std::endl << std::endl << std::endl << "test11" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
-    
-//     if (!last_ptr_){
-//         return 0;
-//     }
-
-//     // world to rob
-//     Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
-//     Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
-//     Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob;
-
-//     // rob to sensor
-//     Vector3d pos_sen = getSensor()->getP()->getState();
-//     Quaterniond quat_sen (getSensor()->getO()->getState().data());
-//     Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
-//     std::cout << std::endl << std::endl << std::endl << std::endl << "test" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
-
-
-//     std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
-
-//     std::cout << std::endl << std::endl << std::endl << std::endl << "test2" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
-
-
-//     int i = 0;
-
-//     std::cout << std::endl << std::endl << std::endl << std::endl << _landmarks_in.size() << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
-
-
-//     for (auto lmk : _landmarks_in)
-//     {
-//         std::cout << std::endl << std::endl << std::endl << std::endl << "test3" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
-
-//         auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
-
-//         std::cout << std::endl << std::endl << std::endl << std::endl << "test4" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
-
-
-//         if(lmk_obj != nullptr)
-//         {
-//             Vector3d pos_lmk(lmk_obj->getP()->getState());
-//             Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
-//             auto t = std::make_tuple(i, pos_lmk, quat_lmk);
-//             lmks.push_back(t);
-//             std::cout << std::endl << std::endl << std::endl << std::endl << std::get<0>(lmks[i]) << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
-//         }
-
-//         i++;
-//     }
-
-//     for (auto feat : detections_incoming_)
-//     {
-//         std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
-
-//         // camera to feat
-//         Vector3d pos_obj = feat->getMeasurement().head(3);
-//         Quaterniond quat_obj (feat->getMeasurement().tail(4).data());
-//         Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj;
-
-//         if (feat->getCapture() != nullptr){
-//             break;
-//         }
-
-//         double e_pos_min = 100;
-//         double e_rot_min = 100;
-//         int    index_min = -1;
-
-//         for (auto lmk : lmks)
-//         {   
-//             auto itr_lmk = _landmarks_in.begin(); 
-//             std::advance(itr_lmk, std::get<0>(lmk));
-
-//             auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*itr_lmk);
-
-//             if(lmk_obj->getObjectType() == object_type)
-//             {
-//                 // world to feat
-//                 Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
-//                 Quaterniond quat_feat;
-//                 Eigen::Matrix3d wRf = w_M_f.linear();
-//                 quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
-//                 Vector3d pos_feat = w_M_f.translation();
-
-//                 // Error computation
-//                 double e_pos = (std::get<1>(lmk) - pos_feat).norm();
-//                 double e_rot = log_q(std::get<2>(lmk).conjugate() * quat_feat).norm();
-
-//                 if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_)
-//                 {
-//                     if (e_pos < e_pos_min && e_rot < e_rot_min)
-//                     {
-//                         e_pos_min = e_pos;
-//                         e_rot_min = e_rot;
-//                         index_min = std::get<0>(lmk); 
-//                     }
-//                 }
-//             }
-//         }
-
-//         auto itr_lmk = _landmarks_in.begin(); 
-//         std::advance(itr_lmk, index_min);
-
-//         _features_out.push_back(feat);
-//         double score(1.0);  // not used
-//         LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(*itr_lmk, score);
-//         _feature_landmark_correspondences.emplace (feat, matched_landmark);
-//         feat->link(_capture); // since all features are created in preProcess() are unlinked  
-//     }
-//     std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
-//     return _features_out.size();
-// }
+unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+                                                           const CaptureBasePtr& _capture,
+                                                           FeatureBasePtrList& _features_out,
+                                                           LandmarkMatchMap& _feature_landmark_correspondences)
+{
+    int ftr_size;
+    if (first_lmk_matching_)
+    {
+        ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
+    }
+        
+    else
+    {
+        ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
+    }
 
+        
+    std::cout << "Features Matched :" << ftr_size << std::endl;
+    return ftr_size;
+}
 
 unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
                                                                          FeatureBasePtrList& _features_out_last,