Allow for detecting last_capture which is now disconnected from the Trajectory

src/processor/processor_motion.cpp

@@ -715,6 +715,14 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
 
 CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
 {
+    // First check if last_ptr is the one we are looking for
+    if (fabs(last_ptr_->getTimeStamp() - _ts) <= this->params_->time_tolerance)
+        return last_ptr_;
+
+
+    // Then look in the Wolf tree...
+    // -----------------------------
+    //
     // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
     // Note: since the buffer goes from a KF in the past until the next KF, we need to:
     //  1. See that the KF contains a CaptureMotion