diff --git a/test/gtest_factor_compass_3d.cpp b/test/gtest_factor_compass_3d.cpp
index de743934b17aab25a5d4a5fc5d952e53a040de75..f63f32b48ef054d9dd8243da38bbaed60755102a 100644
--- a/test/gtest_factor_compass_3d.cpp
+++ b/test/gtest_factor_compass_3d.cpp
@@ -43,11 +43,11 @@ SolverCeres solver(problem);
// Sensor
auto sen = std::static_pointer_cast<SensorCompass>(problem->installSensor("SensorCompass",
- "magnetometer",
- wolf_root + "/test/yaml/sensor_compass_3d.yaml"));
+ wolf_root + "/test/yaml/sensor_compass_3d.yaml",
+ {wolf_root}));
// Frame
-FrameBasePtr frm = problem->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm = problem->emplaceFrame(0, "PO", (Vector7d() << 0,0,0,0,0,0,1).finished() );
// Pointers
CaptureCompassPtr cap;
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index e10a29b62613ed8ad81433e85576041893a44315..2a0b140a6d07ecd4be16383ab9f8dbc5bf08dfcf 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -57,7 +57,7 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -82,8 +82,8 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -139,7 +139,7 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -163,8 +163,8 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -216,7 +216,7 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -240,8 +240,8 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
//solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -293,7 +293,7 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -317,8 +317,8 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -372,7 +372,7 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -396,8 +396,8 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -452,7 +452,7 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -476,8 +476,8 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -528,7 +528,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -552,8 +552,8 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -614,7 +614,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -638,8 +638,8 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -708,7 +708,7 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
ProblemPtr problem;
SolverCeresPtr solver;
ProcessorBasePtr processor_;
@@ -732,8 +732,8 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -801,7 +801,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
ProblemPtr problem;
SolverCeresPtr ceres_manager;
SensorBasePtr sensor;
- SensorImuPtr sensor_imu;
+ SensorImu3dPtr sensor_imu;
SensorOdom3dPtr sensor_odo;
ProcessorBasePtr processor;
ProcessorImuPtr processor_imu;
@@ -825,9 +825,9 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_manager->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
// SENSOR + PROCESSOR Imu
- sensor = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
+ sensor = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
sensor_imu = static_pointer_cast<SensorImu>(sensor);
- processor = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ processor = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
processor_imu = static_pointer_cast<ProcessorImu>(processor);
// SENSOR + PROCESSOR ODOM 3d
@@ -975,7 +975,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
SolverCeresPtr solver;
@@ -1001,8 +1001,8 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -1109,7 +1109,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
{
public:
TimeStamp t;
- SensorImuPtr sen_imu;
+ SensorImu3dPtr sen_imu;
SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
SolverCeresPtr solver;
@@ -1136,8 +1136,8 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
- processor = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ processor = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor);
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 1e40e01894fb977bebd9f782d716b0fcd3303424..be0b47c3ecf4416d2a7a8b8e27bbc8f01492cc1e 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -49,7 +49,8 @@ using std::make_shared;
using std::static_pointer_cast;
using std::string;
-string wolf_root = _WOLF_IMU_ROOT_DIR;
+string wolf_imu_root = _WOLF_IMU_ROOT_DIR;
+string wolf_root = _WOLF_ROOT_DIR;
class Process_Factor_Imu : public testing::Test
{
@@ -57,7 +58,7 @@ class Process_Factor_Imu : public testing::Test
// Wolf objects
ProblemPtr problem;
SolverCeresPtr solver;
- SensorImuPtr sensor_imu;
+ SensorImu3dPtr sensor_imu;
ProcessorImuPtr processor_imu;
CaptureImuPtr capture_imu;
FrameBasePtr KF_0, KF_1; // keyframes
@@ -74,8 +75,9 @@ class Process_Factor_Imu : public testing::Test
Quaterniond q0, q; // initial and current orientations
VectorXd x0; // initial state
Matrix<double,9,9> P0; // initial state covariance
- VectorComposite x0c; // initial state composite
- VectorComposite s0c; // initial sigmas composite
+ SpecComposite problem_prior; // initial states
+ // VectorComposite x0c; // initial state composite
+ // VectorComposite s0c; // initial sigmas composite
// bias
Vector6d bias_real, bias_preint, bias_null; // real, pre-int and zero biases.
@@ -128,9 +130,9 @@ class Process_Factor_Imu : public testing::Test
solver = make_shared<SolverCeres>(problem);
// SENSOR + PROCESSOR Imu
- SensorBasePtr sensor = problem->installSensor ("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml");
- ProcessorBasePtr processor = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
- sensor_imu = static_pointer_cast<SensorImu> (sensor);
+ SensorBasePtr sensor = problem->installSensor ("SensorImu", wolf_imu_root + "/test/yaml/sensor_imu.yaml", {wolf_imu_root, wolf_root});
+ ProcessorBasePtr processor = problem->installProcessor("ProcessorImu", "Main Imu", wolf_imu_root + "/test/yaml/processor_imu.yaml", {wolf_imu_root, wolf_root});
+ sensor_imu = static_pointer_cast<SensorImu3d> (sensor);
processor_imu = static_pointer_cast<ProcessorImu>(processor);
dt = 0.01;
@@ -144,8 +146,11 @@ class Process_Factor_Imu : public testing::Test
x1_optim .resize(10);
x1_optim_imu.resize(10);
- x0c = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
- s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+ // x0c = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+ // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+ problem_prior.emplace('P', SpecState("StatePoint3d", Vector3d::Zero(), "factor", Vector3d::Ones()));
+ problem_prior.emplace('O', SpecState("StateQuaternion", Quaterniond::Identity().coeffs(), "factor", Vector3d::Ones()));
+ problem_prior.emplace('V', SpecState("StateVector3d", Vector3d::Zero(), "factor", Vector3d::Ones()));
}
@@ -346,9 +351,11 @@ class Process_Factor_Imu : public testing::Test
DT = num_integrations * dt;
// wolf objects
- WOLF_INFO("x0c: ", x0c);
- WOLF_INFO("s0c: ", s0c);
- KF_0 = problem->setPriorFactor(x0c, s0c, t0);
+ // WOLF_INFO("x0c: ", x0c);
+ // WOLF_INFO("s0c: ", s0c);
+ // KF_0 = problem->setPriorFactor(x0c, s0c, t0);
+ WOLF_INFO("problem prior: ", problem_prior)
+ KF_0 = problem->setPrior(problem_prior, t0);
processor_imu->setOrigin(KF_0);
WOLF_INFO("prior set");
@@ -504,7 +511,7 @@ class Process_Factor_Imu : public testing::Test
virtual void buildProblem()
{
// ===================================== SET KF in Wolf tree
- FrameBasePtr KF = problem->emplaceFrame(t, x1_exact);
+ FrameBasePtr KF = problem->emplaceFrame(t, "POV", x1_exact);
// ===================================== Imu CALLBACK
problem->keyFrameCallback(KF, nullptr);
@@ -611,7 +618,7 @@ class Process_Factor_Imu_ODO : public Process_Factor_Imu
{
public:
// Wolf objects
- SensorOdomPtr sensor_odo;
+ SensorOdom3dPtr sensor_odo;
ProcessorOdom3dPtr processor_odo;
CaptureOdom3dPtr capture_odo;
@@ -622,10 +629,10 @@ class Process_Factor_Imu_ODO : public Process_Factor_Imu
Process_Factor_Imu::SetUp();
// ===================================== ODO
- SensorBasePtr sensor = problem->installSensor ("SensorOdom", "Odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml" );
- ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom3d", "Odometer", "Odometer" , wolf_root + "/test/yaml/processor_odom_3d.yaml");
+ SensorBasePtr sensor = problem->installSensor ("SensorOdom", wolf_imu_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_imu_root});
+ ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom3d", "Odometer", wolf_imu_root + "/test/yaml/processor_odom_3d.yaml", {wolf_imu_root});
- sensor_odo = static_pointer_cast<SensorOdom>(sensor);
+ sensor_odo = static_pointer_cast<SensorOdom3d>(sensor);
processor_odo = static_pointer_cast<ProcessorOdom3d>(processor);
processor_odo->setTimeTolerance(dt/2);
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
index 14721cbfe75c7f93571f5137105ccfca59c2a2bc..2d467bb694d8d97f90a608eb65c0ae885b9448ec 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -61,8 +61,8 @@ class ProcessorImu2dStaticInitTest : public testing::Test
// Wolf problem
problem_ptr_ = Problem::create("POV", 2);
- sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu2d.yaml");
- processor_motion_ = std::static_pointer_cast<ProcessorMotion>(problem_ptr_->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu2d_static_init.yaml"));
+ sensor_ptr_ = problem_ptr_->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
+ processor_motion_ = std::static_pointer_cast<ProcessorMotion>(problem_ptr_->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d_static_init.yaml", {wolf_root}));
// Time and data variables
dt = 0.1;
@@ -77,11 +77,14 @@ class ProcessorImu2dStaticInitTest : public testing::Test
solver_ = make_shared<SolverCeres>(problem_ptr_);
// Set the origin
- VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
- VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
- //KF0_ = problem_ptr_->setPriorFix(x_origin, t0);
- KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0);
-
+ // VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
+ // VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
+ // //KF0_ = problem_ptr_->setPriorFix(x_origin, t0);
+ // KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0);
+ SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))},
+ {'O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.001))},
+ {'V', SpecState("StateVector2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))}};
+ KF0_ = problem_ptr_->setPrior(problem_prior, t0);
}
void TestStatic(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
@@ -313,7 +316,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
last_frame_ = processor_motion_->getOrigin()->getFrame();
// impose zero odometry
- processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
+ processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateKeys()));
// add zero displacement and rotation capture & feature & factor with all previous frames
assert(sensor_ptr_->getProblem());
diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp
index d0ce78bd6b1d97d0eb83725d5cacfea5ece5ea73..5b76eb43a1ade59161b03ed03f877cb87bc2fc35 100644
--- a/test/gtest_imu_mocap_fusion.cpp
+++ b/test/gtest_imu_mocap_fusion.cpp
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_imu_mocap_fusion.cpp
- *
- * Created on: Feb 25, 2020
- * \author: mfourmy
- */
#include <fstream>
@@ -109,27 +103,31 @@ class ImuMocapFusion_Test : public testing::Test
solver_ = std::make_shared<SolverCeres>(problem_);
solver_->getSolverOptions().max_num_iterations = 500;
// initial guess
- VectorComposite x_origin("POV", {w_p_wb, w_q_b.coeffs(), w_v_wb});
- FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0); // if mocap used
+ // VectorComposite x_origin("POV", {w_p_wb, w_q_b.coeffs(), w_v_wb});
+ // FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0); // if mocap used
+ SpecComposite problem_prior{{'P', SpecState("StatePoint3d", w_p_wb, "initial_guess")},
+ {'O', SpecState("StateQuaternion", w_q_b.coeffs(), "initial_guess")},
+ {'V', SpecState("StateVector3d", w_v_wb, "initial_guess")}};
+ FrameBasePtr KF1 = problem_->setPrior(problem_prior, 0);
// pose sensor and proc (unfixed extrinsics)
- sensor_pose_ = problem_->installSensor("SensorPose",
- "sensor pose",
- wolf_root + "/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml");
+ sensor_pose_ = problem_->installSensor("SensorPose3d",
+ wolf_root + "/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml",
+ {wolf_root});
auto proc_pose = problem_->installProcessor("ProcessorPose",
- "pose",
- "sensor pose",
- wolf_root + "/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml");
+ sensor_pose_,
+ wolf_root + "/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml",
+ {wolf_root});
Matrix6d cov_pose = sensor_pose_->computeNoiseCov(VectorXd(0));
// IMU sensor (unfixed intrinsics)
- sensor_imu_ = problem_->installSensor("SensorImu",
- "Main Imu",
- wolf_root + "/test/yaml/imu_mocap_fusion/sensor_imu.yaml");
+ sensor_imu_ = problem_->installSensor("SensorImu3d",
+ wolf_root + "/test/yaml/imu_mocap_fusion/sensor_imu.yaml",
+ {wolf_root});
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(problem_->installProcessor("ProcessorImu",
- "Imu pre-integrator",
- "Main Imu",
- wolf_root + "/test/yaml/imu_mocap_fusion/processor_imu.yaml"));
+ sensor_imu_,
+ wolf_root + "/test/yaml/imu_mocap_fusion/processor_imu.yaml",
+ {wolf_root}));
proc_imu->setOrigin(KF1);
Matrix6d cov_imu = sensor_imu_->computeNoiseCov(VectorXd(0));
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index 546cc92c8f4c40fd23b81312d55fd4e8ba74211b..a35ee73dcc0c679b19856aa85ff4e65cab659bb8 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -77,8 +77,8 @@ class ProcessorImuStaticInitTest : public testing::Test
// Wolf problem
problem_ptr_ = Problem::create("POV", 3);
- sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu_static_init.yaml");
+ sensor_ptr_ = problem_ptr_->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu_static_init.yaml", {wolf_root});
processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index b4d5363d94ee39a0b824dbab80dcf736d01020ab..b80b1dbf6275dc0e4af83710f1a321987bb21c74 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -53,7 +53,7 @@ class ProcessorImuTest : public testing::Test
{
public:
ParamsSensorImuPtr intr_;
- SensorImuPtr sensor_;
+ SensorImu3dPtr sensor_;
ParamsProcessorImuPtr params_;
ProcessorImuPtr processor_;
ProblemPtr problem_;
@@ -66,10 +66,6 @@ class ProcessorImuTest : public testing::Test
problem_ = Problem::create("POV", 3);
solver_ = std::make_shared<SolverCeres>(problem_);
- Vector4d q_init; q_init << 0,0,0,1;
- VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()});
- VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
-
// ground truth of wheels radius
radius = 1;
@@ -81,10 +77,10 @@ class ProcessorImuTest : public testing::Test
Vector6d bias; bias << 0.42,0,0, 0,0,0;
// Vector6d bias; bias << 0.0,0,0, 0.42,0,0;
- SpecSensorComposite priors{{'P',SpecStateSensor("P",Vector3d::Zero())},
- {'O',SpecStateSensor("O",(Vector4d() << 0,0,0,1).finished())},
- {'I',SpecStateSensor("StateBlock",bias,"factor",Vector6d::Constant(0.001),true,Vector6d::Constant(0.001))}};
- sensor_ = std::static_pointer_cast<SensorImu>(problem_->installSensor("SensorImu", "sensor imu", intr_, priors));
+ SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())},
+ {'O',SpecStateSensor("StateQuaternion",(Vector4d() << 0,0,0,1).finished())},
+ {'I',SpecStateSensor("StateParams6",bias,"factor",Vector6d::Constant(0.001),true,Vector6d::Constant(0.001))}};
+ sensor_ = SensorBase::emplace<SensorImu3d>(problem_->getHardware(), intr_, priors);
// params and processor
params_ = std::make_shared<ParamsProcessorImu>();
@@ -96,9 +92,16 @@ class ProcessorImuTest : public testing::Test
params_->unmeasured_perturbation_std = 1e-4;
params_->time_tolerance = 0.0025;
- processor_ = std::static_pointer_cast<ProcessorImu>(problem_->installProcessor("ProcessorImu", "processor imu", sensor_, params_));
-
- KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0);
+ processor_ = ProcessorBase::emplace<ProcessorImu>(sensor_, params_);
+
+ // Vector4d q_init; q_init << 0,0,0,1;
+ // VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()});
+ // VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
+ // KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0);
+ SpecComposite problem_prior{{'P', SpecState("StatePoint3d", Vector3d::Zero(), "factor", Vector3d::Constant(0.001))},
+ {'O', SpecState("StateQuaternion", Quaterniond::Identity().coeffs(), "factor", Vector3d::Constant(0.001))},
+ {'V', SpecState("StateVector3d", Vector3d::Zero(), "factor", Vector3d::Constant(0.001))}};
+ KF0_ = problem_->setPrior(problem_prior, 0);
processor_->setOrigin(KF0_);
}