diff --git a/src/processor_base.cpp b/src/processor_base.cpp index b03bbdb8b18e3dc1da3a6a70e57a93ac90f3ecc5..430ed8ad9d4cda1b154c9e4ab331e1863a30d702 100644 --- a/src/processor_base.cpp +++ b/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 diff --git a/src/processor_base.h b/src/processor_base.h index fbe714ac3570763a96c2a9491864d063c3a45d23..dc6eaee20c1e01d6b9af17b3a290f959cbc6d24c 100644 --- a/src/processor_base.h +++ b/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(); diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp index b859040c1c005dcb80f034ae856eeeccf6db69dc..3750689edb419bec46bbcd7a05f6ab6829dd030d 100644 --- a/src/test/gtest_processor_base.cpp +++ b/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);