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

Fix gtest IMU: call to proc_odom->process() after callback

parent 9f1a5927
No related branches found
No related tags found
1 merge request!157Kfpackmanager
This commit is part of merge request !157. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
Showing
with 18 additions and 20 deletions
src/test/gtest_IMU.cpp
+ 18
− 20
View file @ 960ecfa0
......@@ -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
......
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