diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index e954ec6807301a2776b179b9b5c1f712ea8d57d9..aee02d389e14e1fa3c7b7fae18ba54f2e6a87db0 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -85,7 +85,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
         void preProcess() override;
         void postProcess() override;
 
-        /** \brief Find provided landmarks as features in the provided capture
+        /** \brief Select which method for matching(first or best) folowing user parameter
          * \param _landmarks_in input list of landmarks to be found
          * \param _capture the capture in which the _landmarks_in should be searched
          * \param _features_out returned list of features  found in \b _capture corresponding to a landmark of _landmarks_in
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 790d230a1ba43f20a4a4daef59dcbb08940b5338..0b99ceef5af59889331f75b5d70dfa0cbdfdd408 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -61,6 +61,7 @@ void ProcessorTrackerLandmarkObject::postProcess()
     if (keyframe_voted_){ 
         // Only if a keyframe is voted so that a suppressed landmark is not assigned to a factor 
         for (auto lmk: getProblem()->getMap()->getLandmarkList()){
+
             auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
             double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get();
             int number_of_factors = lmk_obj->getConstrainedByList().size();
@@ -103,10 +104,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _
     Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
 
     // camera to lmk
-    Vector3d pos = _feature_ptr->getMeasurement().head(3);
-    Quaterniond quat;
-    quat.coeffs() = _feature_ptr->getMeasurement().tail(4);
-    Eigen::Isometry3d c_M_t = Eigen::Translation<double,3>(pos) * quat;
+    Eigen::Isometry3d c_M_t = posevec_to_isometry(_feature_ptr->getMeasurement());
 
     // world to lmk
     Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t;
@@ -245,13 +243,13 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
             //Try matching only if lmk and feat have the same type
             if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
             {
-                // world to feat
+                //world to feat
                 Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
                 
                 //Create a pair that stock both position and rotation error
                 auto error = errorsComputations(w_M_f, lmk_obj);
 
-                //Test if the error is below the thresholds 
+                //Test errors are below thresholds 
                 if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
                     _features_out.push_back(feat);
                     double score(1.0);  // not used
@@ -383,11 +381,13 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
 {
     int ftr_size;
 
+    //First lmk in list (lower than threshold) is matched
     if (first_lmk_matching_)
     {
         ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
     }
-        
+
+    //Best lmk in list (lower than threshold) is matched   
     else
     {
         ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
@@ -406,14 +406,12 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
         return 0;
     }
 
-    // world to rob
+    //world to rob
     Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
 
-    Vector3d pos_rob_incoming = getIncoming()->getFrame()->getP()->getState();
-    Quaterniond quat_rob_incoming (getIncoming()->getFrame()->getO()->getState().data());
-    Eigen::Isometry3d w_M_r_incoming = Eigen::Translation<double,3>(pos_rob_incoming.head(3)) * quat_rob_incoming;
+    Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
 
-    // rob to sensor
+    //rob to sensor
     Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
 
     
@@ -435,7 +433,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
     {
         std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
 
-        // camera to feat
+        //camera to feat
         Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
         Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
         Eigen::Isometry3d c_M_f_last = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;