Do not use makeFrame()

src/processor_motion.cpp

@@ -60,7 +60,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
                                                     Eigen::MatrixXs::Zero(data_size_, data_size_), key_frame_ptr);
 
         // create a new last frame
-//        makeFrame(last_ptr_, key_frame_ptr->getState(), NON_KEY_FRAME);
         FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME, key_frame_ptr->getState(), last_ptr_->getTimeStamp());
         new_frame_ptr->addCapture(last_ptr_); // Add Capture to the new Frame
 
@@ -129,15 +128,23 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
     _origin_frame->addCapture(origin_ptr_);
 
     // make (emtpy) last Capture
-    last_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(), getSensorPtr(),
+    last_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(),
+                                                getSensorPtr(),
                                                 Eigen::VectorXs::Zero(data_size_),
-                                                Eigen::MatrixXs::Zero(data_size_, data_size_), _origin_frame);
+                                                Eigen::MatrixXs::Zero(data_size_, data_size_),
+                                                _origin_frame);
     // Make frame at last Capture
-    makeFrame(last_ptr_, _origin_frame->getState(), NON_KEY_FRAME);
+    //    makeFrame(last_ptr_, _origin_frame->getState(), NON_KEY_FRAME);
+    FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME,
+                                                           _origin_frame->getState(),
+                                                           last_ptr_->getTimeStamp());
+    new_frame_ptr->addCapture(last_ptr_); // Add last Capture to the new Frame
 
     // Reset deltas
     delta_ = deltaZero();
     delta_integrated_ = deltaZero();
+    delta_cov_.setZero();
+    delta_integrated_cov_.setZero();
 
     // clear and reset buffer
     getBuffer().get().clear();