adding profiling in processorTracker

include/core/processor/processor_base.h

@@ -139,7 +139,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
     void stopCaptureProfiling();
     void startKFProfiling();
     void stopKFProfiling();
-    void printProfiling(std::ostream& stream = std::cout) const;
+    virtual void printProfiling(std::ostream& stream = std::cout) const;
 
     /** \brief constructor
      *

include/core/processor/processor_tracker.h

@@ -107,6 +107,10 @@ class ProcessorTracker : public ProcessorBase
     int max_new_features_;  ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0:
                             ///< none.)
 
+    // profiling
+    ProfilingUnit preprocess_profiling_, process_known_profiling_, process_new_profiling_,
+        establish_factors_profiling_, postprocess_profiling_;
+
   public:
     ProcessorTracker(const std::string& _type, const StateKeys& _structure, int _dim, const YAML::Node& _params);
     ~ProcessorTracker() override;
@@ -122,6 +126,8 @@ class ProcessorTracker : public ProcessorBase
     virtual CaptureBaseConstPtr getIncoming() const;
     virtual CaptureBasePtr      getIncoming();
 
+    void printProfiling(std::ostream& stream = std::cout) const override;
+
   protected:
     /** \brief process an incoming capture
      *
@@ -183,7 +189,7 @@ class ProcessorTracker : 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
      *
@@ -195,7 +201,7 @@ class ProcessorTracker : 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() {};
 
     /** \brief Tracker function
      * \return The number of successful tracks.
@@ -271,6 +277,14 @@ class ProcessorTracker : public ProcessorBase
     FeatureBasePtrList& getNewFeaturesListIncoming();
 
     void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
+
+  private:
+    // call to derivate methods with profiling
+    void         preProcessProfiling();
+    void         postProcessProfiling();
+    unsigned int processKnownProfiling();
+    unsigned int processNewProfiling(const int& _max_features);
+    void         establishFactorsProfiling();
 };
 
 inline FeatureBaseConstPtrList ProcessorTracker::getNewFeaturesListLast() const
@@ -345,4 +359,43 @@ inline CaptureBasePtr ProcessorTracker::getIncoming()
     return incoming_ptr_;
 }
 
+inline void ProcessorTracker::preProcessProfiling()
+{
+    preprocess_profiling_.startProfiling();
+    preProcess();
+    preprocess_profiling_.stopProfiling();
+}
+
+inline void ProcessorTracker::postProcessProfiling()
+{
+    postprocess_profiling_.startProfiling();
+    postProcess();
+    postprocess_profiling_.stopProfiling();
+}
+
+inline unsigned int ProcessorTracker::processKnownProfiling()
+{
+    process_known_profiling_.startProfiling();
+    auto result = processKnown();
+    process_known_profiling_.stopProfiling();
+
+    return result;
+}
+
+inline unsigned int ProcessorTracker::processNewProfiling(const int& _max_features)
+{
+    process_new_profiling_.startProfiling();
+    auto result = processNew(_max_features);
+    process_new_profiling_.stopProfiling();
+    
+    return result;
+}
+
+inline void ProcessorTracker::establishFactorsProfiling()
+{
+    establish_factors_profiling_.startProfiling();
+    establishFactors();
+    establish_factors_profiling_.stopProfiling();
+}
+
 }  // namespace wolf

src/processor/processor_tracker.cpp

@@ -60,7 +60,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
     incoming_ptr_ = _incoming_ptr;
 
-    preProcess();  // Derived class operations
+    preProcessProfiling();  // Including profiling and derived class operations
 
     computeProcessingStep();
 
@@ -98,7 +98,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
             // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything
             // to do. TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
-            processKnown();
+            processKnownProfiling();
 
             // Reset this
             resetDerived();
@@ -130,7 +130,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Process info
             // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything
             // to do. TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
-            processKnown();
+            processKnownProfiling();
 
             // Issue KF callback with new KF
             getProblem()->keyFrameCallback(keyframe, shared_from_this());
@@ -175,13 +175,13 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                                                              getProblem()->getState(incoming_ptr_->getTimeStamp()));
 
             // Process known information
-            processKnown();
+            processKnownProfiling();
 
             // Both Trackers:  We have a last_ Capture with not enough features, so populate it.
-            processNew(max_new_features_);
+            processNewProfiling(max_new_features_);
 
             // Establish factors
-            establishFactors();
+            establishFactorsProfiling();
 
             // Reset this
             resetDerived();
@@ -204,7 +204,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                        " callback unpacked with ts= ",
                        keyframe_from_callback->getTimeStamp());
 
-            processKnown();
+            processKnownProfiling();
 
             // chack if the received KF has a capture of this sensor, and if it matches with last_ptr
             if (last_ptr_ == keyframe_from_callback->getCaptureOf(this->getSensor()))
@@ -230,10 +230,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 incoming_ptr_->getTimeStamp(), getProblem()->getFrameTypes(state_keys_), getProblem()->getState());
 
             // Detect new Features, initialize Landmarks, ...
-            processNew(max_new_features_);
+            processNewProfiling(max_new_features_);
 
             // Establish factors
-            establishFactors();
+            establishFactorsProfiling();
 
             // Reset this
             resetDerived();
@@ -248,12 +248,12 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         case RUNNING_WITHOUT_KEYFRAME: {
             WOLF_DEBUG("PT ", getName(), " RUNNING_WITHOUT_KEYFRAME");
 
-            processKnown();
+            processKnownProfiling();
 
             if (voteForKeyFrame() && permittedKeyFrame())
             {
                 // process
-                processNew(max_new_features_);
+                processNewProfiling(max_new_features_);
 
                 // We create a KF
                 // set KF on last
@@ -262,7 +262,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 last_ptr_->link(last_frame_ptr_);
 
                 // Establish factors
-                establishFactors();
+                establishFactorsProfiling();
 
                 // Call the new keyframe callback in order to let the other processors to join
                 getProblem()->keyFrameCallback(last_frame_ptr_, shared_from_this());
@@ -304,7 +304,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             break;
     }
 
-    postProcess();
+    postProcessProfiling();
 }
 
 void ProcessorTracker::computeProcessingStep()
@@ -386,4 +386,20 @@ void ProcessorTracker::printHeader(int           _depth,
                 << "i: Cap" << getIncoming()->id() << std::endl;
 }
 
+void ProcessorTracker::printProfiling(std::ostream& _stream) const
+{
+    ProcessorBase::printProfiling(_stream);
+    _stream << "\n\tpreProcess:";
+    preprocess_profiling_.printProfiling("\t\t", _stream);
+    _stream << "\n\tprocessKnown:";
+    process_known_profiling_.printProfiling("\t\t", _stream);
+    _stream << "\n\tprocessNew:";
+    process_new_profiling_.printProfiling("\t\t", _stream);
+    _stream << "\n\testablishFactors:";
+    establish_factors_profiling_.printProfiling("\t\t", _stream);
+    _stream << "\n\tpostProcess:";
+    postprocess_profiling_.printProfiling("\t\t", _stream);
+    _stream << std::endl;
+}
+
 }  // namespace wolf