diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h
index f16aa35f9ab1475ba5c2cc807de3fbb81dffc683..a68bac64af0ed2670cf37e6587fcaa5d7f945eb8 100644
--- a/include/core/processor/processor_tracker_feature_dummy.h
+++ b/include/core/processor/processor_tracker_feature_dummy.h
@@ -15,6 +15,21 @@
namespace wolf
{
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureDummy);
+
+struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature
+{
+ unsigned int loss_track_ratio; ///< ratio of loosing tracks one of each n
+
+ ProcessorParamsTrackerFeatureDummy() = default;
+ ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::paramsServer & _server):
+ ProcessorParamsTrackerFeature(_unique_name, _server)
+ {
+ loss_track_ratio = _server.getParam<unsigned int>(_unique_name + "/loss_track_ratio", "10");
+ }
+};
+
+
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
//Class
@@ -22,13 +37,14 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
{
public:
- ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
+ ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature);
virtual ~ProcessorTrackerFeatureDummy();
virtual void configure(SensorBasePtr _sensor) { };
protected:
static unsigned int count_;
+ ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_;
unsigned int n_feature_;
/** \brief Track provided features from \b last to \b incoming
@@ -69,10 +85,17 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
+ public:
+
+ static ProcessorBasePtr create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params,
+ const SensorBasePtr sensor_ptr = nullptr);
+
};
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature),
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
+ ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature_dummy),
+ params_tracker_feature_dummy_(_params_tracker_feature_dummy),
n_feature_(0)
{
//
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index c6776995796c55b82a70acd396ea0dca63237190..f213aed65206aac4d8cbb2cdff39262a955fa959 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -21,7 +21,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrLis
for (auto feat_in : _features_last_in)
{
- if (++count_ % 3 == 2) // lose one every 3 tracks
+ if (++count_ % params_tracker_feature_dummy_->loss_track_ratio == 0) // lose one every loss_track_ratio tracks
{
WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost!");
}
@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea
for (unsigned int i = 0; i < max_features; i++)
{
n_feature_++;
- FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
+ FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
n_feature_* Eigen::Vector1s::Ones(),
Eigen::MatrixXs::Ones(1, 1)));
_features_incoming_out.push_back(ftr);
@@ -78,14 +78,36 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea
}
FactorBasePtr ProcessorTrackerFeatureDummy::createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
+ FeatureBasePtr _feature_other_ptr)
{
WOLF_INFO( "creating factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
, " with origin feature " , _feature_other_ptr->id() );
- auto ctr = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr, shared_from_this());
+ auto fac = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr, shared_from_this());
- return ctr;
+ return fac;
}
+ProcessorBasePtr ProcessorTrackerFeatureDummy::create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params,
+ const SensorBasePtr)
+{
+ ProcessorParamsTrackerFeatureDummyPtr params = std::static_pointer_cast<ProcessorParamsTrackerFeatureDummy>(_params);
+
+ // if cast failed use default value
+ if (params == nullptr)
+ params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
+
+ ProcessorTrackerFeatureDummyPtr prc_ptr = std::make_shared<ProcessorTrackerFeatureDummy>(params);
+ prc_ptr->setName(_unique_name);
+
+ return prc_ptr;
+}
+
+} // namespace wolf
+
+// Register in the ProcessorFactory
+#include "core/processor/processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
} // namespace wolf
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 44fac86d12c9134180c83896d6906ee954a65abf..d36a14461c139e60be272f5f54c883e77ff35999 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -114,13 +114,7 @@ TEST(Problem, Installers)
SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
// install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = 0.1;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 10;
- // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
- auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(S, ProcessorTrackerFeatureDummy(params));
- // S->addProcessor(pt);
+ auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
// check motion processor IS NOT set
ASSERT_FALSE(P->getProcessorMotion());
@@ -250,13 +244,8 @@ TEST(Problem, StateBlocks)
SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = 0.1;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 10;
-
- auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
+ auto pm = P->installProcessor("ODOM 3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// 2 state blocks, estimated
auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
@@ -309,11 +298,9 @@ TEST(Problem, Covariances)
params->time_tolerance = 0.1;
params->max_new_features = 5;
params->min_features_for_keyframe = 10;
- // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
- auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
- // St->addProcessor(pt);
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
+ auto pm = P->installProcessor("ODOM 3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// 4 state blocks, estimated
St->unfixExtrinsics();
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index cff4bc083e1fa349e7986df8d6e4af1bb56b15f6..05ce862fddbdfb76937205e65d49471b0f324795 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -38,28 +38,18 @@ TEST(ProcessorBase, KeyFrameCallback)
ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
- // SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
- // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
- // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
- auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(), "FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+ auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SENSOR BASE",
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
- params->time_tolerance = dt/2;
- params->max_new_features = 5;
- params->min_features_for_keyframe = 5;
- // shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params);
- auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, params);
-
- // problem->addSensor(sens_trk);
- // sens_trk->addProcessor(proc_trk);
+ auto proc_trk = problem->installProcessor("TRACKER FEATURE DUMMY", "dummy", sens_trk);
// Install odometer (sensor and processor)
SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odometer", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odom processor", sens_odo, proc_odo_params);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;