add processor_capture_holder

src/CMakeLists.txt

@@ -182,6 +182,7 @@ SET(HDRS_BASE
     pinholeTools.h
     problem.h
     processor_base.h
+    processor_capture_holder.h
     processor_factory.h
     processor_motion.h
     processor_tracker.h
@@ -276,6 +277,7 @@ SET(SRCS_BASE
     node_base.cpp
     problem.cpp
     processor_base.cpp
+    processor_capture_holder.cpp
     processor_motion.cpp
     processor_tracker.cpp
     processor_tracker_feature.cpp

src/capture_buffer.h

+/**
+ * \file capture_buffer.h
+ *
+ *  Created on: Jul 12, 2017
+ *  \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_CAPTURE_BUFFER_H_
+#define _WOLF_CAPTURE_BUFFER_H_
+
+#include "wolf.h"
+#include "time_stamp.h"
+
+#include <list>
+#include <algorithm>
+
+namespace wolf {
+
+//using namespace Eigen;
+
+enum class CaptureBufferPolicy : std::size_t
+{
+  TIME = 0,
+  NUM_CAPTURES,
+  ALL
+};
+
+/** \brief class for capture buffers.
+ *
+ * It consists of a buffer of pre-integrated motions (aka. delta-integrals) that is being filled
+ * by the motion processors (deriving from ProcessorMotion).
+ * Each delta-integral is accompanied by a time stamp, a Jacobian and a covariances matrix.
+ *
+ * This buffer contains the time stamp and delta-integrals:
+ *  - since the last key-Frame
+ *  - until the frame of this capture.
+ *
+ * The buffer can be queried for motion-integrals and delta-integrals corresponding to a provided time stamp,
+ * with the following rules:
+ *   - If the query time stamp is later than the last one in the buffer,
+ *     the last motion-integral or delta-integral is returned.
+ *   - If the query time stamp is earlier than the beginning of the buffer,
+ *     the earliest Motion or Delta is returned.
+ *   - If the query time stamp matches one time stamp in the buffer exactly,
+ *     the returned motion-integral or delta-integral is the one of the queried time stamp.
+ *   - If the query time stamp does not match any time stamp in the buffer,
+ *     the returned motion-integral or delta-integral is the one immediately before the query time stamp.
+ */
+
+//template <CaptureBufferPolicy policy>
+class CaptureBuffer
+{
+public:
+
+  CaptureBuffer(const Scalar _buffer_dt, const int _max_capture = -1);
+  ~CaptureBuffer() = default;
+
+  void push_back(const CaptureBasePtr& capture);
+
+//  std::list<CaptureBasePtr>& get();
+//  const std::list<CaptureBasePtr>& get() const;
+
+  const CaptureBasePtr& getCapture(const TimeStamp& _ts) const;
+  void getCapture(const TimeStamp& _ts, CaptureBasePtr& _motion) const;
+
+  void remove(const CaptureBasePtr& capture);
+
+  void clear();
+//  void print();
+
+  const TimeStamp& earliest() const;
+  const TimeStamp& latest() const;
+
+//private:
+
+  int max_capture_;
+  Scalar buffer_dt_;
+
+  std::list<CaptureBasePtr> container_;
+};
+
+
+CaptureBuffer::CaptureBuffer(const Scalar _buffer_dt, const int _max_capture) :
+  max_capture_(_max_capture), buffer_dt_(_buffer_dt)
+{
+  //
+}
+
+void CaptureBuffer::push_back(const CaptureBasePtr& capture)
+{
+  container_.push_back(capture);
+
+  WOLF_DEBUG("Buffer dt : ", container_.back()->getTimeStamp() -
+             container_.front()->getTimeStamp(), " vs ", buffer_dt_);
+
+  while (container_.back()->getTimeStamp() -
+         container_.front()->getTimeStamp() > buffer_dt_)
+  {
+    container_.pop_front();
+  }
+}
+
+const CaptureBasePtr& CaptureBuffer::getCapture(const TimeStamp& _ts) const
+{
+  //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+  auto previous = std::find_if(container_.rbegin(), container_.rend(),
+  [&](const CaptureBasePtr& c)
+  {
+    return c->getTimeStamp() <= _ts;
+  });
+
+  if (previous == container_.rend())
+    // The time stamp is older than the buffer's oldest data.
+    // We could do something here, and throw an error or something, but by now we'll return the first valid data
+    previous--;
+
+  return *previous;
+}
+
+void CaptureBuffer::getCapture(const TimeStamp& _ts, CaptureBasePtr& _capture) const
+{
+//  //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+//  auto previous = std::find_if(container_.rbegin(), container_.rend(),
+//  [&](const Motion& m)
+//  {
+//    return c->getTimeStamp() <= _ts;
+//  });
+
+//  if (previous == container_.rend())
+//    // The time stamp is older than the buffer's oldest data.
+//    // We could do something here, but by now we'll return the last valid data
+//    previous--;
+
+//  _capture = *previous;
+
+  _capture = getCapture(_ts);
+}
+
+//inline std::list<CaptureBasePtr>& CaptureBuffer::get()
+//{
+//  return container_;
+//}
+
+//inline const std::list<CaptureBasePtr>& CaptureBuffer::get() const
+//{
+//  return container_;
+//}
+
+inline void CaptureBuffer::remove(const CaptureBasePtr& capture)
+{
+  container_.remove(capture);
+}
+
+inline void CaptureBuffer::clear()
+{
+  return container_.clear();
+}
+
+inline const TimeStamp& CaptureBuffer::earliest() const
+{
+  return (!container_.empty())? container_.front()->getTimeStamp() :
+                                InvalidStamp;
+}
+
+inline const TimeStamp& CaptureBuffer::latest() const
+{
+  return (!container_.empty())? container_.back()->getTimeStamp() :
+                                InvalidStamp;
+}
+
+} // namespace wolf
+
+#endif /* _WOLF_CAPTURE_BUFFER_H_ */

