Add test getState(t, struct)

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)
 {