diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index cd7ba019179d7bfb7aa61723073028ccc3b17024..e63438750ef6ca1477a7287a03f840dc48ba596a 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -20,8 +20,13 @@ ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor H_r_s = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState()); } -void ProcessorRangeBearing::process(CaptureBasePtr _capture) +void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) { + if (_capture == nullptr) + { + WOLF_ERROR("Received capture is nullptr."); + return; + } // 1. get KF FrameBasePtr kf(nullptr); diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index a6b2e702003978c80a8c301513eccca05f28b1c5..be9159c30dcb72cdeb54872db4ef64f4e35fa3de 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/hello_wolf/processor_range_bearing.h @@ -59,7 +59,10 @@ class ProcessorRangeBearing : public ProcessorBase protected: // Implementation of pure virtuals from ProcessorBase - virtual void process (CaptureBasePtr _capture) override; + virtual void processCapture (CaptureBasePtr _capture) override; + virtual void processKeyFrame (FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override {}; + virtual bool triggerInCapture (CaptureBasePtr) override { return true;}; + virtual bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override {return false;} virtual bool voteForKeyFrame () override {return false;} private: 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_base.h b/include/core/processor/processor_base.h index 083c2c6f142dfe8df89fb18b77ae79180e2b92d4..cf2bf2c7eb911522fc78e1d4a1b92a9d4a30da6c 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -60,6 +60,10 @@ public: T selectFirstBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance); + T selectFirst(); + + T selectLast(); + /**\brief Buffer size * */ @@ -218,7 +222,32 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce unsigned int id(); - virtual void process(CaptureBasePtr _capture_ptr) = 0; + protected: + /** \brief process an incoming capture + * + * Each derived processor should implement this function. It will be called if: + * - A new capture arrived and triggerInCapture() returned true. + */ + virtual void processCapture(CaptureBasePtr) = 0; + + /** \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) = 0; + + /** \brief trigger in capture + * + * Returns true if processCapture() should be called after the provided capture arrived. + */ + virtual bool triggerInCapture(CaptureBasePtr) = 0; + + /** \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_tolerance) = 0; /** \brief Vote for KeyFrame generation * @@ -261,8 +290,19 @@ 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); + public: + /**\brief notify a new keyframe made by another processor + * + * It stores the new KF in buffer_pack_kf_ and calls triggerInKF() + * + */ + void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other); + + /**\brief notify a new capture + * + * It stores the new capture in buffer_capture_ and calls triggerInCapture() + */ + void captureCallback(CaptureBasePtr _capture_ptr); SensorBasePtr getSensor(); const SensorBasePtr getSensor() const; @@ -408,6 +448,28 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const Scalar& _time } +template <typename T> +T Buffer<T>::selectFirst() +{ + // There is no element + if (container_.empty()) + return nullptr; + + // Returning first map element + return container_.begin()->second; +} + +template <typename T> +T Buffer<T>::selectLast() +{ + // There is no element + if (container_.empty()) + return nullptr; + + // Returning last map element + return container_.rbegin()->second; +} + template <typename T> void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element) { diff --git a/include/core/processor/processor_capture_holder.h b/include/core/processor/processor_capture_holder.h index 46fc538d9a05f53181f7d3d4d5e46a7e0e6a4dc4..19b33f8f2629cf4e6d1a61888732ce3d1d197568 100644 --- a/include/core/processor/processor_capture_holder.h +++ b/include/core/processor/processor_capture_holder.h @@ -11,7 +11,6 @@ //Wolf includes #include "core/processor/processor_base.h" #include "core/capture/capture_base.h" -#include "core/capture/capture_buffer.h" namespace wolf { @@ -23,17 +22,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 +30,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/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h index afd9855b5087e61825706b006456039cba06e93d..459b8f3d9a123960be1d7b99b3ca322317726bb7 100644 --- a/include/core/processor/processor_loopclosure.h +++ b/include/core/processor/processor_loopclosure.h @@ -55,14 +55,31 @@ public: virtual ~ProcessorLoopClosure() = default; virtual void configure(SensorBasePtr _sensor) override { }; - /** \brief Full processing of an incoming Capture. +protected: + /** \brief process an incoming capture * - * Usually you do not need to overload this method in derived classes. - * Overload it only if you want to alter this algorithm. + * The ProcessorLoopClosure is only triggered in KF (see triggerInCapture()) so this function is not called. */ - virtual void process(CaptureBasePtr _incoming_ptr) override; + virtual void processCapture(CaptureBasePtr) override {}; -protected: + /** \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 ProcessorLoopClosure 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; /** \brief Called by process(). Tells if computeFeatures() will be called */ @@ -143,7 +160,7 @@ protected: * * WARNING! This function only votes! It does not create KeyFrames! */ - bool voteForKeyFrame() + virtual bool voteForKeyFrame() override { return false; }; diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index fe77c60535d7124c1c780bf6738a78743e293a1c..093f257064e1b16b4647bd3dbf83353bf287c8f8 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -153,14 +153,10 @@ class ProcessorMotion : public ProcessorBase virtual ~ProcessorMotion(); // Instructions to the processor: - - void process(CaptureBasePtr _incoming_ptr); virtual void resetDerived(); // Queries to the processor: - virtual bool isMotion(); - - virtual bool voteForKeyFrame(); + virtual bool isMotion() override; /** \brief Fill a reference to the state integrated so far * \param _x the returned state vector @@ -226,6 +222,33 @@ class ProcessorMotion : public ProcessorBase // Helper functions: protected: + /** \brief process an incoming capture + * + * Each derived processor should implement this function. It will be called if: + * - A new capture arrived and triggerInCapture() returned true. + */ + virtual void processCapture(CaptureBasePtr) override; + + /** \brief process an incoming key-frame + * + * The ProcessorMotion only processes incoming captures (it is not called). + */ + virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override {}; + + /** \brief trigger in capture + * + * Returns true if processCapture() should be called after the provided capture arrived. + */ + virtual bool triggerInCapture(CaptureBasePtr) override {return true;} + + /** \brief trigger in key-frame + * + * The ProcessorMotion only processes incoming captures, then it returns false. + */ + virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override {return false;} + + virtual bool voteForKeyFrame() override; + Scalar updateDt(); void integrateOneStep(); void reintegrateBuffer(CaptureMotionPtr _capture_ptr); @@ -245,7 +268,7 @@ class ProcessorMotion : public ProcessorBase * - initializing counters, flags, or any derived variables * - initializing algorithms needed for processing the derived data */ - virtual void preProcess() { }; + virtual void preProcess(){ }; /** Post-process * @@ -257,7 +280,7 @@ class ProcessorMotion : public ProcessorBase * - resetting and/or clearing variables and/or algorithms at the end of processing * - drawing / printing / logging the results of the processing */ - virtual void postProcess() { }; + virtual void postProcess(){ }; PackKeyFramePtr computeProcessingStep(); diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index 17de27c05b0e210b12f46a3d43ad89d6ab8510ef..7e701180a3aa2738c3f932e44fe68efbb0d5dc90 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -108,12 +108,6 @@ class ProcessorTracker : public ProcessorBase ProcessorParamsTrackerPtr _params_tracker); virtual ~ProcessorTracker(); - /** \brief Full processing of an incoming Capture. - * - * Usually you do not need to overload this method in derived classes. - * Overload it only if you want to alter this algorithm. - */ - virtual void process(CaptureBasePtr const _incoming_ptr); bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2); bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts); @@ -125,6 +119,31 @@ class ProcessorTracker : public ProcessorBase virtual CaptureBasePtr getIncoming(); protected: + /** \brief process an incoming capture + * + * Each derived processor should implement this function. It will be called if: + * - A new capture arrived and triggerInCapture() returned true. + */ + virtual void processCapture(CaptureBasePtr) override; + + /** \brief process an incoming key-frame + * + * The ProcessorTracker only processes incoming captures (it is not called). + */ + virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override {}; + + /** \brief trigger in capture + * + * Returns true if processCapture() should be called after the provided capture arrived. + */ + virtual bool triggerInCapture(CaptureBasePtr) override; + + /** \brief trigger in key-frame + * + * The ProcessorTracker only processes incoming captures, then it returns false. + */ + virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override {return false;} + /** Pre-process incoming Capture * * This is called by process() just after assigning incoming_ptr_ to a valid Capture. diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 32c555dd44522ee092c34c5b3d372eeb34a0e36b..e379a780e80da372c610ed0fc7da514d3f6dde44 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -53,14 +53,26 @@ FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _captur void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) { WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp()); - if (_keyframe_ptr != nullptr) - buffer_pack_kf_.add(_keyframe_ptr, _time_tol_other); + assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame"); + + // buffering anyway + buffer_pack_kf_.add(_keyframe_ptr, _time_tol_other); + + // if trigger true -> processKeyFrame + if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other)) + processKeyFrame(_keyframe_ptr, _time_tol_other); + } void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) { WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp()); - if (_capture_ptr != nullptr) + assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture"); + + // if trigger, process directly without buffering + if (triggerInCapture(_capture_ptr)) + processCapture(_capture_ptr); + else buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr); } 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 diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp index bbc8c1c41d601759475b810f37f004326a22cada..8448d1913bd6cee2e5b76aa23aa4aa2fbc22c008 100644 --- a/src/processor/processor_loopclosure.cpp +++ b/src/processor/processor_loopclosure.cpp @@ -19,7 +19,7 @@ ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, ProcessorPa } //############################################################################## -void ProcessorLoopClosure::process(CaptureBasePtr _incoming_ptr) +void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) { // the pre-process, if necessary, is implemented in the derived classes preProcess(); @@ -28,7 +28,6 @@ void ProcessorLoopClosure::process(CaptureBasePtr _incoming_ptr) { std::pair<FrameBasePtr,CaptureBasePtr> pairKC = selectPairKC(); - auto cap_1 = pairKC.second; auto kf_1 = pairKC.first; @@ -51,7 +50,7 @@ void ProcessorLoopClosure::process(CaptureBasePtr _incoming_ptr) auto cap_2 = findLoopCandidate(cap_1); if (cap_2==nullptr) return; - if (validateLoop(cap_1, cap_2)==false) + if (!validateLoop(cap_1, cap_2)) return; if (cap_1->getFrame() == nullptr || cap_2->getFrame() == nullptr) { @@ -66,12 +65,17 @@ void ProcessorLoopClosure::process(CaptureBasePtr _incoming_ptr) emplaceFactors(cap_1, cap_2); } } - }; + } // the post-process, if necessary, is implemented in the derived classes postProcess(); } +bool ProcessorLoopClosure::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) +{ + return true; +} + /** * In the default implementation, we select the KF with the most recent TimeStamp * and that is compatible with at least a Capture @@ -79,8 +83,9 @@ void ProcessorLoopClosure::process(CaptureBasePtr _incoming_ptr) std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosure::selectPairKC() { std::map<TimeStamp,PackKeyFramePtr> kf_container = buffer_pack_kf_.getContainer(); - if (kf_container.empty()){ - return std::make_pair(nullptr, nullptr);}; + + if (kf_container.empty()) + return std::make_pair(nullptr, nullptr); for (auto kf_it=kf_container.begin(); kf_it!=kf_container.end(); ++kf_it) { @@ -92,7 +97,7 @@ std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosure::selectPairKC() buffer_pack_kf_.removeUpTo(kf_it->first); // return the KF-Cap pair : return std::make_pair(kf_it->second->key_frame, cap_ptr); - }; + } } return std::make_pair(nullptr, nullptr); } diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 55d2c16b52f55c68d66a8494a87b7e7c906c5b64..27b56edaa822a7d9e0ceee7def1a64d0aa55dde3 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -78,7 +78,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source, reintegrateBuffer(_capture_source); } -void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) +void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) { if (_incoming_ptr == nullptr) { diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 3cde80f33ac88d48ef30d56daf0a43a3ffe7c075..0b4da2dba7f3b5877505f96e3a1af3cd8f751691 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -31,7 +31,7 @@ ProcessorTracker::~ProcessorTracker() // } -void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) +void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) { using std::abs; @@ -43,6 +43,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) incoming_ptr_ = _incoming_ptr; + preProcess(); // Derived class operations computeProcessingStep(); @@ -299,5 +300,10 @@ void ProcessorTracker::computeProcessingStep() } } +bool ProcessorTracker::triggerInCapture(CaptureBasePtr) +{ + return true; +} + } // namespace wolf diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 1be0920fb059789e99b16f3a84d11b3a33b49699..2f5a11c9b25ed40d9613f20d1f68d22d57b72bf1 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -325,7 +325,7 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr) for (const auto processor : processor_list_) { - processor->process(capture_ptr); + processor->captureCallback(capture_ptr); } return true; diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index a25cf1733f6f78f43bb717613956f38306bbfd1b..4c7852585c53a3cf176ffd63f25cb3449eb62fd8 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -408,8 +408,8 @@ TEST(Odom2D, KF_callback) processor_odom2d->keyFrameCallback(keyframe_2, dt/2); ASSERT_TRUE(problem->check(0)); t += dt; - capture->setTimeStamp(t); - processor_odom2d->process(capture); + capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); + sensor_odom2d->process(capture); ASSERT_TRUE(problem->check(0)); CaptureMotionPtr key_capture_n = std::static_pointer_cast<CaptureMotion>(keyframe_2->getCaptureList().front()); @@ -440,8 +440,8 @@ TEST(Odom2D, KF_callback) processor_odom2d->keyFrameCallback(keyframe_1, dt/2); ASSERT_TRUE(problem->check(0)); t += dt; - capture->setTimeStamp(t); - processor_odom2d->process(capture); + capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); + sensor_odom2d->process(capture); ASSERT_TRUE(problem->check(0)); CaptureMotionPtr key_capture_m = std::static_pointer_cast<CaptureMotion>(keyframe_1->getCaptureList().front()); diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 05ce862fddbdfb76937205e65d49471b0f324795..d4e4f082be12c5a39ecf79e5fbb184a0f6777182 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -65,7 +65,7 @@ TEST(ProcessorBase, KeyFrameCallback) // Track CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk)); - proc_trk->process(capt_trk); + proc_trk->captureCallback(capt_trk); for (size_t ii=0; ii<10; ii++ ) { @@ -74,11 +74,11 @@ TEST(ProcessorBase, KeyFrameCallback) WOLF_INFO("----------------------- ts: ", t , " --------------------------"); capt_odo->setTimeStamp(t); - proc_odo->process(capt_odo); + proc_odo->captureCallback(capt_odo); // Track capt_trk = make_shared<CaptureVoid>(t, sens_trk); - proc_trk->process(capt_trk); + proc_trk->captureCallback(capt_trk); // problem->print(4,1,1,0); diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp index a7fc6818d0b739682eb546923bab9cb40e9bef27..8d48be3958841fd6a7da8e98869997f379bb896d 100644 --- a/test/gtest_processor_loopclosure.cpp +++ b/test/gtest_processor_loopclosure.cpp @@ -29,17 +29,19 @@ public: std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();}; protected: - bool voteComputeFeatures() { return true;}; - bool voteSearchLoopClosure() { return true;}; - bool detectFeatures(CaptureBasePtr cap) { return true;}; - CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) { - for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) { - if (kf->isKey()) { - for (CaptureBasePtr cap : kf->getCaptureList()) { - return cap; } - };} return nullptr; + virtual bool voteComputeFeatures() { return true;}; + virtual bool voteSearchLoopClosure() { return true;}; + virtual bool detectFeatures(CaptureBasePtr cap) { return true;}; + virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) + { + for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) + if (kf->isKey()) + for (CaptureBasePtr cap : kf->getCaptureList()) + if (cap != _capture) + return cap; + return nullptr; }; - void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) { + virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) { std::cout << "factor created\n"; *factor_created = true; }; @@ -79,16 +81,16 @@ TEST(ProcessorLoopClosure, installProcessor) Matrix3s P = Matrix3s::Identity() * 0.1; problem->setPrior(x, P, t, dt/2); // KF1 + // new KF t += dt; - FrameBasePtr kf = problem->emplaceFrame(KEY, x, t); //KF2 - proc_lc->keyFrameCallback(kf, dt/2); - - // new capture - CaptureVoidPtr capt_lc(make_shared<CaptureVoid>(t, sens_lc)); + auto kf = problem->emplaceFrame(KEY, x, t); //KF2 + // emplace a capture in KF + auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc); proc_lc->captureCallback(capt_lc); - proc_lc->process(capt_lc); + // callback KF + proc_lc->keyFrameCallback(kf, dt/2); ASSERT_TRUE(factor_created); } diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 57d7a5c685f6e60aa13d57829e8f83b9afcb3233..e27e5cfbfe531427802ea12c40a157875596afbc 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -80,7 +80,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraight) capture->setTimeStamp(t); capture->setData(data); capture->setDataCovariance(data_cov); - processor->process(capture); + processor->captureCallback(capture); WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); } @@ -99,7 +99,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircle) capture->setTimeStamp(t); capture->setData(data); capture->setDataCovariance(data_cov); - processor->process(capture); + processor->captureCallback(capture); WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); } @@ -120,7 +120,7 @@ TEST_F(ProcessorMotion_test, Interpolate) capture->setTimeStamp(t); capture->setData(data); capture->setDataCovariance(data_cov); - processor->process(capture); + processor->captureCallback(capture); motions.push_back(processor->getMotion(t)); WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); }