add first functions of KFPack

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

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

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})

src/test/gtest_feature_motion.cpp

-/*
- * 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 */

src/test/gtest_processor_base.cpp

+/*
+ * 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();
+}
+