From b8f54c3e0ff9a50ddad97459e606ec7a3227128b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Fri, 12 Jun 2020 09:56:08 +0200 Subject: [PATCH] Change order in Prb::getTimeStamp() --- src/problem/problem.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 4d2462f72..74ea27dc4 100644 --- a/src/problem/problem.cpp +++ b/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(); + } + } } -- GitLab