From 0c7c1bb8a9a9b99fed24883331cd3dadb9e24efc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 31 May 2019 12:17:33 +0200
Subject: [PATCH] simplified a little PTFDummy and improved gtest
---
.../processor_tracker_feature_dummy.h | 9 +-
.../processor_tracker_feature_dummy.cpp | 9 +-
.../gtest_processor_tracker_feature_dummy.cpp | 98 ++++++++++++++-----
3 files changed, 81 insertions(+), 35 deletions(-)
diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h
index a68bac64a..150a6ffb0 100644
--- a/include/core/processor/processor_tracker_feature_dummy.h
+++ b/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)
{
//
}
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index f213aed65..999f86207 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/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);
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index dba7c3a69..02e24d90b 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/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...");
--
GitLab