From a43e7415166b0d68299caf700b33f48646a2fd42 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 13 Jun 2020 19:26:48 +0200
Subject: [PATCH] Fix PM::getState(structure)
---
src/processor/processor_motion.cpp | 26 ++++++++++++++++++--------
test/gtest_processor_motion.cpp | 30 ++++++++++++++++++++++++++++++
2 files changed, 48 insertions(+), 8 deletions(-)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index ed53c1ade..0a7722451 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -491,11 +491,16 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
else
{
- // remove states not requested
- for (const auto& pair_key_vec : state)
+ // remove states not requested by structure
+ auto pair_key_vec_it = state.begin();
+ while (pair_key_vec_it != state.end())
{
- if (_structure.find(pair_key_vec.first) == std::string::npos)
- state.erase(pair_key_vec.first);
+ if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+ pair_key_vec_it = state.erase(pair_key_vec_it);
+
+ else
+ pair_key_vec_it ++;
+
}
return state;
}
@@ -581,11 +586,16 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
else
{
- // remove states not requested
- for (const auto& pair_key_vec : state)
+ // remove states not requested by structure
+ auto pair_key_vec_it = state.begin();
+ while (pair_key_vec_it != state.end())
{
- if (_structure.find(pair_key_vec.first) == std::string::npos)
- state.erase(pair_key_vec.first);
+ if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+ pair_key_vec_it = state.erase(pair_key_vec_it);
+
+ else
+ pair_key_vec_it ++;
+
}
return state;
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index bcf69745c..2bd4a5b85 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -101,6 +101,36 @@ TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
+TEST_F(ProcessorMotion_test, getState_structure)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_TRUE (processor->getState("P").count("P"));
+ ASSERT_FALSE(processor->getState("P").count("O"));
+ ASSERT_FALSE(processor->getState("O").count("P"));
+ ASSERT_TRUE (processor->getState("O").count("O"));
+
+ WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState("V"));
+ ASSERT_EQ (processor->getState("V").size(), 0);
+}
+
+
TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
--
GitLab