diff --git a/include/core/processor/processor_tracker_landmark_dummy.h b/include/core/processor/processor_tracker_landmark_dummy.h
index d39d44df0f6cb824a1054e6028ce8aa366f8d6e0..2aae554e0ff51b321723ad414effd3b584128b0a 100644
--- a/include/core/processor/processor_tracker_landmark_dummy.h
+++ b/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_ */
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index b47aa1a0e97ca50238c592a5694ec49c1637c4e2..68f9e1533dd0240e09ecd8f1f2ef6adc01240ead 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/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