From 2f4b1db2615a997a1954ea4ac1d272eaf9092911 Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Thu, 9 Jun 2022 11:27:47 +0200
Subject: [PATCH] Code refactored
---
.../processor_tracker_landmark_object.h | 6 ----
.../processor_tracker_landmark_object.cpp | 33 ++++---------------
2 files changed, 7 insertions(+), 32 deletions(-)
diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 5df5ab9..46efd0c 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -110,13 +110,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
std::pair<double,double> errorsComputations(Eigen::Isometry3d _w_M_f,
std::shared_ptr<wolf::LandmarkObject> _lmk_obj);
-
- Eigen::Isometry3d getw_M_r();
- Eigen::Isometry3d getr_M_c();
- Eigen::Isometry3d getc_M_f(FeatureBasePtr _feat);
-
-
unsigned int multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 852e625..ba8d3db 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -225,16 +225,18 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
}
// world to rob
- Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector());
+
+ Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
+
// rob to sensor
- Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector());
+ Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
for (auto feat : detections_incoming_)
{
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;
@@ -265,27 +267,6 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
return _features_out.size();
}
-Eigen::Isometry3d ProcessorTrackerLandmarkObject::getw_M_r()
-{
- Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
- Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
- return Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob;
-}
-
-Eigen::Isometry3d ProcessorTrackerLandmarkObject::getr_M_c()
-{
- Vector3d pos_sen = getSensor()->getP()->getState();
- Quaterniond quat_sen (getSensor()->getO()->getState().data());
- return Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
-}
-
-Eigen::Isometry3d ProcessorTrackerLandmarkObject::getc_M_f(FeatureBasePtr _feat)
-{
- Vector3d pos_obj = _feat->getMeasurement().head(3);
- Quaterniond quat_obj (_feat->getMeasurement().tail(4).data());
- return Eigen::Translation<double,3>(pos_obj) * quat_obj;
-}
-
std::pair<double,double> ProcessorTrackerLandmarkObject::errorsComputations(Eigen::Isometry3d _w_M_f, std::shared_ptr<wolf::LandmarkObject> _lmk_obj)
{
Quaterniond quat_feat;
@@ -315,9 +296,9 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
}
// world to rob
- Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector());
+ Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
// rob to sensor
- Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector());
+ Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
if (_landmarks_in.size() == 0)
return _features_out.size();
--
GitLab