diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp index d717135617eb64a0a07f019b54a26ed97f481152..1809e2c943b80c8669ebf96b8d88e83781889ad8 100644 --- a/src/test/gtest_IMU.cpp +++ b/src/test/gtest_IMU.cpp @@ -95,6 +95,9 @@ class Process_Constraint_IMU : public testing::Test sensor_imu = static_pointer_cast<SensorIMU> (sensor); processor_imu = static_pointer_cast<ProcessorIMU>(processor); + dt = 0.01; + processor_imu->setTimeTolerance(dt/2); + // Some initializations bias_null .setZero(); x0 .resize(10); @@ -374,7 +377,7 @@ class Process_Constraint_IMU : public testing::Test FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t); // ===================================== IMU CALLBACK - processor_imu->keyFrameCallback(KF, 0.01); + processor_imu->keyFrameCallback(KF, dt/2); @@ -490,6 +493,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU // Wolf objects SensorOdom3DPtr sensor_odo; ProcessorOdom3DPtr processor_odo; + CaptureOdom3DPtr capture_odo; virtual void SetUp( ) override { @@ -504,6 +508,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/src/examples/processor_odom_3D.yaml"); sensor_odo = static_pointer_cast<SensorOdom3D>(sensor); processor_odo = static_pointer_cast<ProcessorOdom3D>(processor); + processor_odo->setTimeTolerance(dt/2); // prevent this processor from voting by setting high thresholds : processor_odo->setAngleTurned(2.0); @@ -537,6 +542,11 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU // ===================================== ODO processor_odo->keyFrameCallback(KF_1, 0.1); + + data = Vector6s::Zero(); + capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov()); + processor_odo->process(capture_odo); + } }; @@ -550,7 +560,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -596,7 +605,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -645,7 +653,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -692,7 +699,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -739,7 +745,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -786,7 +791,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -832,7 +836,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -878,7 +881,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -924,7 +926,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -970,7 +971,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -1016,7 +1016,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -1045,9 +1044,14 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe // ===================================== RUN ALL - string report = runAll(1); +// string report = runAll(1); + configureAll(); + integrateAll(); + buildProblem(); + problem->print(4,1,1,1); + string report = solveProblem(1); - // printAll(report); + printAll(report); assertAll(); @@ -1062,7 +1066,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -1108,7 +1111,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -1154,7 +1156,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -1200,7 +1201,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -1246,7 +1246,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose @@ -1292,7 +1291,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim // // ---------- time t0 = 0; - dt = 0.01; num_integrations = 50; // ---------- initial pose