Finishing refa²ctoring of the code

include/objectslam/processor/processor_tracker_landmark_object.h

@@ -110,6 +110,11 @@ 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,

src/processor/processor_tracker_landmark_object.cpp

@@ -225,23 +225,17 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
     }
 
     // world to rob
-    Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
-    Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
-    Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob;
+    Eigen::Isometry3d w_M_r = getw_M_r();
 
     // rob to sensor
-    Vector3d pos_sen = getSensor()->getP()->getState();
-    Quaterniond quat_sen (getSensor()->getO()->getState().data());
-    Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
+    Eigen::Isometry3d r_M_c = getr_M_c();
 
     for (auto feat : detections_incoming_)
     {
         std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
 
         // camera to feat
-        Vector3d pos_obj = feat->getMeasurement().head(3);
-        Quaterniond quat_obj (feat->getMeasurement().tail(4).data());
-        Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj;
+        Eigen::Isometry3d c_M_f = getc_M_f(feat);
 
         if (feat->getCapture() != nullptr){
             break;
@@ -272,6 +266,27 @@ 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;
@@ -296,48 +311,36 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
                                                              FeatureBasePtrList&        _features_out,
                                                              LandmarkMatchMap&          _feature_landmark_correspondences)
 {
-    if (!last_ptr_)
+    if (!last_ptr_){
         return 0;
+        }
 
     // world to rob
-    Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
-    Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
-    Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob;
-
+    Eigen::Isometry3d w_M_r = getw_M_r();
     // rob to sensor
-    Vector3d pos_sen = getSensor()->getP()->getState();
-    Quaterniond quat_sen (getSensor()->getO()->getState().data());
-    Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
-
+    Eigen::Isometry3d r_M_c = getr_M_c();
+    
     if (_landmarks_in.size() == 0)
             return _features_out.size();
 
     for (auto feat : detections_incoming_)
     {
         std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
-
         // camera to feat
-        Vector3d pos_obj = feat->getMeasurement().head(3);
-        Quaterniond quat_obj (feat->getMeasurement().tail(4).data());
-        Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj;
-
+        Eigen::Isometry3d c_M_f = getc_M_f(feat);
+        
         if (feat->getCapture() != nullptr){
             break;
         }
-
-        std::vector<std::tuple<int, Vector3d, Quaterniond> > lmks;
+        
         int i = 0;
-        int index_lmks = 0;
         int index_min = -1;
         bool match = false;
-
         double e_pos_min = 100;
         double e_rot_min = 100;
-
         for (auto lmk : _landmarks_in)
         {   
             auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
-
             if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
             {
                 // world to feat
@@ -345,38 +348,37 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
 
                 auto error = errorsComputations(w_M_f, lmk_obj);
 
-                if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
+                if (error.first < e_pos_thr_ && error.second < e_rot_thr_)
+                {
+                    if (error.first < e_pos_min && error.second < e_rot_min)
                     {
 
-                        if (error.first < e_pos_min && error.second < e_rot_min)
-                        {
-                            e_pos_min = error.first;
-                            e_rot_min = error.second;
-                            index_min = std::get<0>(lmks[index_lmks]); 
-                            match = true;
-                        } 
-                    }
-                    index_lmks++;
+                        std::cout << std::endl << std::endl << std::endl << std::endl << error.first << "  " << error.second << std::endl << std::endl;
+
+                        e_pos_min = error.first;
+                        e_rot_min = error.second;
+                        index_min = i; 
+                        match = true;
+                    } 
                 }
-                i++;
             }
-
+            i++;
+        }
+            
             if (match)
             {
                 auto itr_lmk = _landmarks_in.begin(); 
                 std::advance(itr_lmk, index_min);
-
                 _features_out.push_back(feat);
                 double score(1.0);  // not used
                 LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(*itr_lmk, score);
                 _feature_landmark_correspondences.emplace (feat, matched_landmark);
                 feat->link(_capture); // since all features are created in preProcess() are unlinked 
+                match =  false;
             }
         }
 
-    
-    }
-
+    std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
     return _features_out.size();
 
 }