diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index b175cb9d07eb344b347e9fbcb266d04371674140..567fc3b8ca6b2c8659a7a48d99cd732e49da8615 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -10,6 +10,7 @@ #include "base/problem.h" #include "base/sensor/sensor_base.h" +#include "base/sensor/sensor_odom_2D.h" #include "base/sensor/sensor_odom_3D.h" #include "base/processor/processor_odom_3D.h" #include "base/processor/processor_tracker_feature_dummy.h" @@ -203,18 +204,18 @@ 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->getStateBlockPtrList().size(), (unsigned int) 2); ASSERT_EQ(P->getStateBlockNotificationMap().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->getStateBlockPtrList().size(), (unsigned int) (2 + 3)); - ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3)); + // 2 state blocks, fixed + SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, ""); + ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 2)); + ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 2)); ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); params->time_tolerance = 0.1; @@ -226,16 +227,16 @@ 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); - ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2)); - ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2)); + P->emplaceFrame("PO 3D", KEY_FRAME, xs3d, 0); + ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 2 + 2)); + ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 2 + 2)); // P->print(4,1,1,1); // change some SB properties St->unfixExtrinsics(); - ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2)); - ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB! + 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); } @@ -245,10 +246,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; @@ -261,7 +263,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); Eigen::MatrixXs Cov = P->getFrameCovariance(F);