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