diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index b0e08b4b59240b359457565c95071d2b7c1385e6..623066614b764486e4cf568e892d8fed95c90cbf 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -90,8 +90,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them. processKnown(); - // Update pointers + // Reset this resetDerived(); + // Update pointers origin_ptr_ = incoming_ptr_; last_ptr_ = incoming_ptr_; incoming_ptr_ = nullptr; @@ -119,8 +120,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Issue KF callback with new KF getProblem()->keyFrameCallback(keyframe, shared_from_this()); - // Update pointers + // Reset this resetDerived(); + // Update pointers origin_ptr_ = incoming_ptr_; last_ptr_ = incoming_ptr_; incoming_ptr_ = nullptr; @@ -163,8 +165,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Establish factors establishFactors(); - // Update pointers + // Reset this resetDerived(); + // Update pointers origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; last_frame_ptr_ = keyframe; @@ -199,8 +202,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Establish factors establishFactors(); - // Update pointers + // Reset this resetDerived(); + // Update pointers origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; last_frame_ptr_ = frame; @@ -237,8 +241,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) getProblem()->getState(incoming_ptr_->getTimeStamp())); incoming_ptr_ ->link(frame); - // Update pointers + // Reset this resetDerived(); + // Update pointers origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; last_frame_ptr_ = frame; @@ -249,9 +254,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) { // We do not create a KF - // Advance this - advanceDerived(); - // Replace last frame for a new NON KEY frame in incoming FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), getProblem()->getFrameStructure(), @@ -259,6 +261,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) incoming_ptr_->link(frame); last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last + // Advance this + advanceDerived(); // Update pointers last_ptr_ = incoming_ptr_; last_frame_ptr_ = frame;