processor_tracker_landmark_dummy working with LandmarkBase and FactorLandmarkDummy

include/core/processor/processor_tracker_landmark_dummy.h

@@ -13,22 +13,37 @@
 namespace wolf
 {
 
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkDummy);
+
+struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandmark
+{
+    unsigned int loss_lmk_ratio; ///< ratio of loosing lmks one of each n
+
+    ProcessorParamsTrackerLandmarkDummy() = default;
+    ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::paramsServer & _server):
+        ProcessorParamsTrackerLandmark(_unique_name, _server)
+    {
+        loss_lmk_ratio = _server.getParam<unsigned int>(_unique_name + "/loss_lmk_ratio", "10");
+    }
+};
+
 WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
 
 class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
 {
     public:
-        ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
+        ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
         virtual ~ProcessorTrackerLandmarkDummy();
         virtual void configure(SensorBasePtr _sensor) { };
 
     protected:
 
+        ProcessorParamsTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
         unsigned int n_feature_;
-        unsigned int landmark_idx_non_visible_;
 
-//        virtual void preProcess() { }
-        virtual void postProcess(); // implemented
+
+        //virtual void preProcess() { }
+        //virtual void postProcess();
 
         /** \brief Find provided landmarks in the incoming capture
          * \param _landmarks_in input list of landmarks to be found in incoming
@@ -74,20 +89,6 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
 };
 
-inline void ProcessorTrackerLandmarkDummy::postProcess()
-{
-    landmark_idx_non_visible_++;
-    std::cout << "------- Landmarks until " << landmark_idx_non_visible_ << " are now out of scope" << std::endl
-            << std::endl;
-}
-
-} // namespace wolf
-
-// IMPLEMENTATION
-
-namespace wolf
-{
-
 } // namespace wolf
 
 #endif /* PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ */

src/processor/processor_tracker_landmark_dummy.cpp

@@ -6,16 +6,16 @@
  */
 
 #include "core/processor/processor_tracker_landmark_dummy.h"
-#include "core/landmark/landmark_corner_2D.h"
-#include "core/factor/factor_corner_2D.h"
+#include "core/landmark/landmark_base.h"
+#include "core/factor/factor_landmark_dummy.h"
 
 namespace wolf
 {
 
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
-        ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark),
-        n_feature_(0),
-        landmark_idx_non_visible_(0)
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
+        ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark_dummy),
+        params_tracker_landmark_dummy_(_params_tracker_landmark_dummy),
+        n_feature_(0)
 {
     //
 
@@ -33,23 +33,19 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
     std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks"  << std::endl;
     std::cout << "\t\t"  << _landmarks_in.size() << " landmarks..." << std::endl;
 
-    // loosing the track of the first 2 features
-    auto landmarks_lost = 0;
+    // loosing the track of the first landmark_idx_non_visible_ features
     for (auto landmark_in_ptr : _landmarks_in)
     {
-        if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_)
-        {
-            landmarks_lost++;
+        if (landmark_in_ptr->id() % params_tracker_landmark_dummy_->loss_lmk_ratio == 0)
             std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " lost!" << std::endl;
-        }
         else
         {
-            _features_incoming_out.push_back(std::make_shared<FeatureBase>(
-                    "POINT IMAGE",
-                    landmark_in_ptr->getDescriptor(),
-                    Eigen::MatrixXs::Identity(1,1)));
-            _feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
-            std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl;
+            FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                             n_feature_* Eigen::Vector1s::Ones(),
+                                                             Eigen::MatrixXs::Ones(1, 1)));
+            _features_incoming_out.push_back(ftr);
+            _feature_landmark_correspondences[ftr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
+            std::cout << "\t\tlandmark " << landmark_in_ptr->id() << " found!" << std::endl;
         }
     }
     return _features_incoming_out.size();
@@ -74,31 +70,37 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_fe
         max_features = 10;
         WOLF_INFO("max_features unlimited, setting it to " , max_features);
     }
+    WOLF_INFO("Detecting " , _max_features , " new features..." );
 
     // detecting new features
-    for (unsigned int i = 1; i <= max_features; i++)
+    for (unsigned int i = 0; i < max_features; i++)
     {
         n_feature_++;
-        _features_incoming_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE",
-                                                                       n_feature_ * Eigen::Vector1s::Ones(),
-                                                                       Eigen::MatrixXs::Ones(1, 1)));
-        std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
+        FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                         n_feature_* Eigen::Vector1s::Ones(),
+                                                         Eigen::MatrixXs::Ones(1, 1)));
+        _features_incoming_out.push_back(ftr);
+
+        WOLF_INFO("feature " , ftr->id() , " detected!" );
     }
+
+    WOLF_INFO(_features_incoming_out.size() , " features detected!");
+
     return _features_incoming_out.size();
 }
 
 LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
 {
     //std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
-    return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), _feature_ptr->getMeasurement(0));
+    return std::make_shared<LandmarkBase>("BASE", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
 }
 
 FactorBasePtr ProcessorTrackerLandmarkDummy::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
 {
     std::cout << "\tProcessorTrackerLandmarkDummy::createFactor" << std::endl;
-    std::cout << "\t\tfeature " << _feature_ptr->getMeasurement() << std::endl;
-    std::cout << "\t\tlandmark "<< _landmark_ptr->getDescriptor() << std::endl;
-    return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr), shared_from_this());
+    std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
+    std::cout << "\t\tlandmark "<< _landmark_ptr->id() << std::endl;
+    return std::make_shared<FactorLandmarkDummy>(_feature_ptr, _landmark_ptr, shared_from_this());
 }
 
 } //namespace wolf