From caec037313f2996ae0d64132acadf9dadd447eb4 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Wed, 21 Feb 2018 09:47:44 +0100
Subject: [PATCH] Debugging two captures of same sensor in a KF....tracker
feature dummy
---
src/processor_tracker_feature_dummy.cpp | 10 ++++----
src/test/gtest_processor_base.cpp | 33 +++++++++++++------------
2 files changed, 22 insertions(+), 21 deletions(-)
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index 4181a7814..bb5d0f1c4 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -24,13 +24,13 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
if (features_lost < 2)
{
features_lost++;
- std::cout << "feature " << feat_in_ptr->getMeasurement() << " lost!" << std::endl;
+ std::cout << "feature " << feat_in_ptr->id() << " lost!" << std::endl;
}
else
{
_feature_list_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance()));
_feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
- std::cout << "feature " << feat_in_ptr->getMeasurement() << " tracked!" << std::endl;
+ std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl;
}
}
@@ -56,7 +56,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
n_feature_++;
new_features_last_.push_back(
std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
- //std::cout << "feature " << new_features_last_.back()->getMeasurement() << " detected!" << std::endl;
+ std::cout << "feature " << new_features_last_.back()->id() << " detected!" << std::endl;
}
std::cout << new_features_last_.size() << " features detected!" << std::endl;
@@ -66,8 +66,8 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- // std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
- // << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl;
+ std::cout << "creating constraint: last feature " << _feature_ptr->id()
+ << " with origin feature " << _feature_other_ptr->id() << std::endl;
auto ctr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
// _feature_ptr->addConstraint(ctr);
// _feature_other_ptr->addConstrainedBy(ctr);
diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp
index fcf023710..0aca6549c 100644
--- a/src/test/gtest_processor_base.cpp
+++ b/src/test/gtest_processor_base.cpp
@@ -216,42 +216,43 @@ TEST(ProcessorBase, KeyFrameCallback)
CaptureOdom2DPtr capture_odo = make_shared<CaptureOdom2D>(t, sen_odo, Vector2s(0.5,0));
- for (size_t ii=0; ii<10; ii++ )
+ for (size_t ii=0; ii<3; ii++ )
{
- WOLF_DEBUG("iter:",ii," ts: ", t);
+// WOLF_DEBUG("iter:",ii," ts: ", t);
// Move
t = t+0.01;
- capture_odo->setTimeStamp(t);
- sen_odo->process(capture_odo);
+// capture_odo->setTimeStamp(t);
+// sen_odo->process(capture_odo);
- WOLF_DEBUG("iter:",ii," ts: ", t);
+// WOLF_DEBUG("iter:",ii," ts: ", t);
- t = t+0.01;
- capture_odo->setTimeStamp(t);
- sen_odo->process(capture_odo);
+// t = t+0.01;
+// capture_odo->setTimeStamp(t);
+// sen_odo->process(capture_odo);
- WOLF_DEBUG("iter:",ii," ts: ", t);
+// WOLF_DEBUG("iter:",ii," ts: ", t);
- t = t+0.01;
- capture_odo->setTimeStamp(t);
- sen_odo->process(capture_odo);
+// t = t+0.01;
+// capture_odo->setTimeStamp(t);
+// sen_odo->process(capture_odo);
- WOLF_DEBUG("iter:",ii," ts: ", t);
+// WOLF_DEBUG("iter:",ii," ts: ", t);
// Track
- proc_tracker->process(make_shared<CaptureVoid>(t, sen_tracker));
+ CaptureVoidPtr cap(make_shared<CaptureVoid>(t, sen_tracker));
+ proc_tracker->process(cap);
problem->print(4,0,0,0);
// Only odom creating KFs
ASSERT_TRUE( problem->getLastKeyFramePtr()->getType().compare("PO 2D")==0 );
- WOLF_DEBUG("iter:",ii," ts: ", t);
+// WOLF_DEBUG("iter:",ii," ts: ", t);
}
// Print WOLF info
- problem->print(2);
+// problem->print(2);
}
int main(int argc, char **argv)
--
GitLab