simplified a little PTFDummy and improved gtest

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

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

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...");