Skip to content
Snippets Groups Projects
Commit 0baa9a03 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

adapted capture holder (removed now duplicated capture buffer)

parent 5bafe106
No related branches found
No related tags found
1 merge request!291Resolve "New processors workflow"
/**
* \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_ */
...@@ -23,17 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder); ...@@ -23,17 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder);
*/ */
struct ProcessorParamsCaptureHolder : public ProcessorParamsBase struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
{ {
Scalar buffer_size = 30; using ProcessorParamsBase::ProcessorParamsBase;
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";
}
}; };
/** /**
...@@ -41,42 +31,55 @@ struct ProcessorParamsCaptureHolder : public ProcessorParamsBase ...@@ -41,42 +31,55 @@ struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
*/ */
class ProcessorCaptureHolder : public ProcessorBase class ProcessorCaptureHolder : public ProcessorBase
{ {
public: public:
ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder); ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder);
virtual ~ProcessorCaptureHolder() = default; virtual ~ProcessorCaptureHolder() = default;
virtual void configure(SensorBasePtr _sensor) override { }; virtual void configure(SensorBasePtr _sensor) override { };
virtual void process(CaptureBasePtr _capture_ptr) override; protected:
/** \brief Vote for KeyFrame generation /** \brief process an incoming capture
* *
* If a KeyFrame criterion is validated, this function returns true, * The ProcessorCaptureHolder is only triggered in KF (see triggerInCapture()) so this function is not called.
* meaning that it wants to create a KeyFrame at the \b last Capture. */
* virtual void processCapture(CaptureBasePtr) override {};
* WARNING! This function only votes! It does not create KeyFrames!
*/ /** \brief process an incoming key-frame
virtual bool voteForKeyFrame() override { return false; } *
* Each derived processor should implement this function. It will be called if:
virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr, * - A new KF arrived and triggerInKF() returned true.
const Scalar& _time_tolerance) override; */
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override;
/**
* \brief Finds the capture that contains the closest previous motion of _ts /** \brief trigger in capture
* \return a pointer to the capture (if it exists) or a nullptr (otherwise) *
*/ * The ProcessorCaptureHolder only processes incoming KF, then it returns false.
CaptureBasePtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const; */
virtual bool triggerInCapture(CaptureBasePtr) override {return false;}
protected:
/** \brief trigger in key-frame
ProcessorParamsCaptureHolderPtr params_capture_holder_; *
CaptureBuffer buffer_; * Returns true if processKeyFrame() should be called after the provided KF arrived.
*/
public: virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override { return true; }
static ProcessorBasePtr create(const std::string& _unique_name, /** \brief Vote for KeyFrame generation
const ProcessorParamsBasePtr _params, *
const SensorBasePtr sensor_ptr = nullptr); * 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 } // namespace wolf
......
...@@ -12,135 +12,67 @@ namespace wolf { ...@@ -12,135 +12,67 @@ namespace wolf {
ProcessorCaptureHolder::ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder) : ProcessorCaptureHolder::ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder) :
ProcessorBase("CAPTURE HOLDER", _params_capture_holder), ProcessorBase("CAPTURE HOLDER", _params_capture_holder),
params_capture_holder_(_params_capture_holder), params_capture_holder_(_params_capture_holder)
buffer_(_params_capture_holder->buffer_size)
{ {
// //
} }
void ProcessorCaptureHolder::process(CaptureBasePtr _capture_ptr) void ProcessorCaptureHolder::processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance)
{ {
buffer_.push_back(_capture_ptr); assert(_keyframe_ptr->getTrajectory() != nullptr
}
void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
const Scalar& _time_tolerance)
{
assert(_keyframe_ptr->getTrajectory() != nullptr
&& "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory."); && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
// get keyframe's time stamp // get keyframe's time stamp
const TimeStamp new_ts = _keyframe_ptr->getTimeStamp(); const TimeStamp new_ts = _keyframe_ptr->getTimeStamp();
// find capture whose buffer is affected by the new keyframe // find capture whose buffer is affected by the new keyframe
CaptureBasePtr existing_capture = findCaptureContainingTimeStamp(new_ts); CaptureBasePtr existing_capture = buffer_capture_.selectFirstBefore(new_ts, _time_tolerance);
buffer_capture_.removeUpTo(existing_capture->getTimeStamp());
if (existing_capture == nullptr) if (existing_capture == nullptr)
{ {
WOLF_WARN("Could not find a capture at ts : ", new_ts.get()); WOLF_WARN("Could not find a capture at ts: ", new_ts.get(), " within time tolerance: ", _time_tolerance);
return; return;
} }
WOLF_DEBUG("ProcessorCaptureHolder::keyFrameCallback", _time_tolerance); WOLF_DEBUG("ProcessorCaptureHolder::keyFrameCallback time tolerance ", _time_tolerance);
WOLF_DEBUG("Capture of type : ", existing_capture->getType()); WOLF_DEBUG("Capture of type : ", existing_capture->getType());
WOLF_DEBUG("CaptureBuffer size : ", buffer_.container_.size()); WOLF_DEBUG("CaptureBuffer size : ", buffer_capture_.size());
// add motion capture to keyframe // add capture to keyframe
if (std::abs(new_ts - existing_capture->getTimeStamp()) < _time_tolerance)
{
auto frame_ptr = existing_capture->getFrame(); 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); existing_capture->link(_keyframe_ptr);
//WOLF_INFO("Adding capture laser !"); if (frame_ptr)
frame_ptr->remove();
//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, ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params, const ProcessorParamsBasePtr _params,
const SensorBasePtr) 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 cast failed use default value
if (params == nullptr) if (params == nullptr)
params = std::make_shared<ProcessorParamsCaptureHolder>(); params = std::make_shared<ProcessorParamsCaptureHolder>();
ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params); ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params);
prc_ptr->setName(_unique_name); prc_ptr->setName(_unique_name);
return prc_ptr; return prc_ptr;
} }
} // namespace wolf } // namespace wolf
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment