diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp index 4181a7814339c9effb00e8ce509c1c15196ff45a..bb5d0f1c4003b8f42eadfee49273f5d7b865e75a 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 fcf02371075ee99a261ff43c71dad3dbe6bf1401..0aca6549c8f7142f38c583494bf046739da2032b 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)