diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index b14237b0366db2f567c8582f6969db8a20cbf933..b03bbdb8b18e3dc1da3a6a70e57a93ac90f3ecc5 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -67,4 +67,43 @@ void ProcessorBase::remove()
}
}
+void KFPackBuffer::removeUpTo(const KFPackPtr& _pack)
+{
+ KFPackBuffer::Iterator post = container_.upper_bound(_pack->key_frame->getTimeStamp());
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
+void KFPackBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
+{
+ TimeStamp time_stamp = _key_frame->getTimeStamp();
+ KFPackPtr kfpack = std::make_shared<KFPack>(_key_frame, _time_tolerance);
+ container_.emplace(time_stamp, kfpack);
+}
+
+KFPackPtr KFPackBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+{
+ KFPackBuffer::Iterator post = container_.upper_bound(_time_stamp);
+ KFPackBuffer::Iterator prev = container_.begin();
+ if (post != container_.begin())
+ prev = post--;
+
+ if (post == container_.end() && prev != container_.end()
+ && checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance))
+ return prev->second;
+ else if (prev == container_.end() && post != container_.end()
+ && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance))
+ return post->second;
+ else if (post != container_.end() && prev != container_.end()
+ && checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance)
+ && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance))
+ {
+ if (std::fabs((*post).first - _time_stamp) < std::fabs((*prev).first - _time_stamp))
+ return post->second;
+ else
+ return prev->second;
+ }
+
+ return nullptr;
+}
+
} // namespace wolf
diff --git a/src/processor_base.h b/src/processor_base.h
index e04aba1e24309e17e3bacd23beead112ac50c9a1..fbe714ac3570763a96c2a9491864d063c3a45d23 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -6,16 +6,51 @@ namespace wolf{
class SensorBase;
}
-//Wolf includes
+// Wolf includes
#include "wolf.h"
#include "node_base.h"
+#include "time_stamp.h"
// std
#include <memory>
+#include <map>
namespace wolf {
+class KFPack
+{
+ public:
+ KFPack(const FrameBasePtr _key_frame, const Scalar _time_tolerance) : key_frame(_key_frame), time_tolerance(_time_tolerance) {};
+ ~KFPack(){};
+ FrameBasePtr key_frame;
+ Scalar time_tolerance;
+};
+
+WOLF_PTR_TYPEDEFS(KFPack);
+
+class KFPackBuffer
+{
+ public:
+
+ typedef std::map<TimeStamp,KFPackPtr>::iterator Iterator;
+ KFPackBuffer(void);
+ ~KFPackBuffer(void);
+
+ KFPackPtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+
+ size_t size(void);
+
+ void add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance);
+
+ void removeUpTo(const KFPackPtr& _pack);
+
+ private:
+
+ std::map<TimeStamp,KFPackPtr> container_;
+
+ bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
+};
/** \brief base struct for processor parameters
*
@@ -125,6 +160,26 @@ inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
time_tolerance_ = _time_tolerance;
}
+inline KFPackBuffer::KFPackBuffer(void)
+{
+
+}
+
+inline KFPackBuffer::~KFPackBuffer(void)
+{
+
+}
+
+inline bool KFPackBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
+{
+ return (std::fabs(_time_stamp1 - _time_stamp2) < std::min(_time_tolerance1, _time_tolerance2) );
+}
+
+inline size_t KFPackBuffer::size(void)
+{
+ return container_.size();
+}
+
} // namespace wolf
#endif
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 738bccd2056638606f1d1a69a94e6978f49f4756..1bab9866320dfeb0e85a501e7b3db0118ba62d3f 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -76,6 +76,10 @@ target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
wolf_add_gtest(gtest_problem gtest_problem.cpp)
target_link_libraries(gtest_problem ${PROJECT_NAME})
+# ProcessorBase class test
+wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
+target_link_libraries(gtest_processor_base ${PROJECT_NAME})
+
# ProcessorMotion class test
wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
diff --git a/src/test/gtest_feature_motion.cpp b/src/test/gtest_feature_motion.cpp
deleted file mode 100644
index 1620131a6dd4b2b8957c783e5693bab3c4e07f04..0000000000000000000000000000000000000000
--- a/src/test/gtest_feature_motion.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * gtest_feature_motion.cpp
- *
- * Created on: Aug 11, 2017
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "/Users/jsola/dev/wolf/src/feature_motion.h"
-
-namespace wolf
-{
-
-TEST(DummyGroup, DummyTestExample)
-{
- // TODO: Automatically generated TEST stub
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
-} /* namespace wolf */
diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b859040c1c005dcb80f034ae856eeeccf6db69dc
--- /dev/null
+++ b/src/test/gtest_processor_base.cpp
@@ -0,0 +1,96 @@
+/*
+ * gtest_capture_base.cpp
+ *
+ * Created on: Feb 15, 2018
+ * Author: asantamaria
+ */
+
+
+#include "utils_gtest.h"
+
+#include "processor_base.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+class KFPackBufferTest : public testing::Test
+{
+ public:
+
+ KFPackBuffer kfpackbuffer;
+ FrameBasePtr f10, f20, f21, f28;
+ Scalar tt10, tt20, tt21, tt28;
+ TimeStamp timestamp;
+ Scalar timetolerance;
+
+ void SetUp(void)
+ {
+ f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
+ f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
+ f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
+ f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
+
+ tt10 = 0.1;
+ tt20 = 0.2;
+ tt21 = 0.21;
+ tt28 = 0.28;
+ };
+};
+
+TEST_F(KFPackBufferTest, add)
+{
+ kfpackbuffer.add(f10, tt10);
+ ASSERT_EQ(kfpackbuffer.size(),1);
+ kfpackbuffer.add(f20, tt20);
+ ASSERT_EQ(kfpackbuffer.size(),2);
+}
+
+TEST_F(KFPackBufferTest, removeUpTo)
+{
+ kfpackbuffer.add(f10, tt10);
+ kfpackbuffer.add(f20, tt20);
+ kfpackbuffer.add(f21, tt21);
+ ASSERT_EQ(kfpackbuffer.size(),3);
+
+ // it should remove f20 and f10, thus size should be 1 after removal
+ // Specifically, only f21 should remain
+ KFPackPtr pack20 = std::make_shared<KFPack>(f20,tt20);
+ kfpackbuffer.removeUpTo( pack20 );
+ ASSERT_EQ(kfpackbuffer.size(),1);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f10->getTimeStamp(),tt10)==nullptr);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f20->getTimeStamp(),tt20)==nullptr);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f21->getTimeStamp(),tt21)!=nullptr);
+
+ // Chech removal of an imprecise time stamp
+ // Specifically, only f28 should remain
+ kfpackbuffer.add(f28, tt28);
+ ASSERT_EQ(kfpackbuffer.size(),2);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
+ KFPackPtr pack22 = std::make_shared<KFPack>(f22,5);
+ kfpackbuffer.removeUpTo( pack22 );
+ ASSERT_EQ(kfpackbuffer.size(),1);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f21->getTimeStamp(),tt21)==nullptr);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f28->getTimeStamp(),tt28)!=nullptr);
+}
+
+//TEST_F(KFPackBufferTest, selectPack)
+//{
+// kfpackbuffer.add(f10, tt10);
+// kfpackbuffer.add(f20, tt20);
+// kfpackbuffer.add(f21, tt21);
+//
+// // It should return f20
+// timestamp = 18;
+// timetolerance = 2;
+//
+// KFPackPtr pack = kfpackbuffer.selectPack(timestamp, timetolerance);
+//
+// std::cout << pack->key_frame->getTimeStamp() << " " << pack->time_tolerance << std::endl;
+//}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+