Write state to last frame in processor tracker

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;