diff --git a/src/processor_base.cpp b/src/processor_base.cpp index c818aca22b7f7bb235474c0c06770ce1d468e169..e79116a6b33a186a60c1de263f1f93bbefdc4812 100644 --- a/src/processor_base.cpp +++ b/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 << "[ "; diff --git a/src/processor_base.h b/src/processor_base.h index 429cdf673b69301593f7cab92bf2b413cdef7f82..a063e66a4f116ff9b76d8c43a844951c8a107eb1 100644 --- a/src/processor_base.h +++ b/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 * */ diff --git a/src/test/gtest_KF_pack_buffer.cpp b/src/test/gtest_KF_pack_buffer.cpp index 810ab58e0497cef5f2381942b0696562a641bbd6..fb325fd84be395d3dc5fe7cb0e926e31b71e9973 100644 --- a/src/test/gtest_KF_pack_buffer.cpp +++ b/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