Resolve "ProcessorLoopClosureBase class"

Closes #220 (closed)

include/core/processor/processor_base.h

@@ -36,19 +36,89 @@ 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 TimeStamp& _time_stamp, const T& _element); //const Scalar& _time_tolerance);
+
+    /** \brief returns the container with elements of the buffer
+    *
+    * elements are ordered from most recent to oldest
+    */
+    std::map<TimeStamp,T> getContainer();
+
+    /**\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 is the buffer empty ?
+    *
+    */
+    bool empty();
+
+    /**\brief Check time tolerance
+    *
+    * Check if the time distance between two time stamps is smaller than
+    * the time tolerance.
+    */
+    static 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.
+    */
+    static 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 BufferPackKeyFrame
+class BufferPackKeyFrame : public Buffer<PackKeyFramePtr>
 {
     public:
 
-        typedef std::map<TimeStamp,PackKeyFramePtr>::iterator Iterator; // buffer iterator
-
-        BufferPackKeyFrame(void);
-        ~BufferPackKeyFrame(void);
-
         /**\brief Select a Pack from the buffer
          *
          *  Select from the buffer the closest pack (w.r.t. time stamp),
@@ -60,48 +130,32 @@ class BufferPackKeyFrame
         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
+        *
+        */
+        static 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 BufferCapture : public Buffer<CaptureBasePtr> {};
+
+
 /** \brief base struct for processor parameters
  *
  * Derive from this struct to create structs of processor parameters.
@@ -143,6 +197,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
         unsigned int processor_id_;
         ProcessorParamsBasePtr params_;
         BufferPackKeyFrame buffer_pack_kf_;
+        BufferCapture buffer_capture_;
 
     private:
         SensorBaseWPtr sensor_ptr_;
@@ -200,6 +255,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
         FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state);
 
         virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
+        virtual void captureCallback(CaptureBasePtr _capture_ptr);
 
         SensorBasePtr getSensor();
         const SensorBasePtr getSensor() const;
@@ -281,31 +337,124 @@ inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
     params_->time_tolerance = _time_tolerance;
 }
 
-inline BufferPackKeyFrame::BufferPackKeyFrame(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 && simpleCheckTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+    if (prev_exists)
+    {
+        Buffer<T>::Iterator prev = std::prev(post);
+
+        bool prev_ok = simpleCheckTimeTolerance(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 BufferPackKeyFrame::~BufferPackKeyFrame(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 (simpleCheckTimeTolerance(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;
 
 }
 
-inline void BufferPackKeyFrame::clear()
+template <typename T>
+void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
+{
+    container_.emplace(_time_stamp, _element);
+}
+
+template <typename T>
+std::map<TimeStamp,T> Buffer<T>::getContainer()
+{
+    return container_;
+}
+
+template <typename T>
+inline void Buffer<T>::clear()
 {
     container_.clear();
 }
 
-inline bool BufferPackKeyFrame::empty()
+template <typename T>
+inline bool Buffer<T>::empty()
 {
     return container_.empty();
 }
 
-inline SizeStd BufferPackKeyFrame::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