Skip to content
Snippets Groups Projects
Commit b02bace5 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix gtest_problem: substitute camera by odom2D

parent be4e8485
No related branches found
No related tags found
No related merge requests found
Pipeline #2813 passed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    Showing
    with 18 additions and 16 deletions
    test/gtest_problem.cpp
    + 18
    − 16
    View file @ b02bace5
    ...@@ -10,6 +10,7 @@ ...@@ -10,6 +10,7 @@
    #include "base/problem.h" #include "base/problem.h"
    #include "base/sensor/sensor_base.h" #include "base/sensor/sensor_base.h"
    #include "base/sensor/sensor_odom_2D.h"
    #include "base/sensor/sensor_odom_3D.h" #include "base/sensor/sensor_odom_3D.h"
    #include "base/processor/processor_odom_3D.h" #include "base/processor/processor_odom_3D.h"
    #include "base/processor/processor_tracker_feature_dummy.h" #include "base/processor/processor_tracker_feature_dummy.h"
    ...@@ -203,18 +204,18 @@ TEST(Problem, StateBlocks) ...@@ -203,18 +204,18 @@ TEST(Problem, StateBlocks)
    std::string wolf_root = _WOLF_ROOT_DIR; std::string wolf_root = _WOLF_ROOT_DIR;
    ProblemPtr P = Problem::create("PO 3D"); ProblemPtr P = Problem::create("PO 3D");
    Eigen::Vector7s xs; Eigen::Vector7s xs3d;
    Eigen::Vector3s xs2d;
    // 2 state blocks, fixed // 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->getStateBlockPtrList().size(), (unsigned int) 2);
    ASSERT_EQ(P->getStateBlockNotificationMap().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. // 2 state blocks, fixed
    // 3 state blocks, fixed SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
    SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml"); ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 2));
    ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3)); ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 2));
    ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
    ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
    params->time_tolerance = 0.1; params->time_tolerance = 0.1;
    ...@@ -226,16 +227,16 @@ TEST(Problem, StateBlocks) ...@@ -226,16 +227,16 @@ TEST(Problem, StateBlocks)
    ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
    // 2 state blocks, estimated // 2 state blocks, estimated
    P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0); P->emplaceFrame("PO 3D", KEY_FRAME, xs3d, 0);
    ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2)); ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 2 + 2));
    ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2)); ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 2 + 2));
    // P->print(4,1,1,1); // P->print(4,1,1,1);
    // change some SB properties // change some SB properties
    St->unfixExtrinsics(); St->unfixExtrinsics();
    ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2)); ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 2 + 2));
    ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB! 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); // P->print(4,1,1,1);
    } }
    ...@@ -245,10 +246,11 @@ TEST(Problem, Covariances) ...@@ -245,10 +246,11 @@ TEST(Problem, Covariances)
    std::string wolf_root = _WOLF_ROOT_DIR; std::string wolf_root = _WOLF_ROOT_DIR;
    ProblemPtr P = Problem::create("PO 3D"); 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 Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, 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 St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
    ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
    params->time_tolerance = 0.1; params->time_tolerance = 0.1;
    ...@@ -261,7 +263,7 @@ TEST(Problem, Covariances) ...@@ -261,7 +263,7 @@ TEST(Problem, Covariances)
    // 4 state blocks, estimated // 4 state blocks, estimated
    St->unfixExtrinsics(); 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); Eigen::MatrixXs Cov = P->getFrameCovariance(F);
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment