Add test Prb::getState()

src/capture/capture_motion.cpp

@@ -60,7 +60,7 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera
     // buffer encloses timestamp, if ts is:
     //   from :  origin.tx + tt not included
     //   to   : capture.ts + tt included
-    if (this->getOriginCapture()->getTimeStamp() +_time_tolerance < _ts and _ts <= this->getBuffer().back().ts_ + _time_tolerance)
+    if (this->getOriginCapture()->getTimeStamp() + _time_tolerance < _ts and _ts <= this->getBuffer().back().ts_ + _time_tolerance)
         return true;
 
     // not found anywhere

test/gtest_problem.cpp

@@ -16,6 +16,7 @@
 #include "dummy/processor_tracker_feature_dummy.h"
 #include "core/solver/solver_manager.h"
 #include "dummy/solver_manager_dummy.h"
+#include "core/yaml/parser_yaml.h"
 
 #include "core/sensor/sensor_diff_drive.h"
 #include "core/processor/processor_diff_drive.h"
@@ -489,10 +490,77 @@ TEST(Problem, check)
     ASSERT_TRUE(problem->check(true, std::cout));
 }
 
+TEST(Problem, getState)
+{
+    std::string wolf_root = _WOLF_ROOT_DIR;
+
+    auto parser = ParserYaml("test/yaml/params_problem_odom_3d.yaml", wolf_root);
+    auto server = ParamsServer(parser.getParams());
+
+    auto P = Problem::autoSetup(server);
+
+    auto S = P->getHardware()->getSensorList().front();
+
+    auto C = std::make_shared<CaptureOdom3d>(0.0, S, 0.1*Vector6d::Ones(), 0.01*Matrix6d::Identity());
+
+    for (TimeStamp t = 0.0; t <= 3.9; t += 0.1)
+    {
+        C->setTimeStamp(t);
+        C->process();
+    }
+    P->print(4,1,1,1);
+
+    // get at t = origin
+    WOLF_DEBUG("P (0) = ", P->getState(0, "P"));        // partial structure
+    WOLF_DEBUG("PO(0) = ", P->getState(0, "PO"));       // all but explicit structure
+    WOLF_DEBUG("x (0) = ", P->getState(TimeStamp(0)));  // own structure
+    ASSERT_EQ(P->getState(0, "P").size(), 1);
+    ASSERT_EQ(P->getState(0, "PO").size(), 2);
+    ASSERT_EQ(P->getState(TimeStamp(0)).size(), 2);
+
+    // get at t = before KF
+    WOLF_DEBUG("P (1) = ", P->getState(1, "P"));
+    WOLF_DEBUG("PO(1) = ", P->getState(1, "PO"));
+    WOLF_DEBUG("x (1) = ", P->getState(1));
+    ASSERT_EQ(P->getState(1, "P").size(), 1);
+    ASSERT_EQ(P->getState(1, "PO").size(), 2);
+    ASSERT_EQ(P->getState(TimeStamp(1)).size(), 2);
+
+    // get at t = KF
+    WOLF_DEBUG("P (2) = ", P->getState(2, "P"));
+    WOLF_DEBUG("PO(2) = ", P->getState(2, "PO"));
+    WOLF_DEBUG("x (2) = ", P->getState(2));
+    ASSERT_EQ(P->getState(2, "P").size(), 1);
+    ASSERT_EQ(P->getState(2, "PO").size(), 2);
+    ASSERT_EQ(P->getState(TimeStamp(2)).size(), 2);
+
+    // get at t = after last KF
+    WOLF_DEBUG("P (3) = ", P->getState(3, "P"));
+    WOLF_DEBUG("PO(3) = ", P->getState(3, "PO"));
+    WOLF_DEBUG("x (3) = ", P->getState(3));
+    ASSERT_EQ(P->getState(3, "P").size(), 1);
+    ASSERT_EQ(P->getState(3, "PO").size(), 2);
+    ASSERT_EQ(P->getState(TimeStamp(3)).size(), 2);
+
+    // get at t = last processed capture
+    WOLF_DEBUG("P (3.9) = ", P->getState(3.9, "P"));
+    WOLF_DEBUG("PO(3.9) = ", P->getState(3.9, "PO"));
+    WOLF_DEBUG("x (3.9) = ", P->getState(3.9));
+    ASSERT_EQ(P->getState(3.9, "P").size(), 1);
+    ASSERT_EQ(P->getState(3.9, "PO").size(), 2);
+    ASSERT_EQ(P->getState(TimeStamp(3.9)).size(), 2);
+
+    // get at t = current state
+    WOLF_DEBUG("P () = ", P->getState("P"));
+    WOLF_DEBUG("PO() = ", P->getState("PO"));
+    WOLF_DEBUG("x () = ", P->getState());
+
+}
+
 
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-//  ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
+  ::testing::GTEST_FLAG(filter) = "Problem.getState";
   return RUN_ALL_TESTS();
 }

test/yaml/params_problem_odom_3d.yaml

+config:
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager: 
+      type: "None"
+  sensors: 
+    -
+      type: "SensorOdom3d"
+      name: "odom"
+      plugin: "core"
+      k_disp_to_disp: 0.1
+      k_disp_to_rot: 0.1
+      k_rot_to_rot: 0.1 
+      min_disp_var: 0.1 
+      min_rot_var: 0.1
+      extrinsic:
+        pose: [1,2,3,0,0,0,1]
+  processors:
+    -
+      type: "ProcessorOdom3d"
+      name: "my_proc_odom3d"
+      sensor_name: "odom"
+      plugin: "core"
+      apply_loss_function: false
+      time_tolerance:         0.01  # seconds
+      keyframe_vote:
+        voting_active:        true
+        voting_aux_active:    false
+        max_time_span:          1.95  # seconds
+        max_buff_length:        999   # motion deltas
+        dist_traveled:          999   # meters
+        angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
+      
+      unmeasured_perturbation_std: 0.00111