Skip to content
Snippets Groups Projects
Commit 378e23c0 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

added create and factory to processorTrackerFeatureDummy and adapted gtests

parent fa891a82
No related branches found
No related tags found
1 merge request!278Resolve "Revisit demos (formerly called examples) and update them"
...@@ -15,6 +15,21 @@ ...@@ -15,6 +15,21 @@
namespace wolf 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); WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
//Class //Class
...@@ -22,13 +37,14 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature ...@@ -22,13 +37,14 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
{ {
public: public:
ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature); ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature);
virtual ~ProcessorTrackerFeatureDummy(); virtual ~ProcessorTrackerFeatureDummy();
virtual void configure(SensorBasePtr _sensor) { }; virtual void configure(SensorBasePtr _sensor) { };
protected: protected:
static unsigned int count_; static unsigned int count_;
ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_;
unsigned int n_feature_; unsigned int n_feature_;
/** \brief Track provided features from \b last to \b incoming /** \brief Track provided features from \b last to \b incoming
...@@ -69,10 +85,17 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature ...@@ -69,10 +85,17 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); 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) : inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature), ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature_dummy),
params_tracker_feature_dummy_(_params_tracker_feature_dummy),
n_feature_(0) n_feature_(0)
{ {
// //
......
...@@ -21,7 +21,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrLis ...@@ -21,7 +21,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrLis
for (auto feat_in : _features_last_in) 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!"); WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost!");
} }
...@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea ...@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea
for (unsigned int i = 0; i < max_features; i++) for (unsigned int i = 0; i < max_features; i++)
{ {
n_feature_++; n_feature_++;
FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
n_feature_* Eigen::Vector1s::Ones(), n_feature_* Eigen::Vector1s::Ones(),
Eigen::MatrixXs::Ones(1, 1))); Eigen::MatrixXs::Ones(1, 1)));
_features_incoming_out.push_back(ftr); _features_incoming_out.push_back(ftr);
...@@ -78,14 +78,36 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea ...@@ -78,14 +78,36 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea
} }
FactorBasePtr ProcessorTrackerFeatureDummy::createFactor(FeatureBasePtr _feature_ptr, 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() WOLF_INFO( "creating factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
, " with origin feature " , _feature_other_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 } // namespace wolf
...@@ -114,13 +114,7 @@ TEST(Problem, Installers) ...@@ -114,13 +114,7 @@ TEST(Problem, Installers)
SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml"); 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) // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
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);
// check motion processor IS NOT set // check motion processor IS NOT set
ASSERT_FALSE(P->getProcessorMotion()); ASSERT_FALSE(P->getProcessorMotion());
...@@ -250,13 +244,8 @@ TEST(Problem, StateBlocks) ...@@ -250,13 +244,8 @@ TEST(Problem, StateBlocks)
SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, ""); SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2)); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
params->time_tolerance = 0.1; auto pm = P->installProcessor("ODOM 3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
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");
// 2 state blocks, estimated // 2 state blocks, estimated
auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0); auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
...@@ -309,11 +298,9 @@ TEST(Problem, Covariances) ...@@ -309,11 +298,9 @@ TEST(Problem, Covariances)
params->time_tolerance = 0.1; params->time_tolerance = 0.1;
params->max_new_features = 5; params->max_new_features = 5;
params->min_features_for_keyframe = 10; 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); auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml"); auto pm = P->installProcessor("ODOM 3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// 4 state blocks, estimated // 4 state blocks, estimated
St->unfixExtrinsics(); St->unfixExtrinsics();
......
...@@ -38,28 +38,18 @@ TEST(ProcessorBase, KeyFrameCallback) ...@@ -38,28 +38,18 @@ TEST(ProcessorBase, KeyFrameCallback)
ProblemPtr problem = Problem::create("PO", 2); ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor) // Install tracker (sensor and processor)
// SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
// std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), "SENSOR BASE",
// std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(), "FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); auto proc_trk = problem->installProcessor("TRACKER FEATURE DUMMY", "dummy", sens_trk);
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);
// Install odometer (sensor and processor) // Install odometer (sensor and processor)
SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), ""); SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>(); ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
proc_odo_params->time_tolerance = dt/2; 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; std::cout << "sensor & processor created and added to wolf problem" << std::endl;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment