From 5f2294625562517e59ac84737524a05e16776edc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 10 Jun 2020 12:24:12 +0200
Subject: [PATCH] Allow for detecting last_capture which is now disconnected
from the Trajectory
---
src/processor/processor_motion.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 472e569a4..c828ab93f 100644
--- a/src/processor/processor_motion.cpp
+++ b/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
--
GitLab