Create a base Buffer class for PackKeyFrameBuffer and CaptureBuffer

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

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

[Joan Solà Ortega]

I think since the container is map<TimeStamp, T> that the API for this fcn should be add(TimeStamp, T), that is, reverse the order of the input parameters. Also in other parts of WOLF we put the TimeStamp upfront other params, so this change increases the consistency throughout the library.

[Pierre Guetschel]

I changed it in 402729e0

[Joan Solà Ortega]

This comment should be "is the buffer empty?"

[Pierre Guetschel]

Fixed in 66198eff

[Joan Solà Ortega]

This could be a static function, since it does not use internal members.

[Pierre Guetschel]

fixed in 8a6f3135