From 8804a4dc0f82912beaecfdb0261abd6978f5c48d Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Thu, 9 Jun 2022 10:10:36 +0200
Subject: [PATCH] Debug posevec
---
src/processor/processor_tracker_landmark_object.cpp | 5 ++---
1 file changed, 2 insertions(+), 3 deletions(-)
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index b095df4..852e625 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/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_)
{
--
GitLab