From aec4f4e8d88648f737f684244fa1fc7acd3dcb49 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 9 Dec 2021 20:13:03 +0100
Subject: [PATCH] Organize checkTimeTolerance() functions

---
 include/core/processor/processor_base.h    |  1 +
 include/core/processor/processor_motion.h  | 28 ++++++++++++++++++++++
 include/core/processor/processor_tracker.h | 11 +++++----
 src/processor/processor_motion.cpp         | 10 ++++----
 src/processor/processor_tracker.cpp        |  1 +
 test/gtest_buffer_frame.cpp                | 20 ++++++++--------
 6 files changed, 50 insertions(+), 21 deletions(-)

diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c5078c10f..8c61f80b9 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -154,6 +154,7 @@ public:
     */
     bool empty();
 
+protected:
     /**\brief Check time tolerance
     *
     * Check if the time distance between two time stamps is smaller than
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 847a2fabe..5186fb9c4 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -303,6 +303,13 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
 
         FrameBasePtr computeProcessingStep();
 
+        bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2);
+        bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts);
+        bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts);
+        bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap);
+
+
+
         // These are the pure virtual functions doing the mathematics
     public:
 
@@ -541,6 +548,27 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
         Eigen::MatrixXd unmeasured_perturbation_cov_;   ///< Covariance of unmeasured DoF to avoid singularity
 };
 
+inline bool ProcessorMotion::checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2)
+{
+    auto dt = std::fabs(_ts1 - _ts2);
+    return dt <= params_motion_->time_tolerance;
+}
+
+inline bool ProcessorMotion::checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts)
+{
+    return checkTimeTolerance(_cap->getTimeStamp(), _ts);
+}
+
+inline bool ProcessorMotion::checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts)
+{
+    return checkTimeTolerance(_frm->getTimeStamp(), _ts);
+}
+
+inline bool ProcessorMotion::checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap)
+{
+    return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
+}
+
 }
 
 #include "core/frame/frame_base.h"
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index b12779a2e..7164a4226 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -138,10 +138,10 @@ class ProcessorTracker : public ProcessorBase
 
         StateStructure getStateStructure() const;
 
-        bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
-        bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
-        bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
-        bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
+        bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2);
+        bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts);
+        bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts);
+        bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap);
 
         virtual CaptureBasePtr getOrigin() const;
         virtual CaptureBasePtr getLast() const;
@@ -308,7 +308,8 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
 
 inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
 {
-    return buffer_frame_.checkTimeTolerance(_ts1,  _ts2,  params_tracker_->time_tolerance);
+    auto   dt  = std::fabs(_ts1 - _ts2);
+    return dt <= params_tracker_->time_tolerance;
 }
 
 inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index e25430f5f..92c6776dc 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -930,9 +930,8 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
 
         if (keyframe_from_callback)
         {
-            if (buffer_frame_.checkTimeTolerance(keyframe_from_callback->getTimeStamp(),
-                                                       incoming_ptr_->getTimeStamp(),
-                                                       params_motion_->time_tolerance))
+            if (checkTimeTolerance(keyframe_from_callback,
+                                   incoming_ptr_))
             {
                 WOLF_DEBUG("First time with a KF compatible.")
                 processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
@@ -966,9 +965,8 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
 
         if (keyframe_from_callback)
         {
-            if (buffer_frame_.checkTimeTolerance(keyframe_from_callback->getTimeStamp(),
-                                                       origin_ptr_->getTimeStamp(),
-                                                       params_motion_->time_tolerance))
+            if (checkTimeTolerance(keyframe_from_callback,
+                                   origin_ptr_))
 
                 processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
 
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 7ead05cd8..32c74f2db 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -333,5 +333,6 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
     if (getIncoming())
         _stream << _tabs << "  " << "i: Cap" << getIncoming()->id() << std::endl;
 }
+
 } // namespace wolf
 
diff --git a/test/gtest_buffer_frame.cpp b/test/gtest_buffer_frame.cpp
index f314325e6..5c5fadc4c 100644
--- a/test/gtest_buffer_frame.cpp
+++ b/test/gtest_buffer_frame.cpp
@@ -97,16 +97,16 @@ TEST_F(BufferFrameTest, clear)
 //    kfpackbuffer.print();
 //}
 
-TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
-{
-    buffer_kf.clear();
-    buffer_kf.emplace(10, f10);
-    buffer_kf.emplace(20, f20);
-    // min time tolerance  > diff between time stamps. It should return true
-    ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(10, 20, 20, 20));
-    // min time tolerance  < diff between time stamps. It should return true
-    ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(10, 1, 20, 20));
-}
+//TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
+//{
+//    buffer_kf.clear();
+//    buffer_kf.emplace(10, f10);
+//    buffer_kf.emplace(20, f20);
+//    // min time tolerance  > diff between time stamps. It should return true
+//    ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(10, 20, 20, 20));
+//    // min time tolerance  < diff between time stamps. It should return true
+//    ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(10, 1, 20, 20));
+//}
 
 //TEST_F(BufferFrameTest, select)
 //{
-- 
GitLab