diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index cc1d33c48b722bf2d0ad94be5e05effe2e744319..733ae9a3c56060fb942eb9c06377a4f00d9f2fcb 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -196,6 +196,7 @@ SET(HDRS_BASE
pinholeTools.h
problem.h
processor_base.h
+ processor_capture_holder.h
processor_factory.h
processor_motion.h
processor_tracker.h
@@ -290,6 +291,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
diff --git a/src/capture_buffer.h b/src/capture_buffer.h
new file mode 100644
index 0000000000000000000000000000000000000000..26537d058486a44e71543311414f62214d8834ea
--- /dev/null
+++ b/src/capture_buffer.h
@@ -0,0 +1,173 @@
+/**
+ * \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_ */
diff --git a/src/processor_capture_holder.cpp b/src/processor_capture_holder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..299b14cf81c33a688ad764414517966668995d61
--- /dev/null
+++ b/src/processor_capture_holder.cpp
@@ -0,0 +1,151 @@
+/**
+ * \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
diff --git a/src/processor_capture_holder.h b/src/processor_capture_holder.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd3b6bd71c146be7d8652fce423751e4658ae973
--- /dev/null
+++ b/src/processor_capture_holder.h
@@ -0,0 +1,72 @@
+/**
+ * \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_