PTLDummy gtest

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):

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();
 }

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})

test/gtest_processor_tracker_landmark_dummy.cpp

+
+// 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();
+}
+