diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp index 58c87a3a65cca48a130abdd13df8bce3ca5f7cba..503a39fb3b2ef55a37a7f92f9a1fc5d949dc57e1 100644 --- a/src/processor_tracker.cpp +++ b/src/processor_tracker.cpp @@ -16,7 +16,7 @@ namespace wolf ProcessorTracker::ProcessorTracker(const std::string& _type, const unsigned int _max_new_features, const Scalar& _time_tolerance) : ProcessorBase(_type, _time_tolerance), - processing_step_(FIRST_TIME), + processing_step_(FIRST_TIME_WITHOUT_PACK), origin_ptr_(nullptr), last_ptr_(nullptr), incoming_ptr_(nullptr), @@ -40,6 +40,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) return; } + incoming_ptr_ = _incoming_ptr; + if ( !kf_pack_buffer_.empty() ) { KFPackPtr pack; @@ -56,23 +58,22 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) } // Select using incoming_ptr - pack = kf_pack_buffer_.selectPack( _incoming_ptr->getTimeStamp(), time_tolerance_ ); + pack = kf_pack_buffer_.selectPack( incoming_ptr_->getTimeStamp(), time_tolerance_ ); if (pack!=nullptr) { keyFrameCallback(pack->key_frame,pack->time_tolerance); - kf_pack_buffer_.removeUpTo( _incoming_ptr->getTimeStamp() ); + kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() ); } } - incoming_ptr_ = _incoming_ptr; - preProcess(); computeProcessingStep(); switch (processing_step_) { - case FIRST_TIME : + case FIRST_TIME_WITHOUT_PACK : + case FIRST_TIME_WITH_PACK : { WOLF_DEBUG( "FIRST TIME" ); @@ -114,7 +115,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) } - case SECOND_TIME : + case SECOND_TIME_WITHOUT_PACK : { WOLF_DEBUG("SECOND TIME or after KEY FRAME CALLBACK"); @@ -151,6 +152,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) break; } + case RUNNING_WITH_PACK : + case RUNNING_WITHOUT_PACK : default: { WOLF_DEBUG("OTHER TIMES"); @@ -296,12 +299,95 @@ void ProcessorTracker::setKeyFrame(CaptureBasePtr _capture_ptr) } } +KFPackPtr ProcessorTracker::selectPack(const CaptureBasePtr & _cap) +{ + if (_cap) + return kf_pack_buffer_.selectPack(_cap->getTimeStamp(), time_tolerance_); + + return nullptr; +} + void ProcessorTracker::computeProcessingStep() { + enum {FIRST_TIME, SECOND_TIME, RUNNING} step; if (origin_ptr_ == nullptr && last_ptr_ == nullptr) - processing_step_ = FIRST_TIME; - else if (origin_ptr_ == nullptr) - processing_step_ = SECOND_TIME; + step = FIRST_TIME; + else if (origin_ptr_ == last_ptr_) + step = SECOND_TIME; + else + step = RUNNING; + + //////////////////// + switch (step) + { + case FIRST_TIME : + { + if (selectPack(incoming_ptr_)) + { + processing_step_ = FIRST_TIME_WITH_PACK; + // (1) + // pack.KF.addCapture(incoming) + // makeF; F.addCapture(incoming) + // o <- i + // l <- i + } + else // ! last && ! pack(incoming) + { + processing_step_ = FIRST_TIME_WITHOUT_PACK; + // (4) WARNING No pack on first incoming! + // makeKF; KF.addCapture(incoming) + // makeF; F.addCapture(incoming) + // o <- i + // l <- i + } + } + break; + case SECOND_TIME : + { + if (selectPack(last_ptr_)) + { + WOLF_WARN("\n||*||"); + WOLF_INFO("\n ... It seems you missed something!"); + WOLF_INFO("\nPack's KF and origin's KF have matching time stamps (i.e. below time tolerances)"); + WOLF_INFO("Check the following:"); + WOLF_INFO(" - You have all processors installed before starting receiving any data"); + WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed"); + WOLF_INFO(" - You have configured all your processors with compatible time tolerances"); + WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances). Check time tolerances!"); + } + else + { + processing_step_ = SECOND_TIME_WITHOUT_PACK; + // (2) + // last.F.setKey() + // makeF; F.addCapture(incoming) + // o <- l + // l <- i + } + break; + } + case RUNNING : + default : + { + if (selectPack(last_ptr_)) + { + processing_step_ = RUNNING_WITH_PACK; + // (1) + // pack.KF.addCapture(last) + // makeF; F.addCapture(incoming) + // o <- l + // l <- i + } + else + { + processing_step_ = RUNNING_WITHOUT_PACK; + // (3) + // o <- o // verbose + // l <- i + } + break; + } + } } diff --git a/src/processor_tracker.h b/src/processor_tracker.h index 755c15f92056f508476eb78cf9d163b19b20fffa..00ba42583ec6e8ca325c46a1fcea7cd6cf0ffdea 100644 --- a/src/processor_tracker.h +++ b/src/processor_tracker.h @@ -68,7 +68,13 @@ struct ProcessorParamsTracker : public ProcessorParamsBase class ProcessorTracker : public ProcessorBase { public: - typedef enum {FIRST_TIME, SECOND_TIME} ProcessingStep ; + typedef enum { + FIRST_TIME_WITH_PACK, + FIRST_TIME_WITHOUT_PACK, + SECOND_TIME_WITHOUT_PACK, + RUNNING_WITH_PACK, + RUNNING_WITHOUT_PACK + } ProcessingStep ; protected: ProcessingStep processing_step_; @@ -202,6 +208,8 @@ class ProcessorTracker : public ProcessorBase void addNewFeatureIncoming(FeatureBasePtr _feature_ptr); + private: + KFPackPtr selectPack(const CaptureBasePtr & _cap); }; inline void ProcessorTracker::setMaxNewFeatures(const unsigned int& _max_new_features)