diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index e1b2744d1f83f4373a777a6692f0d8ab7d8ecac0..227f819941fb886e657cb03c0f0cf1422218738b 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -87,8 +87,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Issue KF callback with new KF getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance); - // Update pointers resetDerived(); + + // Update pointers origin_ptr_ = incoming_ptr_; last_ptr_ = incoming_ptr_; incoming_ptr_ = nullptr; @@ -175,9 +176,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); last_ptr_->getFrame()->setKey(); - // make F; append incoming to new F - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); - incoming_ptr_->link(frm); + // // make F; append incoming to new F + // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); + // incoming_ptr_->link(frm); // Establish factors establishFactors(); @@ -187,6 +188,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Update pointers resetDerived(); + + // make F; append incoming to new F + FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp()); + incoming_ptr_->link(frm); + origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; incoming_ptr_ = nullptr;