diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 18e2d5273737bf1d9b46715b439699bda74f29c6..3951bf2f092fdcce0c518bcabfb7d9d46e66325f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -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)
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index 2134768da0637461e60ffeac9473e1d43e365752..e468787a9fda0decf01374f44018887fb59c04b9 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.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);
}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 14724df2310a7aaed0bac11ce4334d837276e14b..5d04c78a8591b9cf413d47e9d023dcf3d31844f6 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -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);
}
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 78f05eb997127d4dc723f0db15f2cfd8b3de9852..ed4aeede54f33e274dbd6cc9c52837f22be2869c 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -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)
diff --git a/test/gtest_factor_odom_2d_autodiff.cpp b/test/gtest_factor_odom_2d_autodiff.cpp
index a8eed809e2800acac9d6e66c97f0b1b9e9f9ea1f..09a7ed9a048a2fc0547eefc6988a58db50a610bd 100644
--- a/test/gtest_factor_odom_2d_autodiff.cpp
+++ b/test/gtest_factor_odom_2d_autodiff.cpp
@@ -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);