diff --git a/include/objectslam/capture/capture_object.h b/include/objectslam/capture/capture_object.h
index 2ba085719fefb354a3d6b2f54497b00573a37e22..28d59207b0b1691c30a2e5f52ac387a940238beb 100644
--- a/include/objectslam/capture/capture_object.h
+++ b/include/objectslam/capture/capture_object.h
@@ -14,6 +14,8 @@
 
 namespace wolf {
 
+Isometry3d posevec_to_isometry(Vector7d pose);
+
 WOLF_STRUCT_PTR_TYPEDEFS(ParamsCapture);
 
 struct ParamsCapture : public ParamsBase
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index bd7c9a326f0500dbe1a39312454da2c7cf2e2e8d..b095df49b5583a1fb198c20f9426f89feeebb4d7 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -316,9 +316,9 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
         }
 
     // world to rob
-    Eigen::Isometry3d w_M_r = getw_M_r();
+    Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector());
     // rob to sensor
-    Eigen::Isometry3d r_M_c = getr_M_c();
+    Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector());
     
     if (_landmarks_in.size() == 0)
             return _features_out.size();
@@ -327,7 +327,7 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
     {
         std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
         // camera to feat
-        Eigen::Isometry3d c_M_f = getc_M_f(feat);
+        Eigen::Isometry3d c_M_f = posevec_to_isometry(feat->getMeasurement());
         
         if (feat->getCapture() != nullptr){
             break;