diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 5cb8b42909b9fe93be361983a4f664fef17ce608..366d9e4aa1f9fa59880dc3f175a146577786cc6d 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -96,15 +96,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) preProcess(); // Derived class operations - // integrate data - if (last_ptr_ != nullptr) - { - integrateOneStep(); - } - else { - last_nullptr_ = true; - } - PackKeyFramePtr pack = computeProcessingStep(); if (pack) buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() ); @@ -139,6 +130,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) case RUNNING_WITH_KF_ON_ORIGIN : break; + default : + break; + } + + + // integrate data + // Done at this place because setPrior() needs + integrateOneStep(); + + + switch(processing_step_) + { case RUNNING_WITH_KF_BEFORE_ORIGIN : {