add KFPackBuffer class with gtest working

src/processor_base.cpp

@@ -83,27 +83,39 @@ void KFPackBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_toler
 KFPackPtr KFPackBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
 {
     KFPackBuffer::Iterator post = container_.upper_bound(_time_stamp);
-    KFPackBuffer::Iterator prev = container_.begin();
+    KFPackBuffer::Iterator prev = container_.end();
+
     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))
+        prev = std::prev(post);
+        if (!checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance) && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance))
             return post->second;
-        else
+        else if (checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance) && !checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance))
             return prev->second;
+        else if (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;
+        }
+    }
+    else
+    {
+        if (checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance))
+            return post->second;
     }
-
     return nullptr;
 }
 
+void KFPackBuffer::print(void)
+{
+    std::cout << "[ ";
+    for (auto iter : container_)
+    {
+        std::cout << "( tstamp: " << iter.first << ", id: " << iter.second->key_frame->id() << ") ";
+    }
+    std::cout << "]" << std::endl;
+}
+
 } // namespace wolf

src/processor_base.h

@@ -45,11 +45,17 @@ class KFPackBuffer
 
         void removeUpTo(const KFPackPtr& _pack);
 
+        bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
+
+        void clear();
+
+        bool empty();
+
+        void print();
+
     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
@@ -175,6 +181,16 @@ inline bool KFPackBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, cons
     return (std::fabs(_time_stamp1 - _time_stamp2) < std::min(_time_tolerance1, _time_tolerance2) );
 }
 
+inline void KFPackBuffer::clear()
+{
+    container_.clear();
+}
+
+inline bool KFPackBuffer::empty()
+{
+    return container_.empty();
+}
+
 inline size_t KFPackBuffer::size(void)
 {
     return container_.size();

src/test/gtest_processor_base.cpp

@@ -10,6 +10,9 @@
 
 #include "processor_base.h"
 
+// STL
+#include <iterator>
+
 using namespace wolf;
 using namespace Eigen;
 
@@ -30,13 +33,18 @@ class KFPackBufferTest : public testing::Test
             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;
+            tt10 = 1.0;
+            tt20 = 1.0;
+            tt21 = 1.0;
+            tt28 = 1.0;
         };
 };
 
+TEST_F(KFPackBufferTest, empty)
+{
+    ASSERT_TRUE(kfpackbuffer.empty());
+}
+
 TEST_F(KFPackBufferTest, add)
 {
     kfpackbuffer.add(f10, tt10);
@@ -45,12 +53,97 @@ TEST_F(KFPackBufferTest, add)
     ASSERT_EQ(kfpackbuffer.size(),2);
 }
 
+TEST_F(KFPackBufferTest, clear)
+{
+    kfpackbuffer.add(f10, tt10);
+    kfpackbuffer.add(f20, tt20);
+    ASSERT_EQ(kfpackbuffer.size(),2);
+    kfpackbuffer.clear();
+    ASSERT_TRUE(kfpackbuffer.empty());
+}
+
+//TEST_F(KFPackBufferTest, print)
+//{
+//    kfpackbuffer.clear();
+//    kfpackbuffer.add(f10, tt10);
+//    kfpackbuffer.add(f20, tt20);
+//    kfpackbuffer.print();
+//}
+
+TEST_F(KFPackBufferTest, checkTimeTolerance)
+{
+    kfpackbuffer.clear();
+    kfpackbuffer.add(f10, tt10);
+    kfpackbuffer.add(f20, tt20);
+    // min time tolerance  > diff between time stamps. It should return true
+    ASSERT_TRUE(kfpackbuffer.checkTimeTolerance(10, 20, 20, 20));
+    // min time tolerance  < diff between time stamps. It should return true
+    ASSERT_FALSE(kfpackbuffer.checkTimeTolerance(10, 1, 20, 20));
+}
+
+TEST_F(KFPackBufferTest, selectPack)
+{
+    // Evaluation using two packs (p1,p2)
+    // with different time tolerances (tp1,tp2)
+    // using a query pack (q) with also different time tolerances
+    // depending on these tolerances we will get one (p1) or the other (p2)
+    // packages from the buffer (res).
+    // This can be summarized in the table hereafter:
+    //
+    //  p1 p2 q | resulting pack time stamp
+    // --------------------------------
+    //  2  2  2 | nullptr
+    //  2  2  5 | nullptr
+    //  2  2  7 | nullptr
+    //  2  7  2 | nullptr
+    //  2  7  5 | 20
+    //  2  7  7 | 20
+    //  7  2  2 | nullptr
+    //  7  2  5 | nullptr
+    //  7  2  7 | 10
+    //  7  7  2 | nullptr
+    //  7  7  5 | 20
+    //  7  7  7 | 20
+
+    kfpackbuffer.clear();
+
+    // input packages
+    std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances
+    std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances
+    std::vector<int> q = {2, 5, 7}; // Query pack time tolerances
+
+    // Solution matrix
+    Eigen::VectorXi res = Eigen::VectorXi::Zero(12);
+    res(4) = 20;
+    res(5) = 20;
+    res(8) = 10;
+    res(10) = 20;
+    res(11) = 20;
+
+    // test
+    for (auto ip1=0;ip1<p1.size();++ip1)
+    {
+        for (auto ip2=0;ip2<p2.size();++ip2)
+        {
+            kfpackbuffer.add(f10, p1[ip1]);
+            kfpackbuffer.add(f20, p2[ip2]);
+            for (auto iq=0;iq<q.size();++iq)
+            {
+                KFPackPtr packQ = kfpackbuffer.selectPack(16, q[iq]);
+                if (packQ!=nullptr)
+                    ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+            }
+            kfpackbuffer.clear();
+        }
+    }
+}
+
 TEST_F(KFPackBufferTest, removeUpTo)
 {
+    kfpackbuffer.clear();
     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
@@ -73,21 +166,6 @@ TEST_F(KFPackBufferTest, removeUpTo)
     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);