Fixed gtest_problem

test/gtest_problem.cpp

@@ -10,6 +10,7 @@
 
 #include "core/problem/problem.h"
 #include "core/sensor/sensor_base.h"
+#include "core/sensor/sensor_odom_2D.h"
 #include "core/sensor/sensor_odom_3D.h"
 #include "core/processor/processor_odom_3D.h"
 #include "core/processor/processor_tracker_feature_dummy.h"
@@ -203,16 +204,16 @@ TEST(Problem, StateBlocks)
     std::string wolf_root = _WOLF_ROOT_DIR;
 
     ProblemPtr P = Problem::create("PO 3D");
-    Eigen::Vector7s xs;
+    Eigen::Vector7s xs3d;
+    Eigen::Vector3s xs2d;
 
     // 2 state blocks, fixed
-    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
+    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/src/examples/sensor_odom_3D.yaml");
     ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
 
-    //TODO: TO BE FIXED: Since the splitting the core has no camera sensor, so this test fails.
-    // 3 state blocks, fixed
-    SensorBasePtr    St = P->installSensor   ("CAMERA", "camera",   xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
-    ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (/*2 + */3)); // consume empties the notification map, so only should contain notification since last call
+    // 2 state blocks, fixed
+    SensorBasePtr    St = P->installSensor   ("ODOM 2D", "other odometer", xs2d, "");
+    ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (2/* + 3*/)); // consume empties the notification map, so only should contain notification since last call
 
     ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
     params->time_tolerance            = 0.1;
@@ -224,7 +225,7 @@ TEST(Problem, StateBlocks)
     ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
 
     // 2 state blocks, estimated
-    P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
+    P->emplaceFrame("PO 3D", KEY_FRAME, xs3d, 0);
     ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
 
     //    P->print(4,1,1,1);
@@ -232,6 +233,8 @@ TEST(Problem, StateBlocks)
     // change some SB properties
     St->unfixExtrinsics();
     ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
+    // ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 2 + 2));
+    // ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 2 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
 
     //    P->print(4,1,1,1);
 }
@@ -241,10 +244,11 @@ TEST(Problem, Covariances)
     std::string wolf_root = _WOLF_ROOT_DIR;
 
     ProblemPtr P = Problem::create("PO 3D");
-    Eigen::Vector7s xs;
+    Eigen::Vector7s xs3d;
+    Eigen::Vector3s xs2d;
 
-    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
-    SensorBasePtr    St = P->installSensor   ("CAMERA", "camera",   xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
+    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/src/examples/sensor_odom_3D.yaml");
+    SensorBasePtr    St = P->installSensor   ("ODOM 2D", "other odometer", xs2d, "");
 
     ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
     params->time_tolerance            = 0.1;
@@ -257,7 +261,7 @@ TEST(Problem, Covariances)
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();
-    FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
+    FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs3d, 0);
 
     // set covariance (they are not computed without a solver)
     P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());