Organize checkTimeTolerance() functions

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

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"

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)

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;
 

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
 

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)
 //{