src/processor_capture_holder.cpp

+/**
+ * \file processor_capture_holder.h
+ *
+ *  Created on: Jul 12, 2017
+ *  \author: Jeremie Deray
+ */
+
+//Wolf includes
+#include "processor_capture_holder.h"
+
+namespace wolf {
+
+ProcessorCaptureHolder::ProcessorCaptureHolder(const Scalar& _buffer_size) :
+  ProcessorBase("CAPTURE HOLDER", _buffer_size/2.),
+  buffer_(_buffer_size)
+{
+  //
+}
+
+void ProcessorCaptureHolder::process(CaptureBasePtr _capture_ptr)
+{
+  buffer_.push_back(_capture_ptr);
+}
+
+bool ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
+                                              const Scalar& _time_tolerance)
+{
+  assert(_keyframe_ptr->getTrajectoryPtr() != nullptr
+          && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
+
+  // get keyframe's time stamp
+  const TimeStamp new_ts = _keyframe_ptr->getTimeStamp();
+
+  // find capture whose buffer is affected by the new keyframe
+  CaptureBasePtr existing_capture = findCaptureContainingTimeStamp(new_ts);
+
+  if (existing_capture == nullptr)
+  {
+    WOLF_WARN("Could not find a capture at ts : ", new_ts.get());
+    return false;
+  }
+
+  WOLF_DEBUG("ProcessorCaptureHolder::keyFrameCallback", _time_tolerance);
+  WOLF_DEBUG("Capture of type : ", existing_capture->getType());
+  WOLF_DEBUG("CaptureBuffer size : ", buffer_.container_.size());
+
+  // add motion capture to keyframe
+  if (std::abs(new_ts - existing_capture->getTimeStamp()) < _time_tolerance)
+  {
+    auto frame_ptr = existing_capture->getFramePtr();
+
+    if (frame_ptr != _keyframe_ptr)
+    {
+      _keyframe_ptr->addCapture(existing_capture);
+
+      //WOLF_INFO("Adding capture laser !");
+
+      //frame_ptr->remove();
+    }
+    //else
+      //WOLF_INFO("Capture laser already exists !");
+
+    // Remove as we don't want duplicates
+    buffer_.remove(existing_capture);
+
+    return true;
+  }
+  else
+  {
+    WOLF_DEBUG("Capture doesn't fit dt : ",
+               std::abs(new_ts - existing_capture->getTimeStamp()),
+               " vs ", _time_tolerance);
+
+    return false;
+  }
+}
+
+CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
+{
+  WOLF_DEBUG("ProcessorCaptureHolder::findCaptureContainingTimeStamp: ts = ",
+             _ts.getSeconds(), ".", _ts.getNanoSeconds());
+
+//  auto capture_ptr = last_ptr_;
+//  while (capture_ptr != nullptr)
+//  {
+//    // capture containing motion previous than the ts found:
+//    if (buffer_.get().front()->getTimeStamp() < _ts)
+//      return capture_ptr;
+//    else
+//    {
+//      // go to the previous motion capture
+//      if (capture_ptr == last_ptr_)
+//        capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+//      else if (capture_ptr->getOriginFramePtr() == nullptr)
+//        return nullptr;
+//      else
+//      {
+//        CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+//        if (capture_base_ptr == nullptr)
+//          return nullptr;
+//        else
+//          capture_ptr = std::static_pointer_cast<CaptureMotion>(capture_base_ptr);
+//      }
+//    }
+//  }
+
+//  return capture_ptr;.
+
+//  auto capt = buffer_.getCapture(_ts);
+
+  /// @todo
+//  WOLF_WARN("ProcessorCaptureHolder::findCaptureContainingTimeStamp "
+//            "looking for stamp ",
+//            _ts.get() - ((long)_ts.get()),
+//            " in (",
+//            buffer_.earliest().get() - ((long)buffer_.earliest().get()), ",",
+//            buffer_.latest().get() - ((long)buffer_.latest().get()), ").\n",
+//            "Found capture with stamp ",
+//            capt->getTimeStamp().get() - ((long)capt->getTimeStamp().get()));
+
+//  return capt;
+
+
+  return buffer_.getCapture(_ts);
+}
+
+ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name,
+                                                const ProcessorParamsBasePtr _params,
+                                                const SensorBasePtr)
+{
+  ProcessorParamsCaptureHolderPtr params;
+
+  params = std::static_pointer_cast<ProcessorParamsCaptureHolder>(_params);
+
+  // if cast failed use default value
+  if (params == nullptr)
+    params = std::make_shared<ProcessorParamsCaptureHolder>();
+
+  ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params->buffer_size_);
+  prc_ptr->setName(_unique_name);
+
+  return prc_ptr;
+}
+
+} // namespace wolf
+
+// Register in the ProcessorFactory
+#include "processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("CAPTURE HOLDER", ProcessorCaptureHolder)
+} // namespace wolf

