diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h index a68bac64af0ed2670cf37e6587fcaa5d7f945eb8..150a6ffb0068b723c9f237918b81dc56116323e2 100644 --- a/include/core/processor/processor_tracker_feature_dummy.h +++ b/include/core/processor/processor_tracker_feature_dummy.h @@ -19,13 +19,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureDummy); 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(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"); + n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost", "1"); } }; @@ -43,9 +43,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature protected: - static unsigned int count_; ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_; - unsigned int n_feature_; /** \brief Track provided features from \b last to \b incoming * \param _features_last_in input list of features in \b last to track @@ -95,8 +93,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature 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) + params_tracker_feature_dummy_(_params_tracker_feature_dummy) { // } diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp index f213aed65206aac4d8cbb2cdff39262a955fa959..999f8620792dd83a7e34cb295ec4cf32049568ad 100644 --- a/src/processor/processor_tracker_feature_dummy.cpp +++ b/src/processor/processor_tracker_feature_dummy.cpp @@ -11,19 +11,19 @@ namespace wolf { -unsigned int ProcessorTrackerFeatureDummy::count_ = 0; - unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { WOLF_INFO("tracking " , _features_last_in.size() , " features..."); + auto count = 0; 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!"); + count++; } else { @@ -63,9 +63,8 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea // detecting new features for (unsigned int i = 0; i < max_features; i++) { - n_feature_++; FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE", - n_feature_* Eigen::Vector1s::Ones(), + Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1))); _features_incoming_out.push_back(ftr); diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp index dba7c3a6967ec1ad600bdf49cde50a3505f1103a..02e24d90b19dc6a9537ccfbb3a7434887ce6c48e 100644 --- a/test/gtest_processor_tracker_feature_dummy.cpp +++ b/test/gtest_processor_tracker_feature_dummy.cpp @@ -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); } + void callEstablishFactors(){ this->establishFactors();} + TrackMatrix getTrackMatrix(){ return track_matrix_; } FeatureMatchMap getMatchesLastFromIncoming() { return matches_last_from_incoming_; } @@ -69,7 +71,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test params->min_features_for_keyframe = 7; params->time_tolerance = 0.25; 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->link(sensor); } @@ -107,7 +109,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures) processor->callDetectNewFeatures(params->max_new_features, feat_list); // 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); //test trackFeatures @@ -125,33 +127,32 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processNew) processor->setLast(last_cap); // 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); auto n_new_feat = processor->callProcessNew(10); // detect 10 features ASSERT_EQ(n_new_feat, 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->getMatchesLastFromIncoming().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 track lost } TEST_F(ProcessorTrackerFeatureDummyTest, processKnown) { - // 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>(0.1, sensor); + CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor); 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->getIncoming()->getFeatureList().size(), 14); // 1 of each 10 tracks is lost - ASSERT_EQ(processor->getMatchesLastFromIncoming().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 track lost processor->callReset(); // now incoming is last and last is origin @@ -162,13 +163,12 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown) auto n_tracked_feat = processor->callProcessKnown(); 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(processor->getIncoming()->getFeatureList().size(), 13); // 1 of each 10 tracks is lost - ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 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 track lost + ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 13); // 1 track lost } - -TEST_F(ProcessorTrackerFeatureDummyTest, createFactors) +TEST_F(ProcessorTrackerFeatureDummyTest, createFactor) { FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE", Eigen::Vector1s::Ones(), @@ -180,7 +180,58 @@ TEST_F(ProcessorTrackerFeatureDummyTest, createFactors) FactorBasePtr fac = processor->callCreateFactor(ftr, ftr_other); fac->link(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->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) @@ -188,16 +239,15 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process) //1ST TIME -> KF (origin) WOLF_DEBUG("First time..."); - CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0.0, sensor); + CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor); cap1->process(); ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr); - ASSERT_DOUBLE_EQ(problem->getTrajectory()->getLastKeyFrame()->getTimeStamp().get(), 0.0); - ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame()); + ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap1->getFrame()->id()); //2ND TIME WOLF_DEBUG("Second time..."); - CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(0.1, sensor); + CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor); cap2->process(); ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features); @@ -205,25 +255,25 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process) //3RD TIME WOLF_DEBUG("Third time..."); - CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(0.2, sensor); + CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor); cap3->process(); ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2); //4TH TIME WOLF_DEBUG("Forth time..."); - CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(0.3, sensor); + CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor); cap4->process(); ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3); //5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe)) WOLF_DEBUG("Fifth time..."); - CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(0.4, sensor); + CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor); cap5->process(); - ASSERT_DOUBLE_EQ(problem->getTrajectory()->getLastKeyFrame()->getTimeStamp().get(), 0.3); - ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame()); + ASSERT_TRUE(cap4->getFrame()->isKey()); + ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id()); // check factors WOLF_DEBUG("checking factors...");