diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 2bd4a5b8569df7c850ca516484c46dfcba732321..410ebdf1faea8175c8e73435670fdb46de85fd9e 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) {