From 44d0919dce419354e4ba98a5d4b0923bc60e5e65 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:50:31 +0200
Subject: [PATCH] Add test getState(t, struct)
---
test/gtest_processor_motion.cpp | 30 ++++++++++++++++++++++++++++++
1 file changed, 30 insertions(+)
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 2bd4a5b85..410ebdf1f 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -131,6 +131,36 @@ TEST_F(ProcessorMotion_test, getState_structure)
}
+TEST_F(ProcessorMotion_test, getState_time_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(7, "P").count("P"));
+ ASSERT_FALSE(processor->getState(7, "P").count("O"));
+ ASSERT_FALSE(processor->getState(7, "O").count("P"));
+ ASSERT_TRUE (processor->getState(7, "O").count("O"));
+
+ WOLF_DEBUG("processor->getState(7, \"V\") = ", processor->getState(7, "V"));
+ ASSERT_EQ (processor->getState(7, "V").size(), 0);
+}
+
+
TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
--
GitLab