gtest_processor_tracker_dummy

test/CMakeLists.txt

@@ -190,6 +190,10 @@ target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
 wolf_add_gtest(gtest_processor_odom_3D gtest_processor_odom_3D.cpp)
 target_link_libraries(gtest_processor_odom_3D ${PROJECT_NAME})
 
+# ProcessorTrackerFeatureDummy class test
+wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
+target_link_libraries(gtest_processor_tracker_feature_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_feature_dummy.cpp

+
+// wolf includes
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/sensor_factory.h"
+#include "core/processor/processor_tracker_feature_dummy.h"
+#include "core/capture/capture_void.h"
+
+using namespace wolf;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummyDummy);
+
+class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
+{
+	public:
+
+        ProcessorTrackerFeatureDummyDummy(ProcessorParamsTrackerFeatureDummyPtr& _params) :
+            ProcessorTrackerFeatureDummy(_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 callTrackFeatures(const FeatureBasePtrList& _features_last_in,
+                                       FeatureBasePtrList& _features_incoming_out,
+                                       FeatureMatchMap& _feature_correspondences){ return this->trackFeatures(_features_last_in, _features_incoming_out, _feature_correspondences); }
+
+        FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return this->createFactor(_feature_ptr, _feature_other_ptr); }
+
+		TrackMatrix getTrackMatrix(){ return track_matrix_; }
+
+        FeatureMatchMap    getMatchesLastFromIncoming() { return matches_last_from_incoming_; }
+
+        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 ProcessorTrackerFeatureDummyTest : public testing::Test
+{
+    public:
+        ProblemPtr problem;
+        SensorBasePtr sensor;
+        ProcessorParamsTrackerFeatureDummyPtr params;
+        ProcessorTrackerFeatureDummyDummyPtr 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<ProcessorParamsTrackerFeatureDummy>();
+            params->max_new_features = 10;
+            params->min_features_for_keyframe = 7;
+            params->time_tolerance = 0.25;
+            params->voting_active = true;
+            params->loss_track_ratio = 10;
+            processor  = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
+            processor->link(sensor);
+        }
+};
+
+TEST_F(ProcessorTrackerFeatureDummyTest, installProcessor)
+{
+    ASSERT_EQ(processor->getProblem(), problem);
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, callDetectNewFeatures)
+{
+    // 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());
+    ASSERT_EQ(n_feat, params->max_new_features);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    //fill feat_last list
+    processor->callDetectNewFeatures(params->max_new_features, feat_list);
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(0.1, sensor);
+    processor->setInc(inc_cap);
+
+    //test trackFeatures
+    FeatureBasePtrList feat_list_out;
+    FeatureMatchMap feat_correspondance;
+    processor->callTrackFeatures(feat_list, feat_list_out, feat_correspondance);
+    ASSERT_EQ(feat_list_out.size(), feat_correspondance.size());
+    ASSERT_EQ(feat_list.size(), feat_list_out.size()+1); // one of each 10 tracks is lost
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, 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>(0.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
+}
+
+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);
+    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->getMatchesLastFromIncoming().size(), 14); // 1 of each 10 tracks is lost
+
+    processor->callReset(); // now incoming is last and last is origin
+
+    // Put a capture on last_ptr_
+    CaptureBasePtr new_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setInc(new_cap);
+
+    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
+}
+
+
+TEST_F(ProcessorTrackerFeatureDummyTest, createFactors)
+{
+    FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                     Eigen::Vector1s::Ones(),
+                                                     Eigen::MatrixXs::Ones(1, 1)));
+    FeatureBasePtr ftr_other(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                           Eigen::Vector1s::Ones(),
+                                                           Eigen::MatrixXs::Ones(1, 1)));
+
+    FactorBasePtr fac = processor->callCreateFactor(ftr, ftr_other);
+    fac->link(ftr);
+    ASSERT_EQ(fac->getFeature(),ftr);
+    ASSERT_EQ(fac->getFeatureOther(),ftr_other);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, process)
+{
+
+    //1ST TIME -> KF (origin)
+    WOLF_DEBUG("First time...");
+    CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0.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());
+
+    //2ND TIME
+    WOLF_DEBUG("Second time...");
+    CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(0.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>(0.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);
+    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);
+    cap5->process();
+
+    ASSERT_DOUBLE_EQ(problem->getTrajectory()->getLastKeyFrame()->getTimeStamp().get(), 0.3);
+    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame());
+
+    // check factors
+    WOLF_DEBUG("checking factors...");
+    TrackMatrix track_matrix = processor->getTrackMatrix();
+    ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
+    unsigned int n_factors0 = 0;
+    unsigned int n_factors4 = 0;
+    for (auto feat : cap4->getFeatureList())
+    {
+        if (!feat->getFactorList().empty())
+        {
+            n_factors0++;
+            ASSERT_EQ(feat->getFactorList().size(), 1);
+            ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
+            ASSERT_EQ(feat->getFactorList().front()->getFeatureOther(), track_matrix.feature(feat->trackId(), cap1));
+        }
+    }
+
+    for (auto feat : cap1->getFeatureList())
+    {
+        if (!feat->getConstrainedByList().empty())
+        {
+            n_factors4++;
+            ASSERT_EQ(feat->getConstrainedByList().size(), 1);
+            ASSERT_EQ(feat->getConstrainedByList().front()->getFeatureOther(), feat);
+            ASSERT_EQ(feat->getConstrainedByList().front()->getFeature(), track_matrix.feature(feat->trackId(), cap4));
+        }
+    }
+    ASSERT_EQ(n_factors0, 7);
+    ASSERT_EQ(n_factors4, 7);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+