src/processor_capture_holder.h

+/**
+ * \file processor_capture_holder.h
+ *
+ *  Created on: Jul 12, 2017
+ *  \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_PROCESSOR_CAPTURE_HOLDER_H_
+#define _WOLF_PROCESSOR_CAPTURE_HOLDER_H_
+
+//Wolf includes
+#include "processor_base.h"
+#include "capture_base.h"
+#include "capture_buffer.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ProcessorCaptureHolder);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder);
+
+/**
+ * \brief ProcessorParamsCaptureHolder
+ */
+struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
+{
+  Scalar buffer_size_ = 30;
+};
+
+/**
+ * \brief ProcessorCaptureHolder
+ */
+class ProcessorCaptureHolder : public ProcessorBase
+{
+public:
+
+  ProcessorCaptureHolder(const Scalar& _buffer_size = 1);
+  virtual ~ProcessorCaptureHolder() = default;
+
+  virtual void process(CaptureBasePtr _capture_ptr) override;
+
+  /** \brief Vote for KeyFrame generation
+   *
+   * If a KeyFrame criterion is validated, this function returns true,
+   * meaning that it wants to create a KeyFrame at the \b last Capture.
+   *
+   * WARNING! This function only votes! It does not create KeyFrames!
+   */
+  virtual bool voteForKeyFrame() override { return false; }
+
+  virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr,
+                                const Scalar& _time_tolerance) override;
+
+  /**
+   * \brief Finds the capture that contains the closest previous motion of _ts
+   * \return a pointer to the capture (if it exists) or a nullptr (otherwise)
+   */
+  CaptureBasePtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+
+protected:
+
+  CaptureBuffer buffer_;
+
+public:
+
+  static ProcessorBasePtr create(const std::string& _unique_name,
+                                 const ProcessorParamsBasePtr _params,
+                                 const SensorBasePtr sensor_ptr = nullptr);
+};
+
+} // namespace wolf
+
+#endif // _WOLF_PROCESSOR_CAPTURE_HOLDER_H_