From 61561e77ef271a7a64fda41c68bd37e2be2cf04a Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 16 Jan 2025 14:41:17 +0100
Subject: [PATCH] improved Buffer class and added BufferVectorComposite
---
CMakeLists.txt | 1 +
include/core/processor/buffer.h | 63 ++++++-----
src/processor/buffer.cpp | 38 +++++++
src/processor/processor_base.cpp | 4 +-
src/processor/processor_loop_closure.cpp | 4 +-
test/CMakeLists.txt | 3 +
test/gtest_buffer.cpp | 134 +++++++++++++++++++++++
test/gtest_buffer_frame.cpp | 113 ++++++-------------
8 files changed, 249 insertions(+), 111 deletions(-)
create mode 100644 src/processor/buffer.cpp
create mode 100644 test/gtest_buffer.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dab498ba7..ea35abb22 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -147,6 +147,7 @@ SET(SRCS
# problem
src/problem/problem.cpp
# processor
+ src/processor/buffer.cpp
src/processor/motion_buffer.cpp
src/processor/motion_provider.cpp
src/processor/processor_base.cpp
diff --git a/include/core/processor/buffer.h b/include/core/processor/buffer.h
index f652476bd..d675d0185 100644
--- a/include/core/processor/buffer.h
+++ b/include/core/processor/buffer.h
@@ -25,6 +25,7 @@
namespace wolf
{
+
/** \brief Buffer for arbitrary type objects
*
* Object and functions to manage a buffer of objects.
@@ -36,8 +37,8 @@ class Buffer
typedef typename std::map<TimeStamp, T>::iterator Iterator; // buffer iterator
typedef typename std::map<TimeStamp, T>::const_iterator ConstIterator; // buffer iterator
- Buffer(){};
- ~Buffer(void){};
+ Buffer() {};
+ ~Buffer(void) {};
/**\brief Select an element from the buffer
*
@@ -122,22 +123,6 @@ class Buffer
std::map<TimeStamp, T> container_; // Main buffer container
};
-/** \brief Buffer of Frames
- *
- * Object and functions to manage a buffer of FrameBasePtr objects.
- */
-class BufferFrame : public Buffer<FrameBasePtr>
-{
-};
-
-/** \brief Buffer of Captures
- *
- * Object and functions to manage a buffer of CaptureBasePtr objects.
- */
-class BufferCapture : public Buffer<CaptureBasePtr>
-{
-};
-
} // namespace wolf
#include "core/common/time_stamp.h"
@@ -221,7 +206,7 @@ inline typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _
template <typename T>
inline T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
- if (container_.empty()) return nullptr;
+ if (container_.empty()) return T(); // nullprt in case of T being a pointer
auto it = selectIterator(_time_stamp, _time_tolerance);
@@ -232,14 +217,14 @@ inline T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tol
return it->second;
}
- return nullptr;
+ return T(); // nullprt in case of T being a pointer
}
template <typename T>
inline T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
- if (container_.empty()) return nullptr;
+ if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Checking on begin() since elements are ordered in time
// Return first element if is older than time stamp
@@ -249,14 +234,14 @@ inline T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double
if (checkTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance)) return container_.begin()->second;
// otherwise return nullptr (no element before the provided ts or within the tolerance was found)
- return nullptr;
+ return T(); // nullprt in case of T being a pointer
}
template <typename T>
inline T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
- if (container_.empty()) return nullptr;
+ if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Checking on rbegin() since elements are ordered in time
// Return last element if is newer than time stamp
@@ -267,14 +252,14 @@ inline T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double&
return container_.rbegin()->second;
// otherwise return nullptr (no element after the provided ts or within the tolerance was found)
- return nullptr;
+ return T(); // nullprt in case of T being a pointer
}
template <typename T>
inline T Buffer<T>::selectFirst() const
{
// There is no element
- if (container_.empty()) return nullptr;
+ if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Returning first map element
return container_.begin()->second;
@@ -284,7 +269,7 @@ template <typename T>
inline T Buffer<T>::selectLast() const
{
// There is no element
- if (container_.empty()) return nullptr;
+ if (container_.empty()) return T(); // nullprt in case of T being a pointer
// Returning last map element
return container_.rbegin()->second;
@@ -362,4 +347,30 @@ inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
return pass;
}
+// SPECIFIC BUFFERS
+// fwd declarations
+class VectorComposite;
+
+/** \brief Buffer of Frames
+ */
+class BufferFrame : public Buffer<FrameBasePtr>
+{
+ public:
+ void emplace(FrameBasePtr _frm);
+};
+
+/** \brief Buffer of Captures
+ */
+class BufferCapture : public Buffer<CaptureBasePtr>
+{
+ public:
+ void emplace(CaptureBasePtr _cap);
+};
+
+/** \brief Buffer of VectorComposite
+ */
+class BufferVectorComposite : public Buffer<VectorComposite>
+{
+};
+
} // namespace wolf
diff --git a/src/processor/buffer.cpp b/src/processor/buffer.cpp
new file mode 100644
index 000000000..8e79ca399
--- /dev/null
+++ b/src/processor/buffer.cpp
@@ -0,0 +1,38 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024,2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "core/processor/buffer.h"
+#include "core/frame/frame_base.h"
+#include "core/capture/capture_base.h"
+
+namespace wolf
+{
+
+void BufferFrame::emplace(FrameBasePtr _frm)
+{
+ Buffer<FrameBasePtr>::emplace(_frm->getTimeStamp(), _frm);
+}
+
+void BufferCapture::emplace(CaptureBasePtr _cap)
+{
+ Buffer<CaptureBasePtr>::emplace(_cap->getTimeStamp(), _cap);
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 78d716785..10b3e1a58 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -62,7 +62,7 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe)
startKFProfiling();
// asking if frame should be stored
- if (storeKeyFrame(_keyframe)) buffer_frame_.emplace(_keyframe->getTimeStamp(), _keyframe);
+ if (storeKeyFrame(_keyframe)) buffer_frame_.emplace(_keyframe);
// asking if frame should be processed
if (triggerInKeyFrame(_keyframe)) processKeyFrame(_keyframe);
@@ -111,7 +111,7 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture)
getProblem()->applyFirstFrameOptions(_capture->getTimeStamp());
// asking if capture should be stored
- if (storeCapture(_capture)) buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
+ if (storeCapture(_capture)) buffer_capture_.emplace(_capture);
// asking if capture should be processed
if (triggerInCapture(_capture)) processCapture(_capture);
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 26a655362..4596c718a 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -71,7 +71,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
}
// CASE 3:
WOLF_DEBUG("CASE 3");
- buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
+ buffer_capture_.emplace(_capture);
}
void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe)
@@ -125,7 +125,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe)
WOLF_DEBUG("CASE 3");
// store frame
- buffer_frame_.emplace(_keyframe->getTimeStamp(), _keyframe);
+ buffer_frame_.emplace(_keyframe);
return;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 31836743b..c6e89aadf 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -41,6 +41,9 @@ wolf_add_gtest(gtest_example gtest_example.cpp) #
# ------------------ First Base classes ------------------
+# Buffer
+wolf_add_gtest(gtest_buffer gtest_buffer.cpp)
+
# BufferFrame
wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
diff --git a/test/gtest_buffer.cpp b/test/gtest_buffer.cpp
new file mode 100644
index 000000000..2cbd795f8
--- /dev/null
+++ b/test/gtest_buffer.cpp
@@ -0,0 +1,134 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024,2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "core/utils/utils_gtest.h"
+#include "core/processor/buffer.h"
+
+using namespace wolf;
+
+TEST(BufferTest, ConstructorInitializesCorrectly)
+{
+ Buffer<int> buffer;
+
+ // Add assertions to verify the initial state of the buffer
+ ASSERT_EQ(buffer.size(), 0);
+ ASSERT_TRUE(buffer.empty());
+}
+
+TEST(BufferTest, emplace)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10); // Assuming emplace is a method of Buffer
+ ASSERT_EQ(buffer.size(), 1);
+ ASSERT_EQ(buffer.getContainer().at(TimeStamp(10)), 10);
+}
+
+TEST(BufferTest, ClearEmptiesBuffer)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.clear();
+ ASSERT_EQ(buffer.size(), 0);
+ ASSERT_TRUE(buffer.empty());
+}
+
+TEST(BufferTest, SelectFirstReturnsCorrectValue)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.emplace(TimeStamp(20), 20);
+ ASSERT_EQ(buffer.selectFirst(), 10);
+}
+
+TEST(BufferTest, SelectLastReturnsCorrectValue)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.emplace(TimeStamp(20), 20);
+ ASSERT_EQ(buffer.selectLast(), 20);
+}
+
+TEST(BufferTest, SelectFirstBeforeReturnsCorrectValue)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.emplace(TimeStamp(20), 20);
+ ASSERT_EQ(buffer.selectFirstBefore(TimeStamp(15), 5), 10);
+ ASSERT_EQ(buffer.selectFirstBefore(TimeStamp(5), 1), 0); // Assuming 0 is the default value for int
+}
+
+TEST(BufferTest, SelectLastAfterReturnsCorrectValue)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.emplace(TimeStamp(20), 20);
+ ASSERT_EQ(buffer.selectLastAfter(TimeStamp(15), 5), 20);
+ ASSERT_EQ(buffer.selectLastAfter(TimeStamp(25), 1), 0); // Assuming 0 is the default value for int
+}
+
+TEST(BufferTest, SelectReturnsCorrectValue)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.emplace(TimeStamp(20), 20);
+ ASSERT_EQ(buffer.select(TimeStamp(11), 5), 10);
+ ASSERT_EQ(buffer.select(TimeStamp(11), 100), 10);
+ ASSERT_EQ(buffer.select(TimeStamp(16), 5), 20);
+ ASSERT_EQ(buffer.select(TimeStamp(16), 100), 20);
+ ASSERT_EQ(buffer.select(TimeStamp(25), 4), 0); // Assuming 0 is the default value for int
+}
+
+TEST(BufferTest, RemoveUpToRemovesCorrectElements)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.emplace(TimeStamp(20), 20);
+ buffer.removeUpTo(TimeStamp(15));
+ ASSERT_EQ(buffer.size(), 1);
+ ASSERT_EQ(buffer.selectFirst(), 20);
+}
+
+TEST(BufferTest, RemoveUpToLowerRemovesCorrectElements)
+{
+ Buffer<int> buffer;
+ buffer.emplace(TimeStamp(10), 10);
+ buffer.emplace(TimeStamp(20), 20);
+ buffer.removeUpToLower(TimeStamp(20));
+ ASSERT_EQ(buffer.size(), 1);
+ ASSERT_EQ(buffer.selectFirst(), 20);
+}
+
+TEST(BufferTest, CheckTimeToleranceWorksCorrectly)
+{
+ ASSERT_TRUE(Buffer<int>::checkTimeTolerance(TimeStamp(10), TimeStamp(15), 5));
+ ASSERT_FALSE(Buffer<int>::checkTimeTolerance(TimeStamp(10), TimeStamp(20), 5));
+}
+
+TEST(BufferTest, DoubleCheckTimeToleranceWorksCorrectly)
+{
+ ASSERT_TRUE(Buffer<int>::doubleCheckTimeTolerance(TimeStamp(10), 5, TimeStamp(15), 5));
+ ASSERT_FALSE(Buffer<int>::doubleCheckTimeTolerance(TimeStamp(10), 5, TimeStamp(20), 5));
+}
+
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_buffer_frame.cpp b/test/gtest_buffer_frame.cpp
index 8e23b962e..7020522f5 100644
--- a/test/gtest_buffer_frame.cpp
+++ b/test/gtest_buffer_frame.cpp
@@ -20,14 +20,10 @@
#include "core/utils/utils_gtest.h"
-#include "core/processor/processor_base.h"
-#include "core/capture/capture_void.h"
+#include "core/processor/buffer.h"
+#include "core/frame/frame_base.h"
#include "core/problem/problem.h"
-// STL
-#include <iterator>
-#include <iostream>
-
using namespace wolf;
using namespace Eigen;
@@ -61,16 +57,16 @@ TEST_F(BufferFrameTest, empty)
TEST_F(BufferFrameTest, emplace)
{
- buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(f10);
ASSERT_EQ(buffer_kf.size(), (unsigned int)1);
- buffer_kf.emplace(20, f20);
+ buffer_kf.emplace(f20);
ASSERT_EQ(buffer_kf.size(), (unsigned int)2);
}
TEST_F(BufferFrameTest, clear)
{
- buffer_kf.emplace(10, f10);
- buffer_kf.emplace(20, f20);
+ buffer_kf.emplace(f10);
+ buffer_kf.emplace(f20);
ASSERT_EQ(buffer_kf.size(), (unsigned int)2);
buffer_kf.clear();
ASSERT_TRUE(buffer_kf.empty());
@@ -79,80 +75,35 @@ TEST_F(BufferFrameTest, clear)
TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
{
buffer_kf.clear();
- buffer_kf.emplace(10, f10);
- buffer_kf.emplace(20, f20);
+ buffer_kf.emplace(f10);
+ buffer_kf.emplace(f20);
// min time tolerance > diff between time stamps. It should return true
ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 20, TimeStamp(20), 20));
// min time tolerance < diff between time stamps. It should return true
ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 1, TimeStamp(20), 20));
}
-// TEST_F(BufferFrameTest, select)
-//{
-// // 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
-//
-// buffer_kf.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 (unsigned int ip1=0;ip1<p1.size();++ip1)
-// {
-// for (unsigned int ip2=0;ip2<p2.size();++ip2)
-// {
-// buffer_kf.emplace(f10, p1[ip1]);
-// buffer_kf.emplace(f20, p2[ip2]);
-// for (unsigned int iq=0;iq<q.size();++iq)
-// {
-// PackKeyFramePtr packQ = buffer_kf.selectPack(16, q[iq]);
-// if (packQ!=nullptr)
-// {
-// ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
-// }
-// }
-// buffer_kf.clear();
-// }
-// }
-//}
+TEST_F(BufferFrameTest, select)
+{
+ buffer_kf.emplace(f10);
+ buffer_kf.emplace(f20);
+ buffer_kf.emplace(f21);
+ buffer_kf.emplace(f28);
+
+ ASSERT_EQ(buffer_kf.select(16, 1), nullptr);
+ ASSERT_EQ(buffer_kf.select(10, 1), f10);
+ ASSERT_EQ(buffer_kf.select(10, 0), f10);
+ ASSERT_EQ(buffer_kf.select(11, 1), f10);
+ ASSERT_EQ(buffer_kf.select(20.1, 0.1), f20);
+}
TEST_F(BufferFrameTest, selectFirstBefore)
{
buffer_kf.clear();
- buffer_kf.emplace(10, f10);
- buffer_kf.emplace(20, f20);
- buffer_kf.emplace(21, f21);
+ buffer_kf.emplace(f10);
+ buffer_kf.emplace(f20);
+ buffer_kf.emplace(f21);
// input time stamps
std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
@@ -192,16 +143,16 @@ TEST_F(BufferFrameTest, selectFirstBefore)
for (int i = 0; i < 3; i++)
{
- FrameBasePtr packQ;
+ FrameBasePtr frame;
int j = 0;
for (auto ts : q_ts)
{
- packQ = buffer_kf.selectFirstBefore(ts, tt);
- if (packQ) res(i, j) = packQ->getTimeStamp().get();
+ frame = buffer_kf.selectFirstBefore(ts, tt);
+ if (frame) res(i, j) = frame->getTimeStamp().get();
j++;
}
- buffer_kf.removeUpTo(packQ->getTimeStamp());
+ buffer_kf.removeUpTo(frame->getTimeStamp());
}
ASSERT_MATRIX_APPROX(res, truth, 1e-6);
@@ -212,9 +163,9 @@ TEST_F(BufferFrameTest, removeUpTo)
// Small time tolerance for all test asserts
double tt = 0.1;
buffer_kf.clear();
- buffer_kf.emplace(10, f10);
- buffer_kf.emplace(20, f20);
- buffer_kf.emplace(21, f21);
+ buffer_kf.emplace(f10);
+ buffer_kf.emplace(f20);
+ buffer_kf.emplace(f21);
// it should remove f20 and f10, thus size should be 1 after removal
// Specifically, only f21 should remain
@@ -226,7 +177,7 @@ TEST_F(BufferFrameTest, removeUpTo)
// Chech removal of an imprecise time stamp
// Specifically, only f28 should remain
- buffer_kf.emplace(28, f28);
+ buffer_kf.emplace(f28);
ASSERT_EQ(buffer_kf.size(), (unsigned int)2);
FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22), TypeComposite(), VectorComposite());
buffer_kf.removeUpTo(f22->getTimeStamp());
--
GitLab