Skip to content
Snippets Groups Projects
Commit ccf6b198 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

[skip ci] fixing more gtests

parent a78a9178
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #12216 skipped
......@@ -138,26 +138,26 @@ wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
# ------- Now Derived classes ----------
# # FactorAbs(P/O/V) classes test
# wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
# FactorAbs(P/O/V) classes test
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
# # FactorAutodiffDistance3d test
# wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
# FactorAutodiffDistance3d test
wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
# # FactorBlockDifference class test
# wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
# FactorBlockDifference class test
wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
# # FactorDiffDrive class test
# wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
# FactorDiffDrive class test
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
# # FactorOdom2dAutodiff class test
# wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
# FactorOdom2dAutodiff class test
wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
# # FactorPose2d class test
# wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
# FactorPose2d class test
wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
# # FactorPose3d class test
# wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
# FactorPose3d class test
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
# # FactorRelativePose2d class test
# wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
......
......@@ -76,9 +76,8 @@ class FactorAutodiffDistance3d_Test : public testing::Test
problem = Problem::create("PO", 3);
solver = std::make_shared<SolverCeres>(problem);
F1 = problem->emplaceFrame (1.0, pose1);
F2 = problem->emplaceFrame (2.0, pose2);
F1 = problem->emplaceFrame(1.0, "PO", pose1);
F2 = problem->emplaceFrame(2.0, "PO", pose2);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
}
......
......@@ -67,17 +67,14 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t0(0);
TimeStamp t1(1);
// Vector10d x_origin = problem_->stateZero().vector("POV");
VectorComposite x_origin(problem_->stateZero().vector("POV"), "POV", {3, 4, 3});
// Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
VectorComposite cov_prior("POV", {Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))});
KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0);
VectorComposite state_zero = problem_->stateZero();
SpecComposite priors;
priors.emplace('P', SpecState("StatePoint3d", state_zero.vector("P"), "fix"));
priors.emplace('O', SpecState("StateQuaternion", state_zero.vector("O"), "fix"));
priors.emplace('V', SpecState("StateVector3d", state_zero.vector("V"), "fix"));
KF0_ = problem_->setPrior(priors, t0);
//CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
//FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
//FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
KF1_ = problem_->emplaceFrame(t1, problem_->stateZero());
KF1_ = problem_->emplaceFrame(t1, state_zero);
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
}
......
......@@ -163,15 +163,16 @@ class FactorDiffDriveTest : public testing::Test
// make a sensor first
params_sen = make_shared<ParamsSensorDiffDrive>();
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
params_sen->name = "cool sensor";
SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
{'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
{'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
SpecSensorComposite priors;
priors.emplace('P',SpecStateSensor("StatePoint2d", Vector2d::Zero())); //default "fix", not dynamic
priors.emplace('O',SpecStateSensor("StateAngle", Vector1d::Zero())); //default "fix", not dynamic
priors.emplace('I',SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")); // not dynamic
sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
params_sen,
priors));
sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
params_sen,
priors);
calib = sensor->getIntrinsic()->getState();
// params and processor
......@@ -179,20 +180,13 @@ class FactorDiffDriveTest : public testing::Test
params_proc->angle_turned = 1;
params_proc->dist_traveled = 2;
params_proc->max_buff_length = 3;
processor = static_pointer_cast<ProcessorDiffDrivePublic>(problem->installProcessor("ProcessorDiffDrive",
"diff drive",
sensor,
params_proc));
params_proc->name = "diff drive processor";
processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor,
params_proc);
// frames
F0 = FrameBase::emplace<FrameBase>(trajectory,
0.0,
"PO",
list<VectorXd>{Vector2d(0,0), Vector1d(0)});
F1 = FrameBase::emplace<FrameBase>(trajectory,
1.0,
"PO",
list<VectorXd>{Vector2d(1,0), Vector1d(0)});
F0 = problem->emplaceFrame(0.0, "PO", Vector3d::Zero());
F1 = problem->emplaceFrame(1.0, "PO", Vector3d(1,0,0));
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
......@@ -465,15 +459,17 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
// make a sensor first
ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
params_sen->name = "cool sensor";
SpecSensorComposite prior_sen;
prior_sen.emplace('P',SpecStateSensor("StatePoint2d", Vector2d::Zero())); //default "fix", not dynamic
prior_sen.emplace('O',SpecStateSensor("StateAngle", Vector1d::Zero())); //default "fix", not dynamic
prior_sen.emplace('I',SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")); // not dynamic
auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
params_sen,
prior_sen);
SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
{'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
{'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
auto sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
params_sen,
priors));
auto calib_preint = sensor->getIntrinsic()->getState();
Vector3d calib_gt(1,1,1);
......@@ -484,19 +480,21 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
params_proc->max_buff_length = 99;
params_proc->voting_active = true;
params_proc->max_time_span = 1.5;
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params_proc);
auto processor = static_pointer_cast<ProcessorDiffDrivePublic>(prc);
params_proc->name = "diff drive processor";
auto processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor,
params_proc);
TimeStamp t(0.0);
double dt = 1.0;
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
// set prior at t=0 and processor origin
auto F0 = problem->setPriorFactor(x0, s0, t);
SpecComposite problem_prior;
problem_prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.1)));
problem_prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.1)));
auto F0 = problem->setPrior(problem_prior, t);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
......@@ -600,15 +598,16 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
// make a sensor first
ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
params_sen->name = "cool sensor";
SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
{'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
{'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
auto sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
params_sen,
priors));
SpecSensorComposite prior_sen;
prior_sen.emplace('P',SpecStateSensor("StatePoint2d", Vector2d::Zero())); //default "fix", not dynamic
prior_sen.emplace('O',SpecStateSensor("StateAngle", Vector1d::Zero())); //default "fix", not dynamic
prior_sen.emplace('I',SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")); // not dynamic
auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
params_sen,
prior_sen);
auto calib_preint = sensor->getIntrinsic()->getState();
Vector3d calib_gt(1,1,1);
......@@ -619,20 +618,21 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
params->max_buff_length = 99;
params->voting_active = true;
params->max_time_span = 1.5;
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = static_pointer_cast<ProcessorDiffDrivePublic>(prc);
params->name = "diff drive processor";
auto processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor,
params);
TimeStamp t(0.0);
double dt = 1.0;
// Vector3d x0(0,0,0);
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
// set prior at t=0 and processor origin
auto F0 = problem->setPriorFactor(x0, s0, t);
SpecComposite problem_prior;
problem_prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1)));
problem_prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1)));
auto F0 = problem->setPrior(problem_prior, t);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
......
......@@ -40,8 +40,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector3d::Zero());
FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector3d::Zero());
FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "PO", Vector3d::Zero());
FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "PO", Vector3d::Zero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment