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

simplified a little PTFDummy and improved gtest

parent ce8aa09f
No related branches found
No related tags found
1 merge request!278Resolve "Revisit demos (formerly called examples) and update them"
...@@ -19,13 +19,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureDummy); ...@@ -19,13 +19,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureDummy);
struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature
{ {
unsigned int loss_track_ratio; ///< ratio of loosing tracks one of each n unsigned int n_tracks_lost; ///< number of tracks lost each time track is called (the first ones)
ProcessorParamsTrackerFeatureDummy() = default; ProcessorParamsTrackerFeatureDummy() = default;
ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::paramsServer & _server): ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::paramsServer & _server):
ProcessorParamsTrackerFeature(_unique_name, _server) ProcessorParamsTrackerFeature(_unique_name, _server)
{ {
loss_track_ratio = _server.getParam<unsigned int>(_unique_name + "/loss_track_ratio", "10"); n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost", "1");
} }
}; };
...@@ -43,9 +43,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature ...@@ -43,9 +43,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
protected: protected:
static unsigned int count_;
ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_; ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_;
unsigned int n_feature_;
/** \brief Track provided features from \b last to \b incoming /** \brief Track provided features from \b last to \b incoming
* \param _features_last_in input list of features in \b last to track * \param _features_last_in input list of features in \b last to track
...@@ -95,8 +93,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature ...@@ -95,8 +93,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) : inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature_dummy), ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature_dummy),
params_tracker_feature_dummy_(_params_tracker_feature_dummy), params_tracker_feature_dummy_(_params_tracker_feature_dummy)
n_feature_(0)
{ {
// //
} }
......
...@@ -11,19 +11,19 @@ ...@@ -11,19 +11,19 @@
namespace wolf namespace wolf
{ {
unsigned int ProcessorTrackerFeatureDummy::count_ = 0;
unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_last_in, unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_last_in,
FeatureBasePtrList& _features_incoming_out, FeatureBasePtrList& _features_incoming_out,
FeatureMatchMap& _feature_correspondences) FeatureMatchMap& _feature_correspondences)
{ {
WOLF_INFO("tracking " , _features_last_in.size() , " features..."); WOLF_INFO("tracking " , _features_last_in.size() , " features...");
auto count = 0;
for (auto feat_in : _features_last_in) for (auto feat_in : _features_last_in)
{ {
if (++count_ % params_tracker_feature_dummy_->loss_track_ratio == 0) // lose one every loss_track_ratio tracks if (count < params_tracker_feature_dummy_->n_tracks_lost) // lose first ntrackslost tracks
{ {
WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost!"); WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost!");
count++;
} }
else else
{ {
...@@ -63,9 +63,8 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea ...@@ -63,9 +63,8 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea
// detecting new features // detecting new features
for (unsigned int i = 0; i < max_features; i++) for (unsigned int i = 0; i < max_features; i++)
{ {
n_feature_++;
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE", FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
n_feature_* Eigen::Vector1s::Ones(), 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);
......
...@@ -33,6 +33,8 @@ class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy ...@@ -33,6 +33,8 @@ class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return this->createFactor(_feature_ptr, _feature_other_ptr); } FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return this->createFactor(_feature_ptr, _feature_other_ptr); }
void callEstablishFactors(){ this->establishFactors();}
TrackMatrix getTrackMatrix(){ return track_matrix_; } TrackMatrix getTrackMatrix(){ return track_matrix_; }
FeatureMatchMap getMatchesLastFromIncoming() { return matches_last_from_incoming_; } FeatureMatchMap getMatchesLastFromIncoming() { return matches_last_from_incoming_; }
...@@ -69,7 +71,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test ...@@ -69,7 +71,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
params->min_features_for_keyframe = 7; params->min_features_for_keyframe = 7;
params->time_tolerance = 0.25; params->time_tolerance = 0.25;
params->voting_active = true; params->voting_active = true;
params->loss_track_ratio = 10; params->n_tracks_lost = 1; // 1 (the first) track is lost each time trackFeatures is called
processor = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params); processor = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
processor->link(sensor); processor->link(sensor);
} }
...@@ -107,7 +109,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures) ...@@ -107,7 +109,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
processor->callDetectNewFeatures(params->max_new_features, feat_list); processor->callDetectNewFeatures(params->max_new_features, feat_list);
// Put a capture on incoming_ptr_ // Put a capture on incoming_ptr_
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(0.1, sensor); CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
processor->setInc(inc_cap); processor->setInc(inc_cap);
//test trackFeatures //test trackFeatures
...@@ -125,33 +127,32 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processNew) ...@@ -125,33 +127,32 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
processor->setLast(last_cap); processor->setLast(last_cap);
// Put a capture on incoming_ptr_ // Put a capture on incoming_ptr_
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(0.1, sensor); CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
processor->setInc(inc_cap); processor->setInc(inc_cap);
auto n_new_feat = processor->callProcessNew(10); // detect 10 features auto n_new_feat = processor->callProcessNew(10); // detect 10 features
ASSERT_EQ(n_new_feat, 10); // detected 10 features ASSERT_EQ(n_new_feat, 10); // detected 10 features
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 tracks is lost ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 9); // 1 of each 10 tracks is lost ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 9); // 1 track lost
} }
TEST_F(ProcessorTrackerFeatureDummyTest, processKnown) TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
{ {
// Put a capture on last_ptr_ // Put a capture on last_ptr_
CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor); CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
processor->setLast(last_cap); processor->setLast(last_cap);
// Put a capture on incoming_ptr_ // Put a capture on incoming_ptr_
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(0.1, sensor); CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
processor->setInc(inc_cap); processor->setInc(inc_cap);
processor->callProcessNew(15); // detect 15 features, 1 of each 10 tracks is lost processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 of each 10 tracks is lost ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 of each 10 tracks is lost ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
processor->callReset(); // now incoming is last and last is origin processor->callReset(); // now incoming is last and last is origin
...@@ -162,13 +163,12 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown) ...@@ -162,13 +163,12 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
auto n_tracked_feat = processor->callProcessKnown(); auto n_tracked_feat = processor->callProcessKnown();
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 14); // 14 previously tracked features ASSERT_EQ(processor->getLast()->getFeatureList().size(), 14); // 14 previously tracked features
ASSERT_EQ(n_tracked_feat, 13); // 1 of each 10 tracks is lost ASSERT_EQ(n_tracked_feat, 13); // 1 track lost
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 13); // 1 of each 10 tracks is lost ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 13); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 13); // 1 of each 10 tracks is lost ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 13); // 1 track lost
} }
TEST_F(ProcessorTrackerFeatureDummyTest, createFactor)
TEST_F(ProcessorTrackerFeatureDummyTest, createFactors)
{ {
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE", FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
Eigen::Vector1s::Ones(), Eigen::Vector1s::Ones(),
...@@ -180,7 +180,58 @@ TEST_F(ProcessorTrackerFeatureDummyTest, createFactors) ...@@ -180,7 +180,58 @@ TEST_F(ProcessorTrackerFeatureDummyTest, createFactors)
FactorBasePtr fac = processor->callCreateFactor(ftr, ftr_other); FactorBasePtr fac = processor->callCreateFactor(ftr, ftr_other);
fac->link(ftr); fac->link(ftr);
ASSERT_EQ(fac->getFeature(),ftr); ASSERT_EQ(fac->getFeature(),ftr);
ASSERT_EQ(fac->getFrameOther(),nullptr);
ASSERT_EQ(fac->getCaptureOther(),nullptr);
ASSERT_EQ(fac->getFeatureOther(),ftr_other); ASSERT_EQ(fac->getFeatureOther(),ftr_other);
ASSERT_EQ(fac->getLandmarkOther(),nullptr);
}
TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
{
// Put a capture on last_ptr_
CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
processor->setLast(last_cap);
// Put a capture on incoming_ptr_
CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
processor->setInc(inc_cap);
processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
processor->callReset(); // now incoming is last and last is origin
// test establishFactors()
processor->callEstablishFactors();
TrackMatrix track_matrix = processor->getTrackMatrix();
unsigned int n_factors_ori = 0;
unsigned int n_factors_last = 0;
for (auto feat : processor->getLast()->getFeatureList())
{
if (!feat->getFactorList().empty())
{
n_factors_last++;
ASSERT_EQ(feat->getFactorList().size(), 1);
ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
ASSERT_EQ(feat->getFactorList().front()->getFeatureOther(), track_matrix.feature(feat->trackId(), processor->getOrigin()));
}
}
for (auto feat : processor->getOrigin()->getFeatureList())
{
if (!feat->getConstrainedByList().empty())
{
n_factors_ori++;
ASSERT_EQ(feat->getConstrainedByList().size(), 1);
ASSERT_EQ(feat->getConstrainedByList().front()->getFeatureOther(), feat);
ASSERT_EQ(feat->getConstrainedByList().front()->getFeature(), track_matrix.feature(feat->trackId(), processor->getLast()));
}
}
ASSERT_EQ(n_factors_ori, 14);
ASSERT_EQ(n_factors_last, 14);
} }
TEST_F(ProcessorTrackerFeatureDummyTest, process) TEST_F(ProcessorTrackerFeatureDummyTest, process)
...@@ -188,16 +239,15 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process) ...@@ -188,16 +239,15 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
//1ST TIME -> KF (origin) //1ST TIME -> KF (origin)
WOLF_DEBUG("First time..."); WOLF_DEBUG("First time...");
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0.0, sensor); CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process(); cap1->process();
ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr); ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
ASSERT_DOUBLE_EQ(problem->getTrajectory()->getLastKeyFrame()->getTimeStamp().get(), 0.0); ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap1->getFrame()->id());
ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame());
//2ND TIME //2ND TIME
WOLF_DEBUG("Second time..."); WOLF_DEBUG("Second time...");
CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(0.1, sensor); CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
cap2->process(); cap2->process();
ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features); ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
...@@ -205,25 +255,25 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process) ...@@ -205,25 +255,25 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
//3RD TIME //3RD TIME
WOLF_DEBUG("Third time..."); WOLF_DEBUG("Third time...");
CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(0.2, sensor); CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
cap3->process(); cap3->process();
ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2); ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2);
//4TH TIME //4TH TIME
WOLF_DEBUG("Forth time..."); WOLF_DEBUG("Forth time...");
CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(0.3, sensor); CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
cap4->process(); cap4->process();
ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3); ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);
//5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe)) //5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
WOLF_DEBUG("Fifth time..."); WOLF_DEBUG("Fifth time...");
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(0.4, sensor); CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process(); cap5->process();
ASSERT_DOUBLE_EQ(problem->getTrajectory()->getLastKeyFrame()->getTimeStamp().get(), 0.3); ASSERT_TRUE(cap4->getFrame()->isKey());
ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame()); ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id());
// check factors // check factors
WOLF_DEBUG("checking factors..."); WOLF_DEBUG("checking factors...");
......
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