From 80ec79d8aaf3d05019b1507fd3dc14161fbae2fb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 12 Jun 2020 16:57:51 +0200
Subject: [PATCH] Consider prior state if nothing else is found in getState()
---
src/problem/problem.cpp | 18 ++++++++++++++++--
1 file changed, 16 insertions(+), 2 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index ee3884ba3..5a38d11f4 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -461,9 +461,16 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
}
// check for empty blocks and fill them with the last KF, or with zeros in the worst case
+
VectorComposite state_last;
+
const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
- if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);
+
+ if (last_kf_or_aux)
+ state_last = last_kf_or_aux->getState(structure);
+
+ else if (prior_options_ and prior_options_->mode != "nothing")
+ state_last = prior_options_->state;
for (const auto& ckey : structure)
{
@@ -504,9 +511,16 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
}
// check for empty blocks and fill them with the closest KF to ts, or with zeros in the worst case
+
VectorComposite state_last;
+
const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
- if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);
+
+ if (last_kf_or_aux)
+ state_last = last_kf_or_aux->getState(structure);
+
+ else if (prior_options_ and prior_options_->mode != "nothing")
+ state_last = prior_options_->state;
for (const auto& ckey : structure)
{
--
GitLab