From a08ccfc24edaf2e57741f3e2a40af4c10ed1bdd6 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Mon, 30 Oct 2017 17:04:54 +0100
Subject: [PATCH] ProcessorMotion::keyFrameCallback for time tolerance

---
 src/processor_motion.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index f4dd03469..5adb3f922 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -250,6 +250,16 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
     assert(existing_capture != nullptr
             && "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found");
 
+    // If the difference between the found capture time-stamp and the new capture time-stamp
+    // is greater than the time tolerance, abort.
+    if (std::abs(new_ts - existing_capture->getTimeStamp()) > _time_tol_other)
+    {
+      WOLF_DEBUG("Not joinning the key-frame due to time tol : ",
+                 std::abs(new_ts - existing_capture->getTimeStamp()),
+                 " > ", _time_tol_other);
+      return false;
+    }
+
     // Find the frame acting as the capture's origin
     FrameBasePtr new_keyframe_origin = existing_capture->getOriginFramePtr();
 
-- 
GitLab