From c308264e1b7e82d69a6a5144146b859928031832 Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Wed, 8 Jun 2022 16:15:20 +0200
Subject: [PATCH] Debug overloaded function

---
 include/objectslam/capture/capture_object.h         | 2 ++
 src/processor/processor_tracker_landmark_object.cpp | 6 +++---
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/include/objectslam/capture/capture_object.h b/include/objectslam/capture/capture_object.h
index 2ba0857..28d5920 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 bd7c9a3..b095df4 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;
-- 
GitLab