Merge branch 'devel' into 248-processor-constructors-and-creators-requiring-a-sensor-pointer

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;
 }

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;
 }

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);

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;
         }