Add problem::getTimeStamp()

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;

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
 {