diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index df300c7267b4e6ff0bbf3f425392487fd5b90841..852a52e45af7d045d8482be09be7c3193809b70b 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -18,6 +18,7 @@ class SensorBase;
// std
#include <memory>
#include <map>
+#include <chrono>
namespace wolf {
@@ -265,6 +266,18 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
unsigned int id() const;
+ // PROFILING
+ unsigned int n_capture_callback_;
+ unsigned int n_kf_callback_;
+ std::chrono::microseconds duration_capture_;
+ std::chrono::microseconds duration_kf_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_;
+ void startCaptureProfiling();
+ void stopCaptureProfiling();
+ void startKFProfiling();
+ void stopKFProfiling();
+
protected:
/** \brief process an incoming capture
*
@@ -384,6 +397,26 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
+inline void ProcessorBase::startCaptureProfiling()
+{
+ start_capture_ = std::chrono::high_resolution_clock::now();
+}
+
+inline void ProcessorBase::stopCaptureProfiling()
+{
+ duration_capture_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_capture_);
+}
+
+inline void ProcessorBase::startKFProfiling()
+{
+ start_kf_ = std::chrono::high_resolution_clock::now();
+}
+
+inline void ProcessorBase::stopKFProfiling()
+{
+ duration_kf_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_kf_);
+}
+
inline bool ProcessorBase::isVotingActive() const
{
return params_->voting_active;
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index a6759817ae22e8b38c5f5dbce2144e2d8aee23c5..c2dce560c5a701f2d5131c34c31fa489da96a4f2 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -70,6 +70,11 @@ class SolverManager
FULL
};
+ // PROFILING
+ unsigned int n_solve_;
+ std::chrono::microseconds duration_manager_;
+ std::chrono::microseconds duration_solver_;
+
protected:
ProblemPtr wolf_problem_;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b151a6929ed93ecb4c231ffcd409df89ee3c36d1..bfcb33fa67482f2907ce1cdc715e4dacc44d60ed 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -605,6 +605,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+ // pause processor profiling
+ if (_processor_ptr)
+ _processor_ptr->stopCaptureProfiling();
+
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor && (processor != _processor_ptr) )
@@ -629,6 +633,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
// notify tree manager
if (tree_manager_)
tree_manager_->keyFrameCallback(_keyframe_ptr);
+
+ // resume processor profiling
+ if (_processor_ptr)
+ _processor_ptr->startCaptureProfiling();
}
bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index caa850ebcd7b3b6d9e443c05266cc1e1d67491c0..a4ff214e113e41c67725bae938988ff3d10d857b 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -12,7 +12,11 @@ ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ParamsProcessor
processor_id_(++processor_id_count_),
params_(_params),
dim_(_dim),
- sensor_ptr_()
+ sensor_ptr_(),
+ n_capture_callback_(0),
+ n_kf_callback_(0),
+ duration_capture_(0),
+ duration_kf_(0)
{
// WOLF_DEBUG("constructed +p" , id());
}
@@ -37,6 +41,9 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _
assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
+ // profiling
+ n_kf_callback_++;
+ startKFProfiling();
// asking if key frame should be stored
if (storeKeyFrame(_keyframe_ptr))
@@ -46,6 +53,8 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _
if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other))
processKeyFrame(_keyframe_ptr, _time_tol_other);
+ // profiling
+ stopKFProfiling();
}
void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
@@ -53,6 +62,10 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ // profiling
+ n_capture_callback_++;
+ startCaptureProfiling();
+
// apply prior in problem if not done (very first capture)
if (getProblem() && !getProblem()->isPriorSet())
getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
@@ -64,6 +77,9 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
// if trigger, process directly without buffering
if (triggerInCapture(_capture_ptr))
processCapture(_capture_ptr);
+
+ // profiling
+ stopCaptureProfiling();
}
void ProcessorBase::remove()
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 4994631c38737949c6aef75b2cada0cd810c0d96..8a9e15d6e81b88905ffdd3fb81b0635013ebf426 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -12,6 +12,9 @@ SolverManager::SolverManager(const ProblemPtr& _problem) :
SolverManager::SolverManager(const ProblemPtr& _problem,
const ParamsSolverPtr& _params) :
+ n_solve_(0),
+ duration_manager_(0),
+ duration_solver_(0),
wolf_problem_(_problem),
params_(_params)
{
@@ -132,11 +135,16 @@ std::string SolverManager::solve()
std::string SolverManager::solve(const ReportVerbosity report_level)
{
+ auto start = std::chrono::high_resolution_clock::now();
+ n_solve_++;
+
// update problem
update();
// call derived solver
+ auto start_derived = std::chrono::high_resolution_clock::now();
std::string report = solveDerived(report_level);
+ duration_solver_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_derived);
// update StateBlocks with optimized state value.
/// @todo whatif someone has changed the state notification during opti ??
@@ -151,6 +159,8 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
}
+ duration_manager_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
+
return report;
}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index a2fefcff6348c3e304c6a8684c607dd04adbf709..dc44348f5943d7702372e28ebfb4e8ea2a707876 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -113,7 +113,7 @@ TEST(Problem, SetOrigin_PO_2d)
VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
P->setPriorFactor(x0, s0, t0, 1.0);
-
+ WOLF_INFO("printing.-..");
P->print(4,1,1,1);
// check that no sensor has been added