From 86510a97dea363cb53e69b60ba4dddd3b33b8965 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 12 Jun 2020 10:53:36 +0200
Subject: [PATCH] Redesign Prb::getTimeStamp()
---
src/problem/problem.cpp | 28 ++++++++++++++--------------
1 file changed, 14 insertions(+), 14 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 2851bface..c793cd3e1 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -419,27 +419,27 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
TimeStamp Problem::getTimeStamp ( ) const
{
- if (not processor_is_motion_list_.empty() ) // Use most recent Motion processor data
- {
- TimeStamp ts(0);
- for (const auto& prc : processor_is_motion_list_)
- if (prc->getTimeStamp() > ts)
+ TimeStamp ts = TimeStamp::Invalid();
+
+ for (const auto& prc : processor_is_motion_list_)
+ if (prc->getTimeStamp().ok())
+ if ( (not ts.ok() ) or prc->getTimeStamp() > ts)
ts = prc->getTimeStamp();
- return ts;
- }
- else
+
+
+ if (not ts.ok())
{
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
+ ts = 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();
- }
}
+
+ if (not ts.ok())
+ WOLF_WARN( "Problem has nowhere to find a valid timestamp!");
+
+ return ts;
}
--
GitLab