From 5bafe1060b265d0439a88587f9ba7ad379687f1a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 26 Jun 2019 13:17:06 +0200
Subject: [PATCH] adapted processors to new API
---
hello_wolf/processor_range_bearing.cpp | 7 +++-
hello_wolf/processor_range_bearing.h | 5 ++-
.../core/processor/processor_loopclosure.h | 29 ++++++++++++---
include/core/processor/processor_motion.h | 37 +++++++++++++++----
include/core/processor/processor_tracker.h | 31 +++++++++++++---
src/processor/processor_loopclosure.cpp | 19 ++++++----
src/processor/processor_motion.cpp | 2 +-
src/processor/processor_tracker.cpp | 8 +++-
src/sensor/sensor_base.cpp | 2 +-
9 files changed, 109 insertions(+), 31 deletions(-)
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index cd7ba0191..e63438750 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 a6b2e7020..be9159c30 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/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
index afd9855b5..459b8f3d9 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 fe77c6053..093f25706 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 17de27c05..7e701180a 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_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
index bbc8c1c41..8448d1913 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 55d2c16b5..27b56edaa 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 3cde80f33..0b4da2dba 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 1be0920fb..2f5a11c9b 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;
--
GitLab