Code refactored

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);

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();