diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 8a3cbd5d13e6db8fb60c053e40cbded040e18279..f18a4a5eba1470f5c605d7983cd11744f17d993f 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -329,7 +329,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;} public: - virtual bool isMotion(); + virtual bool isMotion() const; void setTimeTolerance(Scalar _time_tolerance); @@ -380,7 +380,7 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... return prc; } -inline bool ProcessorBase::isMotion() +inline bool ProcessorBase::isMotion() const { return false; } diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 03c9f4308a2d0d1b6bba6ca0e722478a8d850e73..6a5fb31c68922e9dee7cf7274e61fe8e4d95899d 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -159,7 +159,7 @@ class ProcessorMotion : public ProcessorBase virtual void resetDerived(); // Queries to the processor: - virtual bool isMotion() override; + virtual bool isMotion() const override; /** \brief Fill a reference to the state integrated so far * \param _x the returned state vector @@ -571,7 +571,7 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const return capture_ptr->getBuffer().getMotion(_ts); } -inline bool ProcessorMotion::isMotion() +inline bool ProcessorMotion::isMotion() const { return true; } diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index 1c9277989d11fee7e0ea568e8cbccdb1bec58b0e..0a5d46a6be09c2b918d89886048bb16ca09f7e04 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -108,6 +108,8 @@ class ProcessorTracker : public ProcessorBase ProcessorParamsTrackerPtr _params_tracker); virtual ~ProcessorTracker(); + virtual bool isMotion() const final override {return false; } + bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2); bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts); diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 988b8066864fb03c9fab5a3dc9e21869000dfd25..75659594a9af9cff6da09e9eae9c62ce8d974c83 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -159,6 +159,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) } case RUNNING_WITHOUT_PACK : { + WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_PACK"); + processKnown(); if (voteForKeyFrame() && permittedKeyFrame()) @@ -210,12 +212,14 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance); // Advance this + advanceDerived(); + + // Replace last frame for a new one in incoming last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame // do not remove! last_ptr_->remove(); incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp()); // Update pointers - advanceDerived(); last_ptr_ = incoming_ptr_; incoming_ptr_ = nullptr; }*/ @@ -224,14 +228,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // We do not create a KF // Advance this + advanceDerived(); + + // Replace last frame for a new one in incoming FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp()); incoming_ptr_->link(frm); last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove(); // Update pointers - advanceDerived(); - last_ptr_ = incoming_ptr_; - incoming_ptr_ = nullptr; + last_ptr_ = incoming_ptr_; + incoming_ptr_ = nullptr; } break; }