diff --git a/include/core/processor/processor_tracker_landmark_dummy.h b/include/core/processor/processor_tracker_landmark_dummy.h index d45ea755e43abe2c1911b10b641e77ace8afcefe..804ecc9869789b9326d0b572c210884c8f9d15b3 100644 --- a/include/core/processor/processor_tracker_landmark_dummy.h +++ b/include/core/processor/processor_tracker_landmark_dummy.h @@ -17,7 +17,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkDummy); struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandmark { - unsigned int n_landmarks_lost; ///< number of landmarks lost each time findLandmarks is called (the first ones) + unsigned int n_landmarks_lost; ///< number of landmarks lost each time findLandmarks is called (the last ones) ProcessorParamsTrackerLandmarkDummy() = default; ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::paramsServer & _server): diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp index a7fabea185985422196aa59049141de052a45d57..61540a8afec1b4c204e59b92fca645efde1cd4ca 100644 --- a/src/processor/processor_tracker_landmark_dummy.cpp +++ b/src/processor/processor_tracker_landmark_dummy.cpp @@ -33,15 +33,12 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL std::cout << "\t\t" << _landmarks_in.size() << " landmarks..." << std::endl; // loosing the track of the first landmark_idx_non_visible_ features + auto prev_found = matches_landmark_from_last_.size(); + if (prev_found == 0) prev_found = _landmarks_in.size(); auto count = 0; for (auto landmark_in_ptr : _landmarks_in) { - if (count < params_tracker_landmark_dummy_->n_landmarks_lost) // lose first n_landmarks_lost landmarks - { - WOLF_INFO("landmark: " , landmark_in_ptr->id() , " lost!"); - count++; - } - else + if (count < prev_found - params_tracker_landmark_dummy_->n_landmarks_lost) // lose last n_landmarks_lost landmarks { FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE", Eigen::Vector1s::Ones(), @@ -50,6 +47,11 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL _feature_landmark_correspondences[ftr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1); std::cout << "\t\tlandmark " << landmark_in_ptr->id() << " found!" << std::endl; } + else + { + WOLF_INFO("landmark: " , landmark_in_ptr->id() , " lost!"); + } + count++; } return _features_incoming_out.size(); } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 635c6878ad14ad5cac68dd3a781480e8a375fcef..2bdc605a30dc5f70f4028c6c5007f8b731b48467 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -194,6 +194,10 @@ target_link_libraries(gtest_processor_odom_3D ${PROJECT_NAME}) wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp) target_link_libraries(gtest_processor_tracker_feature_dummy ${PROJECT_NAME}) +# ProcessorTrackerLandmarkDummy class test +wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp) +target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME}) + # yaml conversions wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp) target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME}) diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a5f3f5945b13320516d713ab30f535383f273d55 --- /dev/null +++ b/test/gtest_processor_tracker_landmark_dummy.cpp @@ -0,0 +1,349 @@ + +// wolf includes +#include "core/utils/utils_gtest.h" +#include "core/sensor/sensor_factory.h" +#include "core/processor/processor_tracker_landmark_dummy.h" +#include "core/capture/capture_void.h" + +using namespace wolf; + +std::string wolf_root = _WOLF_ROOT_DIR; + +WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummyDummy); + +class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy +{ + public: + + ProcessorTrackerLandmarkDummyDummy(ProcessorParamsTrackerLandmarkDummyPtr& _params) : + ProcessorTrackerLandmarkDummy(_params){} + + void setLast(CaptureBasePtr _last_ptr){ last_ptr_ = _last_ptr; } + void setInc(CaptureBasePtr _incoming_ptr){ incoming_ptr_ = _incoming_ptr; } + + unsigned int callProcessKnown(){ return this->processKnown(); } + + unsigned int callProcessNew(const int& _max_new_features){ return this->processNew(_max_new_features); } + + unsigned int callDetectNewFeatures(const int& _max_features, + FeatureBasePtrList& _features_incoming_out){ return this->detectNewFeatures(_max_features, + _features_incoming_out); } + + unsigned int callFindLandmarks(const LandmarkBasePtrList& _landmarks_in, + FeatureBasePtrList& _features_incoming_out, + LandmarkMatchMap& _feature_landmark_correspondences){ return this->findLandmarks(_landmarks_in, + _features_incoming_out, + _feature_landmark_correspondences); } + + LandmarkBasePtr callCreateLandmark(FeatureBasePtr _feature_ptr){ return this->createLandmark(_feature_ptr); } + void callCreateNewLandmarks(){ this->createNewLandmarks(); } + FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr, + LandmarkBasePtr _landmark_ptr){ return this->createFactor(_feature_ptr, + _landmark_ptr); } + + void callEstablishFactors(){ this->establishFactors();} + + void setNewFeaturesLast(FeatureBasePtrList& _new_features_list){ new_features_last_ = _new_features_list;} + + LandmarkMatchMap getMatchesLandmarkFromIncoming() { return matches_landmark_from_incoming_;} + LandmarkMatchMap getMatchesLandmarkFromLast() { return matches_landmark_from_last_;} + LandmarkBasePtrList getNewLandmarks() { return new_landmarks_;} + + void callReset() + { + this->resetDerived(); + origin_ptr_ = last_ptr_; + last_ptr_ = incoming_ptr_; + incoming_ptr_ = nullptr; + }; +}; + +// Use the following in case you want to initialize tests with predefines variables or methods. +class ProcessorTrackerLandmarkDummyTest : public testing::Test +{ + public: + ProblemPtr problem; + SensorBasePtr sensor; + ProcessorParamsTrackerLandmarkDummyPtr params; + ProcessorTrackerLandmarkDummyDummyPtr processor; + + virtual void SetUp() + { + // Wolf problem + problem = Problem::create("PO", 2); + + // Install camera + sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>()); + + // Install processor + params = std::make_shared<ProcessorParamsTrackerLandmarkDummy>(); + params->max_new_features = 10; + params->min_features_for_keyframe = 7; + params->time_tolerance = 0.25; + params->voting_active = true; + params->n_landmarks_lost = 1; // 1 (the last) landmark is not found each time findLandmark is called + processor = std::make_shared<ProcessorTrackerLandmarkDummyDummy>(params); + processor->link(sensor); + } +}; + +TEST_F(ProcessorTrackerLandmarkDummyTest, installProcessor) +{ + ASSERT_EQ(processor->getProblem(), problem); + ASSERT_TRUE(problem->check(0)); +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, detectNewFeatures) +{ + // Put a capture on last_ptr_ + CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor); + processor->setLast(last_cap); + + FeatureBasePtrList feat_list; + + // demo callDetectNewFeatures + unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list); + ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features + ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, createLandmark) +{ + // Put a capture on last_ptr_ + CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor); + processor->setLast(last_cap); + + FeatureBasePtrList feat_list; + + // demo callDetectNewFeatures + unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list); + ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features + ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features + + for (auto ftr : feat_list) + { + auto lmk = processor->callCreateLandmark(ftr); + lmk->link(problem->getMap()); + } + ASSERT_EQ(problem->getMap()->getLandmarkList().size(),n_feat); // created 10 landmarks +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, createNewLandmarks) +{ + // Put a capture on last_ptr_ + CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor); + processor->setLast(last_cap); + + FeatureBasePtrList feat_list; + + // test detectNewFeatures + unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list); + ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features + ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features + + // test createNewLandmarks + processor->setNewFeaturesLast(feat_list); + processor->callCreateNewLandmarks(); + ASSERT_EQ(processor->getNewLandmarks().size(),n_feat); // created 10 landmarks +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks) +{ + // Put a capture on last_ptr_ + CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor); + processor->setLast(last_cap); + + FeatureBasePtrList feat_list; + + // test detectNewFeatures + unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list); + ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features + ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features + + // test createNewLandmarks + processor->setNewFeaturesLast(feat_list); + processor->callCreateNewLandmarks(); + LandmarkBasePtrList new_landmarks = processor->getNewLandmarks(); + ASSERT_EQ(new_landmarks.size(),n_feat); // created 10 landmarks + + //test findLandmarks + LandmarkMatchMap feature_landmark_correspondences; + FeatureBasePtrList feat_found; + processor->callFindLandmarks(new_landmarks, feat_found, feature_landmark_correspondences); + ASSERT_EQ(feature_landmark_correspondences.size(), feat_found.size()); + ASSERT_EQ(feat_list.size(), feat_found.size()+1); // one of each 10 tracks is lost +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, processNew) +{ + // 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); + + 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(problem->getMap()->getLandmarkList().size(), 10); // created 10 landmarks + ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found + ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(), 10); // all last features have the landmark correspondence + ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown) +{ + // create 10 landmarks and link them to map + FeatureBasePtrList feat_list; + processor->callDetectNewFeatures(params->max_new_features, feat_list); + for (auto ftr : feat_list) + { + auto lmk = processor->callCreateLandmark(ftr); + lmk->link(problem->getMap()); + } + ASSERT_EQ(problem->getMap()->getLandmarkList().size(),10); // created 10 landmarks + + // Put a capture on incoming_ptr_ + CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor); + processor->setInc(inc_cap); + + // Test processKnown + processor->callProcessKnown(); + + ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found + ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, createFactor) +{ + FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE", + Eigen::Vector1s::Ones(), + Eigen::MatrixXs::Ones(1, 1))); + LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("BASE", + std::make_shared<StateBlock>(1), + std::make_shared<StateBlock>(1))); + + FactorBasePtr fac = processor->callCreateFactor(ftr, lmk); + fac->link(ftr); + ASSERT_EQ(fac->getFeature(),ftr); + ASSERT_EQ(fac->getFrameOther(),nullptr); + ASSERT_EQ(fac->getCaptureOther(),nullptr); + ASSERT_EQ(fac->getFeatureOther(),nullptr); + ASSERT_EQ(fac->getLandmarkOther(),lmk); +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, 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 of each 10 tracks is lost + + 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->getMatchesLandmarkFromLast().size(), 15); // all landmarks + ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 14); // 1 of each 10 tracks is lost + + // test establishFactors() + processor->callEstablishFactors(); + LandmarkMatchMap landmark_from_last = processor->getMatchesLandmarkFromLast(); + unsigned int n_factors_last = 0; + unsigned int n_factors_landmark = 0; + for (auto feat : processor->getLast()->getFeatureList()) + { + n_factors_last++; + ASSERT_EQ(feat->getFactorList().size(), 1); + ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat); + ASSERT_EQ(feat->getFactorList().front()->getLandmarkOther(), landmark_from_last[feat]->landmark_ptr_); + } + + for (auto lmk : problem->getMap()->getLandmarkList()) + { + n_factors_landmark++; + ASSERT_EQ(lmk->getConstrainedByList().size(), 1); + ASSERT_EQ(lmk->getConstrainedByList().front()->getLandmarkOther(), lmk); + ASSERT_EQ(landmark_from_last[lmk->getConstrainedByList().front()->getFeature()]->landmark_ptr_, lmk); + } + ASSERT_EQ(n_factors_last, 15); + ASSERT_EQ(n_factors_landmark, 15); +} + +TEST_F(ProcessorTrackerLandmarkDummyTest, process) +{ + + //1ST TIME -> KF (origin) + WOLF_DEBUG("First time..."); + CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor); + cap1->process(); + + ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr); + ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame()); + + //2ND TIME + WOLF_DEBUG("Second time..."); + CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor); + cap2->process(); + + ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features); + ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-1); + + //3RD TIME + WOLF_DEBUG("Third time..."); + 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>(3, sensor); + cap4->process(); + + ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3); + + //5TH TIME -> KF in cap4 (found landmarks < 7 (params->min_features_for_keyframe)) + WOLF_DEBUG("Fifth time..."); + CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor); + cap5->process(); + + ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame()); + ASSERT_EQ(processor->getOrigin(), cap4); + ASSERT_EQ(processor->getLast(), cap5); + + // check factors + WOLF_DEBUG("checking factors..."); + LandmarkMatchMap landmark_from_last = processor->getMatchesLandmarkFromLast(); + unsigned int n_factors_cap1 = 0; + unsigned int n_factors_cap4 = 0; + + for (auto lmk : problem->getMap()->getLandmarkList()) + { + for (auto fac : lmk->getConstrainedByList()) + { + ASSERT_EQ(fac->getFeature()->getFactorList().size(), 1); // only one factor per feature + if (fac->getFeature()->getCapture() == cap1) + n_factors_cap1++; + else if (fac->getFeature()->getCapture() == cap4) + n_factors_cap4++; + else + ASSERT_TRUE(false);// this shouldn't happen! + } + } + ASSERT_EQ(n_factors_cap1, 10); + ASSERT_EQ(n_factors_cap4, 17); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +