diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index a6f8ac588195b3402ba2aebab998a8d8f6fcf1f7..3c18e9d5b31571dad6e63c0cebfb4c7371388da3 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -36,19 +36,83 @@ class PackKeyFrame
WOLF_PTR_TYPEDEFS(PackKeyFrame);
+
+/** \brief Buffer for arbitrary type objects
+ *
+ * Object and functions to manage a buffer of objects.
+ */
+template <typename T>
+class Buffer
+{
+public:
+
+ typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator
+
+ Buffer(){};
+ ~Buffer(void){};
+
+ /**\brief Select a Pack from the buffer
+ *
+ * Select from the buffer the closest pack (w.r.t. time stamp),
+ * respecting a defined time tolerances
+ */
+ T select(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+
+ T selectFirstBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+
+ /**\brief Buffer size
+ *
+ */
+ SizeStd size(void);
+
+ /**\brief Add a pack to the buffer
+ *
+ */
+ void add(const T& _element, const TimeStamp& _time_stamp); //const Scalar& _time_tolerance);
+
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpTo(const TimeStamp& _time_stamp);
+
+ /**\brief Clear the buffer
+ *
+ */
+ void clear();
+
+ /**\brief Empty the buffer
+ *
+ */
+ bool empty();
+
+ /**\brief Check time tolerance
+ *
+ * Check if the time distance between two time stamps is smaller than
+ * the time tolerance.
+ */
+ bool simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance);
+
+ /**\brief Check time tolerance
+ *
+ * Check if the time distance between two time stamps is smaller than
+ * the minimum time tolerance of the two frames.
+ */
+ bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
+
+protected:
+
+ std::map<TimeStamp,T> container_; // Main buffer container
+};
+
+
/** \brief Buffer of Key frame class objects
*
* Object and functions to manage a buffer of KFPack objects.
*/
-class PackKeyFrameBuffer
+class PackKeyFrameBuffer : public Buffer<PackKeyFramePtr>
{
public:
- typedef std::map<TimeStamp,PackKeyFramePtr>::iterator Iterator; // buffer iterator
-
- PackKeyFrameBuffer(void);
- ~PackKeyFrameBuffer(void);
-
/**\brief Select a Pack from the buffer
*
* Select from the buffer the closest pack (w.r.t. time stamp),
@@ -60,48 +124,32 @@ class PackKeyFrameBuffer
PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
- /**\brief Buffer size
- *
- */
- SizeStd size(void);
-
/**\brief Add a pack to the buffer
*
*/
void add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance);
- /**\brief Remove all packs in the buffer with a time stamp older than the specified
- *
- */
- void removeUpTo(const TimeStamp& _time_stamp);
-
- /**\brief Check time tolerance
- *
- * Check if the time distance between two time stamps is smaller than
- * the minimum time tolerance of the two frames.
- */
- bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
-
- /**\brief Clear the buffer
- *
- */
- void clear();
-
- /**\brief Empty the buffer
- *
- */
- bool empty();
-
/**\brief Print buffer information
*
*/
void print();
- private:
+ /**\brief Alias funct
+ *
+ */
+ bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
+ { return doubleCheckTimeTolerance(_time_stamp1, _time_tolerance1, _time_stamp2, _time_tolerance2); };
- std::map<TimeStamp,PackKeyFramePtr> container_; // Main buffer container
};
+
+/** \brief Buffer of Capture class objects
+ *
+ * Object and functions to manage a buffer of Capture objects.
+ */
+class CaptureBuffer2 : public Buffer<CaptureBasePtr> {};
+
+
/** \brief base struct for processor parameters
*
* Derive from this struct to create structs of processor parameters.
@@ -282,31 +330,130 @@ inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
params_->time_tolerance = _time_tolerance;
}
-inline PackKeyFrameBuffer::PackKeyFrameBuffer(void)
+/////////////////////////////////////////////////////////////////////////////////////////
+
+// template <typename T>
+// inline Buffer<T>::Buffer(void)
+// {
+//
+// }
+//
+// template <typename T>
+// inline Buffer<T>::~Buffer(void)
+// {
+//
+// }
+
+template <typename T>
+T Buffer<T>::select(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
{
+ if (container_.empty())
+ return nullptr;
+
+ Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+ bool post_exists = (post != container_.end());
+
+ bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+ if (prev_exists)
+ {
+ Buffer<T>::Iterator prev = std::prev(post);
+ bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+ if (prev_ok && !post_ok)
+ return prev->second;
+
+ else if (!prev_ok && post_ok)
+ return post->second;
+
+ else if (prev_ok && post_ok)
+ {
+ if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+ return post->second;
+ else
+ return prev->second;
+ }
+ }
+ else if (post_ok)
+ return post->second;
+
+ return nullptr;
}
-inline PackKeyFrameBuffer::~PackKeyFrameBuffer(void)
+template <typename T>
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
{
+ // There is no element
+ if (container_.empty())
+ return nullptr;
+
+ // Checking on begin() since packs are ordered in time
+ // Return first pack if is older than time stamp
+ if (container_.begin()->first < _time_stamp)
+ return container_.begin()->second;
+
+ // Return first pack if despite being newer, it is within the time tolerance
+ if (checkTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance))
+ return container_.begin()->second;
+ // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
+ return nullptr;
+
+}
+
+template <typename T>
+void Buffer<T>::add(const T& _element, const TimeStamp& _time_stamp)
+{
+ container_.emplace(_time_stamp, _element);
}
-inline void PackKeyFrameBuffer::clear()
+template <typename T>
+inline void Buffer<T>::clear()
{
container_.clear();
}
-inline bool PackKeyFrameBuffer::empty()
+template <typename T>
+inline bool Buffer<T>::empty()
{
return container_.empty();
}
-inline SizeStd PackKeyFrameBuffer::size(void)
+template <typename T>
+inline SizeStd Buffer<T>::size(void)
{
return container_.size();
}
+template <typename T>
+inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.upper_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
+template <typename T>
+inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
+ const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
+{
+ Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
+ Scalar time_tol = std::min(_time_tolerance1, _time_tolerance2);
+ bool pass = time_diff <= time_tol;
+ return pass;
+}
+
+template <typename T>
+inline bool Buffer<T>::simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2,
+ const Scalar& _time_tolerance)
+{
+ Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
+ bool pass = time_diff <= _time_tolerance;
+ return pass;
+}
+
} // namespace wolf
#endif
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 07da090c9b6e789f3d942a8aef226665927972cf..a06dd5e8799706a6fe7e0bb9b2a5409822a68a24 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -93,17 +93,12 @@ void ProcessorBase::remove()
}
/////////////////////////////////////////////////////////////////////////////////////////
-void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp)
-{
- PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
- container_.erase(container_.begin(), post); // erasing by range
-}
-
void PackKeyFrameBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
{
TimeStamp time_stamp = _key_frame->getTimeStamp();
PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
- container_.emplace(time_stamp, kfpack);
+ //Buffer<PackKeyFramePtr>::add(kfpack, time_stamp);
+ Buffer::add(kfpack, time_stamp);
}
PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
@@ -122,13 +117,13 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
- bool post_ok = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
+ bool post_ok = post_exists && doubleCheckTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
if (prev_exists)
{
PackKeyFrameBuffer::Iterator prev = std::prev(post);
- bool prev_ok = checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
+ bool prev_ok = doubleCheckTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev->second;
@@ -170,7 +165,7 @@ PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time
return container_.begin()->second;
// Return first pack if despite being newer, it is within the time tolerance
- if (checkTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance))
+ if (doubleCheckTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance))
return container_.begin()->second;
// otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
@@ -193,12 +188,4 @@ void PackKeyFrameBuffer::print(void)
std::cout << "]" << std::endl;
}
-bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
- const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
-{
- Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
- Scalar time_tol = std::min(_time_tolerance1, _time_tolerance2);
- bool pass = time_diff <= time_tol;
- return pass;
-}
} // namespace wolf