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