Resolve "ProcessorLoopClosureBase class"

Closes #220 (closed)

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