New selectPackBefore() with test

src/processor_base.cpp

@@ -137,6 +137,31 @@ KFPackPtr KFPackBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _
     return nullptr;
 }
 
+KFPackPtr KFPackBuffer::selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+{
+    if (container_.empty())
+        return nullptr;
+
+    KFPackBuffer::Iterator post = container_.upper_bound(_time_stamp);
+
+    bool prev_exists = (post != container_.begin());
+
+    if (prev_exists)
+        return container_.begin()->second;
+
+    else
+    {
+        bool post_exists = (post != container_.end());
+        bool post_ok     = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
+
+        if (post_ok)
+            return post->second;
+    }
+
+    return nullptr;
+}
+
+
 void KFPackBuffer::print(void)
 {
     std::cout << "[ ";

src/processor_base.h

@@ -50,10 +50,12 @@ class KFPackBuffer
         /**\brief Select a Pack from the buffer
          *
          *  Select from the buffer the closest pack (w.r.t. time stamp),
-         * respecting a defined time tolerance
+         * respecting a defined time tolerances
          */
         KFPackPtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
 
+        KFPackPtr selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+
         /**\brief Buffer size
          *
          */

src/test/gtest_KF_pack_buffer.cpp

@@ -144,6 +144,68 @@ TEST_F(KFPackBufferTest, selectPack)
     }
 }
 
+TEST_F(KFPackBufferTest, selectPackBefore)
+{
+    kfpackbuffer.clear();
+
+    kfpackbuffer.add(f10, tt10);
+    kfpackbuffer.add(f20, tt20);
+    kfpackbuffer.add(f21, tt21);
+
+    // input time stamps
+    std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
+    Scalar tt = 0.01;
+
+    // Solution matrix
+    // q_ts  |  pack
+    //=================
+    // first time
+    //-----------------
+    // 9.5      nullptr
+    // 9.995    10
+    // 10,005   10
+    // 19.5     10
+    // 20.5     10
+    // 21.5     10
+    // second time
+    //-----------------
+    // 9.5      nullptr
+    // 9.995    null
+    // 10,005   null
+    // 19.5     null
+    // 20.5     20
+    // 21.5     20
+    // third time
+    //-----------------
+    // 9.5      nullptr
+    // 9.995    null
+    // 10,005   null
+    // 19.5     null
+    // 20.5     null
+    // 21.5     21
+
+    Eigen::MatrixXs truth(3,6), res(3,6);
+    truth << 0,10,10,10,10,10,  0,0,0,0,20,20,  0,0,0,0,0,21;
+    res.setZero();
+
+    for (int i=0; i<3; i++)
+    {
+        KFPackPtr packQ;
+        int j = 0;
+        for (auto ts : q_ts)
+        {
+            packQ = kfpackbuffer.selectPackBefore(ts, tt);
+            if (packQ)
+                res(i,j) = packQ->key_frame->getTimeStamp().get();
+
+            j++;
+        }
+        kfpackbuffer.removeUpTo(packQ->key_frame->getTimeStamp());
+    }
+
+    ASSERT_MATRIX_APPROX(res, truth, 1e-6);
+}
+
 TEST_F(KFPackBufferTest, removeUpTo)
 {
     // Small time tolerance for all test asserts