diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 455d3b16669268e8aed0973cb58880ff3e969fcd..8f5bb0e509d0d6d563b189857cfcf60f7720b6b9 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -312,6 +312,7 @@ class Problem : public std::enable_shared_from_this<Problem> // State getters VectorComposite getState ( ) const; VectorComposite getState (const TimeStamp& _ts ) const; + TimeStamp getTimeStamp ( ) const; // Zero state provider Eigen::VectorXd zeroState ( ) const; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 995d322384a5db44fa45a2a847d298749e862e95..d437e313bbdd87b2dba64d1fd364932290509b39 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -415,6 +415,25 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // // _state = zeroState(); //} +TimeStamp Problem::getTimeStamp ( ) const +{ + if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state + { + auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); + + assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!"); + + return last_kf_or_aux->getTimeStamp(); + } + else + { + TimeStamp ts(0); + for (const auto& prc : processor_is_motion_list_) + if (prc->getTimeStamp() > ts) + ts = prc->getTimeStamp(); + return ts; + } +} VectorComposite Problem::getState() const {