diff --git a/include/core/capture/capture_buffer.h b/include/core/capture/capture_buffer.h
deleted file mode 100644
index 6ecabc34ea5d62b3055c84b7cbf0d5cb4a94efed..0000000000000000000000000000000000000000
--- a/include/core/capture/capture_buffer.h
+++ /dev/null
@@ -1,172 +0,0 @@
-/**
- * \file capture_buffer.h
- *
- * Created on: Jul 12, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_CAPTURE_BUFFER_H_
-#define _WOLF_CAPTURE_BUFFER_H_
-
-#include "core/common/wolf.h"
-#include "core/common/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/include/core/processor/processor_capture_holder.h b/include/core/processor/processor_capture_holder.h
index 46fc538d9a05f53181f7d3d4d5e46a7e0e6a4dc4..db38fc5ab4524db72028a18bf247c05fe45c8b02 100644
--- a/include/core/processor/processor_capture_holder.h
+++ b/include/core/processor/processor_capture_holder.h
@@ -23,17 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder);
*/
struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
{
- Scalar buffer_size = 30;
- ProcessorParamsCaptureHolder() = default;
- ProcessorParamsCaptureHolder(std::string _unique_name, const wolf::paramsServer & _server):
- ProcessorParamsBase(_unique_name, _server)
- {
- buffer_size = _server.getParam<Scalar>(_unique_name + "/buffer_size");
- }
- std::string print()
- {
- return "\n" + ProcessorParamsBase::print() + "buffer_size: " + std::to_string(buffer_size) + "\n";
- }
+ using ProcessorParamsBase::ProcessorParamsBase;
};
/**
@@ -41,42 +31,55 @@ struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
*/
class ProcessorCaptureHolder : public ProcessorBase
{
-public:
-
- ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder);
- virtual ~ProcessorCaptureHolder() = default;
- virtual void configure(SensorBasePtr _sensor) override { };
-
- 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 void 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:
-
- ProcessorParamsCaptureHolderPtr params_capture_holder_;
- CaptureBuffer buffer_;
-
-public:
-
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr sensor_ptr = nullptr);
+ public:
+
+ ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder);
+ virtual ~ProcessorCaptureHolder() = default;
+ virtual void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ /** \brief process an incoming capture
+ *
+ * The ProcessorCaptureHolder is only triggered in KF (see triggerInCapture()) so this function is not called.
+ */
+ virtual void processCapture(CaptureBasePtr) override {};
+
+ /** \brief process an incoming key-frame
+ *
+ * Each derived processor should implement this function. It will be called if:
+ * - A new KF arrived and triggerInKF() returned true.
+ */
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override;
+
+ /** \brief trigger in capture
+ *
+ * The ProcessorCaptureHolder only processes incoming KF, then it returns false.
+ */
+ virtual bool triggerInCapture(CaptureBasePtr) override {return false;}
+
+ /** \brief trigger in key-frame
+ *
+ * Returns true if processKeyFrame() should be called after the provided KF arrived.
+ */
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override { return true; }
+
+ /** \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; }
+
+ ProcessorParamsCaptureHolderPtr params_capture_holder_;
+
+ public:
+
+ static ProcessorBasePtr create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params,
+ const SensorBasePtr sensor_ptr = nullptr);
};
} // namespace wolf
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
index d8a98166c11ecc1361ed1194a5b2ad0e1b241cb6..5cad88a4e5c5eca8e94d6af99a67eb22ba9e3147 100644
--- a/src/processor/processor_capture_holder.cpp
+++ b/src/processor/processor_capture_holder.cpp
@@ -12,135 +12,67 @@ namespace wolf {
ProcessorCaptureHolder::ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder) :
ProcessorBase("CAPTURE HOLDER", _params_capture_holder),
- params_capture_holder_(_params_capture_holder),
- buffer_(_params_capture_holder->buffer_size)
+ params_capture_holder_(_params_capture_holder)
{
//
}
-void ProcessorCaptureHolder::process(CaptureBasePtr _capture_ptr)
+void ProcessorCaptureHolder::processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance)
{
- buffer_.push_back(_capture_ptr);
-}
-
-void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tolerance)
-{
- assert(_keyframe_ptr->getTrajectory() != nullptr
+ assert(_keyframe_ptr->getTrajectory() != nullptr
&& "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
- // get keyframe's time stamp
- const TimeStamp new_ts = _keyframe_ptr->getTimeStamp();
+ // 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);
+ // find capture whose buffer is affected by the new keyframe
+ CaptureBasePtr existing_capture = buffer_capture_.selectFirstBefore(new_ts, _time_tolerance);
+ buffer_capture_.removeUpTo(existing_capture->getTimeStamp());
- if (existing_capture == nullptr)
- {
- WOLF_WARN("Could not find a capture at ts : ", new_ts.get());
- return;
- }
+ if (existing_capture == nullptr)
+ {
+ WOLF_WARN("Could not find a capture at ts: ", new_ts.get(), " within time tolerance: ", _time_tolerance);
+ return;
+ }
- WOLF_DEBUG("ProcessorCaptureHolder::keyFrameCallback", _time_tolerance);
- WOLF_DEBUG("Capture of type : ", existing_capture->getType());
- WOLF_DEBUG("CaptureBuffer size : ", buffer_.container_.size());
+ WOLF_DEBUG("ProcessorCaptureHolder::keyFrameCallback time tolerance ", _time_tolerance);
+ WOLF_DEBUG("Capture of type : ", existing_capture->getType());
+ WOLF_DEBUG("CaptureBuffer size : ", buffer_capture_.size());
- // add motion capture to keyframe
- if (std::abs(new_ts - existing_capture->getTimeStamp()) < _time_tolerance)
- {
+ // add capture to keyframe
auto frame_ptr = existing_capture->getFrame();
- if (frame_ptr != _keyframe_ptr)
+ if (frame_ptr != nullptr && frame_ptr->isKey())
{
- // _keyframe_ptr->addCapture(existing_capture);
+ WOLF_WARN_COND(frame_ptr != _keyframe_ptr, "found a capture already in a different KF");
+ WOLF_INFO_COND(frame_ptr == _keyframe_ptr, "found a capture already in a this KF");
+ }
+ else
+ {
+ WOLF_INFO("Adding capture laser !");
existing_capture->link(_keyframe_ptr);
- //WOLF_INFO("Adding capture laser !");
-
- //frame_ptr->remove();
+ if (frame_ptr)
+ frame_ptr->remove();
}
- //else
- //WOLF_INFO("Capture laser already exists !");
-
- // Remove as we don't want duplicates
- buffer_.remove(existing_capture);
-
- return;
- }
- else
- {
- WOLF_DEBUG("Capture doesn't fit dt : ",
- std::abs(new_ts - existing_capture->getTimeStamp()),
- " vs ", _time_tolerance);
-
- return;
- }
-}
-
-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 = origin_ptr_;
-// else if (capture_ptr->getOriginFrame() == nullptr)
-// return nullptr;
-// else
-// {
-// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFrame()->getCaptureOf(getSensor());
-// 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;
+ ProcessorParamsCaptureHolderPtr params;
- params = std::static_pointer_cast<ProcessorParamsCaptureHolder>(_params);
+ params = std::static_pointer_cast<ProcessorParamsCaptureHolder>(_params);
- // if cast failed use default value
- if (params == nullptr)
- params = std::make_shared<ProcessorParamsCaptureHolder>();
+ // if cast failed use default value
+ if (params == nullptr)
+ params = std::make_shared<ProcessorParamsCaptureHolder>();
- ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params);
- prc_ptr->setName(_unique_name);
+ ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params);
+ prc_ptr->setName(_unique_name);
- return prc_ptr;
+ return prc_ptr;
}
} // namespace wolf