From 3510cd0981531e112609313e2c27cac6c131abff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Tue, 4 Jun 2019 13:12:23 +0200 Subject: [PATCH] PTLDummy gtest working --- ...gtest_processor_tracker_landmark_dummy.cpp | 333 +++++++++--------- 1 file changed, 167 insertions(+), 166 deletions(-) diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp index 870963ae2..f6159ad00 100644 --- a/test/gtest_processor_tracker_landmark_dummy.cpp +++ b/test/gtest_processor_tracker_landmark_dummy.cpp @@ -211,172 +211,173 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks) ASSERT_TRUE(isFeatureLinked(feat, inc_cap)); } -//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->callEmplaceLandmark(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->callEmlaceFactor(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); -//} +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 in last, create landmarks, find landmarks in incoming + + 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) +{ + // Put a capture on last_ptr_ + CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor); + processor->setLast(last_cap); + + // create 10 landmarks and link them to map + FeatureBasePtrList feat_list; + processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list); + processor->setNewFeaturesLast(feat_list); + processor->callEmplaceNewLandmarks(); + 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->callEmlaceFactor(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) { -- GitLab