Fix bugs after conflicting merge

test/gtest_processor_motion.cpp

@@ -63,7 +63,7 @@ class ProcessorMotion_test : public testing::Test{
             problem = Problem::create("PO", 2);
             sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"));
             ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
-            params->time_tolerance  = 0.25;
+            params->time_tolerance  = 0.5;
             params->dist_traveled   = 100;
             params->angle_turned    = 6.28;
             params->max_time_span   = 2.5*dt; // force KF at every third process()
@@ -146,10 +146,13 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
         capture->setTimeStamp(t);
         capture->setData(data);
         capture->setDataCovariance(data_cov);
+//        capture->process();
         processor->captureCallback(capture);
         WOLF_DEBUG("t: ", t, "  x: ", problem->getState().vector("PO").transpose());
     }
 
+    problem->print(2,1,1,1);
+
     ASSERT_TRUE (processor->getState(7, "P").count('P'));
     ASSERT_FALSE(processor->getState(7, "P").count('O'));
     ASSERT_FALSE(processor->getState(7, "O").count('P'));

test/gtest_trajectory.cpp

@@ -36,11 +36,11 @@ TEST(TrajectoryBase, ClosestKeyFrame)
     FrameBasePtr F2 = P->emplaceKeyFrame(          2, Eigen::Vector3d::Zero() );
     // FrameBasePtr F3 = P->emplaceFrame(AUXILIARY,     3, Eigen::Vector3d::Zero() );
     FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(),
-                                                              P->getDim(),
-                                                              Eigen::Vector3d::Zero() );
+//                                                              P->getDim(),
+                                                              std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
     FrameBasePtr F4 = FrameBase::createNonKeyFrame<FrameBase>(4, P->getFrameStructure(),
-                                                              P->getDim(),
-                                                              Eigen::Vector3d::Zero() );
+//                                                              P->getDim(),
+                                                              std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
 
     FrameBasePtr KF; // closest key-frame queried
 
@@ -95,8 +95,8 @@ TEST(TrajectoryBase, Add_Remove_Frame)
 
     // add F3
     FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(),
-                                                              P->getDim(),
-                                                              Eigen::Vector3d::Zero());
+//                                                              P->getDim(),
+                                                              std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 2);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);