Resolve "Problem::getState/getTimeStamp considering not initialized processors"

Closes #344 (closed)

src/problem/problem.cpp

@@ -419,15 +419,7 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
 
 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
+    if (not processor_is_motion_list_.empty() ) // Use most recent Motion processor data
     {
         TimeStamp  ts(0);
         for (const auto& prc : processor_is_motion_list_)
@@ -435,6 +427,19 @@ TimeStamp Problem::getTimeStamp ( ) const
                 ts = prc->getTimeStamp();
         return ts;
     }
+    else
+    {
+        const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+
+        if (last_kf_or_aux)
+            return last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
+
+        else
+        {
+            WOLF_WARN( "Problem has no Keyframe so no timestamp can be obtained!");
+            return TimeStamp::Invalid();
+        }
+    }
 }