Debug posevec

src/processor/processor_tracker_landmark_object.cpp

@@ -225,10 +225,9 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
     }
 
     // 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());
 
     for (auto feat : detections_incoming_)
     {