From 6701ddc49e5604896e31ba551ac36f8889b6bab4 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 7 Jul 2022 13:52:12 +0200
Subject: [PATCH] [skip ci] still fixing gtests

---
 schema/processor/ProcessorTracker.schema      |  12 +++
 .../processor/ProcessorTrackerFeature.schema  |   1 +
 .../processor/ProcessorTrackerLandmark.schema |   1 +
 schema/sensor/SensorDiffDrive.schema          |   4 +-
 src/problem/problem.cpp                       |  22 ++--
 src/state_block/state_block.cpp               |   2 +-
 src/state_block/state_block_derived.cpp       |  14 +--
 test/CMakeLists.txt                           |   4 +-
 .../dummy/ProcessorTrackerFeatureDummy.schema |   6 ++
 test/dummy/SensorDummyPoia3d.schema           |   4 +-
 test/dummy/TreeManagerDummy.schema            |   6 ++
 test/gtest_factory_state_block.cpp            |  10 +-
 test/gtest_prior.cpp                          |  54 +++++-----
 test/gtest_problem.cpp                        | 102 +++++++-----------
 test/gtest_processor_base.cpp                 |   8 +-
 test/gtest_processor_motion.cpp               |   2 +-
 test/gtest_sensor_base.cpp                    |   2 +-
 test/gtest_tree_manager.cpp                   |  18 ++--
 test/yaml/params3.yaml                        |   3 +-
 test/yaml/params_prior.yaml                   |  18 ++--
 ...ree_manager_sliding_window_dual_rate1.yaml |   3 +-
 ...ree_manager_sliding_window_dual_rate3.yaml |   3 +-
 ...ger_sliding_window_dual_rate_baseline.yaml |   3 +-
 .../yaml/processor_tracker_feature_dummy.yaml |   4 +
 test/yaml/sensor_odom_3d.yaml                 |   1 +
 test/yaml/sensor_odom_3d_other.yaml           |   2 +
 26 files changed, 168 insertions(+), 141 deletions(-)
 create mode 100644 schema/processor/ProcessorTracker.schema
 create mode 100644 schema/processor/ProcessorTrackerFeature.schema
 create mode 100644 schema/processor/ProcessorTrackerLandmark.schema
 create mode 100644 test/dummy/ProcessorTrackerFeatureDummy.schema
 create mode 100644 test/dummy/TreeManagerDummy.schema
 create mode 100644 test/yaml/processor_tracker_feature_dummy.yaml
 create mode 100644 test/yaml/sensor_odom_3d_other.yaml

diff --git a/schema/processor/ProcessorTracker.schema b/schema/processor/ProcessorTracker.schema
new file mode 100644
index 000000000..73323ef25
--- /dev/null
+++ b/schema/processor/ProcessorTracker.schema
@@ -0,0 +1,12 @@
+follow: ProcessorBase.schema
+min_features_for_keyframe:
+  mandatory: true
+  type: unsigned int
+  yaml_type: scalar
+  doc: Minimum number of features to vote for keyframe.
+
+max_new_features:
+  mandatory: true
+  type: int
+  yaml_type: scalar
+  doc: Maximum number of new features to be processed when adding a keyframe (-1=unlimited. 0=none.)
diff --git a/schema/processor/ProcessorTrackerFeature.schema b/schema/processor/ProcessorTrackerFeature.schema
new file mode 100644
index 000000000..4a3bf2eb5
--- /dev/null
+++ b/schema/processor/ProcessorTrackerFeature.schema
@@ -0,0 +1 @@
+follow: ProcessorTracker.schema
\ No newline at end of file
diff --git a/schema/processor/ProcessorTrackerLandmark.schema b/schema/processor/ProcessorTrackerLandmark.schema
new file mode 100644
index 000000000..4a3bf2eb5
--- /dev/null
+++ b/schema/processor/ProcessorTrackerLandmark.schema
@@ -0,0 +1 @@
+follow: ProcessorTracker.schema
\ No newline at end of file
diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema
index e42352f97..d685842ed 100644
--- a/schema/sensor/SensorDiffDrive.schema
+++ b/schema/sensor/SensorDiffDrive.schema
@@ -21,8 +21,8 @@ states:
       type: string
       yaml_type: scalar
       mandatory: false
-      default: StateParam3
-      options: [StateParam3]
+      default: StateParams3
+      options: [StateParams3]
       doc: The type of the SensorDiffDrive intrinsic parameters is StateParam3.
     state:
       type: Vector3d
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6f1ace470..ee4631c8d 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -293,14 +293,22 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type,
     
     ProcessorBasePtr prc_ptr = FactoryProcessorYaml::create(_prc_type, _params_yaml_filename, _folders_schema);
 
-    //Dimension check
-    int prc_dim = prc_ptr->getDim();
-    auto prb = this;
-    assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
-
-    prc_ptr->configure(sen_ptr);
-    prc_ptr->link(sen_ptr);
+    if (not prc_ptr)
+    {
+        throw std::runtime_error("Processor could not be created.");
+    }
+    else
+    {
+        //Dimension check
+        if (prc_ptr->getDim() != 0 and prc_ptr->getDim() != this->getDim())
+        {
+            throw std::runtime_error("Processor not compatible with the Problem dimension.");
+        }
 
+        // add processor
+        prc_ptr->configure(sen_ptr);
+        prc_ptr->link(sen_ptr);
+    }
     return prc_ptr;
 }
 
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index a1d831c46..c061ea96b 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -86,7 +86,7 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
     if (_state.size() == 4)
         return StateQuaternion::create(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
+    throw std::runtime_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
 
     return nullptr;
 }
diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp
index c5bc71bbc..e1eca5d66 100644
--- a/src/state_block/state_block_derived.cpp
+++ b/src/state_block/state_block_derived.cpp
@@ -29,28 +29,28 @@ StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed)
 {
     if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for Point2d.");
+    throw std::runtime_error("Wrong vector size for Point2d.");
 }
 
 StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed)
 {
     if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for Point3d.");
+    throw std::runtime_error("Wrong vector size for Point3d.");
 }
 
 StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed)
 {
     if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for Vector2d.");
+    throw std::runtime_error("Wrong vector size for Vector2d.");
 }
 
 StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed)
 {
     if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for Vector3d.");
+    throw std::runtime_error("Wrong vector size for Vector3d.");
 }
 
 StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed)
@@ -60,7 +60,7 @@ StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed)
     else if (_state.size() == 3)
         return std::make_shared<StatePoint3d>(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for Point.");
+    throw std::runtime_error("Wrong vector size for Point.");
 }
 
 StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed)
@@ -70,7 +70,7 @@ StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed)
     else if (_state.size() == 3)
         return std::make_shared<StateVector3d>(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for Vector.");
+    throw std::runtime_error("Wrong vector size for Vector.");
 }
 
 template <size_t size>
@@ -78,7 +78,7 @@ StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fix
 {
     if (_state.size() == size) return std::make_shared<StateParams<size>>(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for Params.");
+    throw std::runtime_error("Wrong vector size for Params.");
 }
 
 namespace
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 34537dbc5..ad07f3044 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -92,8 +92,8 @@ wolf_add_gtest(gtest_prior gtest_prior.cpp)
 # wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
 
 # Problem class test
-# wolf_add_gtest(gtest_problem gtest_problem.cpp)
-# target_link_libraries(gtest_problem PUBLIC dummy)
+wolf_add_gtest(gtest_problem gtest_problem.cpp)
+target_link_libraries(gtest_problem PUBLIC dummy)
 
 # ProcessorBase class test
 wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
diff --git a/test/dummy/ProcessorTrackerFeatureDummy.schema b/test/dummy/ProcessorTrackerFeatureDummy.schema
new file mode 100644
index 000000000..f5dcdeed9
--- /dev/null
+++ b/test/dummy/ProcessorTrackerFeatureDummy.schema
@@ -0,0 +1,6 @@
+follow: ProcessorTrackerFeature.schema
+n_tracks_lost:
+  mandatory: true
+  type: unsigned int
+  yaml_type: scalar
+  doc: dummy parameter.
\ No newline at end of file
diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema
index 3c54a371c..180e9e842 100644
--- a/test/dummy/SensorDummyPoia3d.schema
+++ b/test/dummy/SensorDummyPoia3d.schema
@@ -10,8 +10,8 @@ states:
       type: string
       yaml_type: scalar
       mandatory: false
-      default: StateParam5
-      options: [StateParam5]
+      default: StateParams5
+      options: [StateParams5]
       doc: The derived type of the state in 'I'
     state:
       type: Vector5d
diff --git a/test/dummy/TreeManagerDummy.schema b/test/dummy/TreeManagerDummy.schema
new file mode 100644
index 000000000..32f690f19
--- /dev/null
+++ b/test/dummy/TreeManagerDummy.schema
@@ -0,0 +1,6 @@
+follow: TreeManagerBase.schema
+toy_param:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: a toy parameter.
\ No newline at end of file
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 7468d09e3..ea1f4be3c 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -122,7 +122,7 @@ TEST(FactoryStateBlock, creator_Point)
     ASSERT_FALSE(sba->hasLocalParametrization());
 
     // fails
-    ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+    ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error);
 }
 
 TEST(FactoryStateBlock, creator_P)
@@ -140,7 +140,7 @@ TEST(FactoryStateBlock, creator_P)
     ASSERT_FALSE(sba->hasLocalParametrization());
 
     // fails
-    ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::length_error);
+    ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::runtime_error);
 }
 
 TEST(FactoryStateBlock, creator_Vector)
@@ -158,7 +158,7 @@ TEST(FactoryStateBlock, creator_Vector)
     ASSERT_FALSE(sba->hasLocalParametrization());
 
     // fails
-    ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+    ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error);
 }
 
 TEST(FactoryStateBlock, creator_V)
@@ -176,7 +176,7 @@ TEST(FactoryStateBlock, creator_V)
     ASSERT_FALSE(sba->hasLocalParametrization());
 
     // fails
-    ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::length_error);
+    ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::runtime_error);
 }
 
 TEST(FactoryStateBlock, creator_Params)
@@ -207,7 +207,7 @@ TEST(FactoryStateBlock, creator_Params)
     ASSERT_FALSE(sb1->hasLocalParametrization());
 
     // fails
-    ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::length_error);
+    ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error);
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index b82a64b48..c8abf4fa5 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -203,45 +203,45 @@ TEST(Prior, StateBlock)
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
   // Unknown type -> StateBlock
-  setups_ok   .push_back(PriorAsStruct({"K","initial_guess",vector1,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"K","initial_guess",vector1,vector0,false})); // unknown K
 
   // Initial guess - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","initial_guess",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams3","initial_guess",vector3,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams4","initial_guess",vector4,vector0,false}));
 
   // Initial guess - dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,true}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,true,vector3}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,true,vector4}));
-  setups_death.push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,true,vector4})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","initial_guess",vector2,vector0,true}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","initial_guess",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams3","initial_guess",vector3,vector0,true,vector3}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams4","initial_guess",vector4,vector0,true,vector4}));
+  setups_death.push_back(PriorAsStruct({"StateParams3","initial_guess",vector3,vector0,true,vector4})); // inconsistent size
   
   // Fix - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,false})); // wrong size
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","fix",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams3","fix",vector3,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"StateParams3","fix",vector4,vector0,false})); // wrong size
 
   // Fix - dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,true,vector0}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,true,vector3}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,true,vector4}));
-  setups_death.push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,true,vector4})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","fix",vector2,vector0,true,vector0}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","fix",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams3","fix",vector3,vector0,true,vector3}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams4","fix",vector4,vector0,true,vector4}));
+  setups_death.push_back(PriorAsStruct({"StateParams3","fix",vector3,vector0,true,vector4})); // inconsistent size
   
   // Factor - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector4,vector4,false}));
-  setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector2,vector3,false})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","factor",vector2,vector2,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams3","factor",vector3,vector3,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams4","factor",vector4,vector4,false}));
+  setups_death.push_back(PriorAsStruct({"StateParams2","factor",vector2,vector3,false})); // inconsistent size
 
   // Factor - dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,true}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,true,vector3}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector4,vector4,true,vector4}));
-  setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,true,vector4})); // inconsistent size
-  setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector3,vector4,true,vector3})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","factor",vector2,vector2,true}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams2","factor",vector2,vector2,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams3","factor",vector3,vector3,true,vector3}));
+  setups_ok   .push_back(PriorAsStruct({"StateParams4","factor",vector4,vector4,true,vector4}));
+  setups_death.push_back(PriorAsStruct({"StateParams3","factor",vector3,vector3,true,vector4})); // inconsistent size
+  setups_death.push_back(PriorAsStruct({"StateParams3","factor",vector3,vector4,true,vector3})); // inconsistent size
 
   // TEST SETUPS
   testPriors(setups_ok, true);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index fbc835ae1..b73e36e12 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -76,12 +76,12 @@ TEST(Problem, Sensors)
     ProblemPtr P = Problem::create("POV", 3);
 
     // add a dummy sensor
-    auto S = SensorBase::emplace<SensorDummy>(P->getHardware(),
-                                              "dummy_name", 
-                                              3, 
-                                              std::make_shared<ParamsSensorDummy>(),
-                                              Priors({{'P',Prior("P",Vector3d::Zero())},
-                                                      {'O',Prior("O",Vector4d::Random().normalized())}}));
+    auto params = std::make_shared<ParamsSensorDummy>();
+    params->name = "dummy_name";
+    auto S = SensorBase::emplace<SensorDummy3d>(P->getHardware(),
+                                                params,
+                                                Priors({{'P',Prior("P",Vector3d::Zero())},
+                                                        {'O',Prior("O",Vector4d::Random().normalized())}}));
     // check pointers
     ASSERT_EQ(P, S->getProblem());
     ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -96,12 +96,12 @@ TEST(Problem, Processor)
     ASSERT_TRUE(P->getMotionProviderMap().empty());
 
     // add a motion sensor and processor
-    auto Sm = SensorBase::emplace<SensorOdom>(P->getHardware(), 
-                                              "odometer", 
-                                              3, 
-                                              std::make_shared<ParamsSensorOdom>(), 
-                                              Priors({{'P',Prior("P",Vector3d::Zero())},
-                                                      {'O',Prior("O",Vector4d::Random().normalized())}}));
+    auto params = std::make_shared<ParamsSensorOdom>();
+    params->name = "dummy_name";
+    auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), 
+                                                params, 
+                                                Priors({{'P',Prior("P",Vector3d::Zero())},
+                                                        {'O',Prior("O",Vector4d::Random().normalized())}}));
 
     // add motion processor
     auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
@@ -116,16 +116,16 @@ TEST(Problem, Installers)
     ProblemPtr P = Problem::create("PO", 3);
     Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize();
 
-    SensorBasePtr    S = P->installSensor   ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+    SensorBasePtr S = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
 
     // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
-    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
+    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", S->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root});
 
     // check motion processor IS NOT set
     ASSERT_TRUE(P->getMotionProviderMap().empty());
 
     // install processor motion
-    ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
+    ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", S->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root});
 
     // check motion processor is set
     ASSERT_FALSE(P->getMotionProviderMap().empty());
@@ -292,11 +292,11 @@ TEST(Problem, StateBlocks)
     Eigen::Vector3d xs2d;
 
     // 2 state blocks, fixed
-    SensorBasePtr    Sm = P->installSensor   ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+    SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
 
-    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy",  "dummy",            "odometer");
-    auto pm = P->installProcessor("ProcessorOdom3d",                "odom integrator",  "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
+    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root});
+    auto pm = P->installProcessor("ProcessorOdom3d", Sm->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root});
 
     // 2 state blocks, estimated
     auto KF = P->emplaceFrame(0, "PO", 3, xs3d );
@@ -345,16 +345,17 @@ TEST(Problem, Covariances)
 
     Eigen::Vector3d xs2d;
 
-    SensorBasePtr    Sm = P->installSensor   ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml");
-    SensorBasePtr    St = P->installSensor   ("SensorOdom", "other odometer",  wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+    SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
+    SensorBasePtr St = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d_other.yaml", {wolf_root});
 
     ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>();
     params->time_tolerance            = 0.1;
     params->max_new_features          = 5;
     params->min_features_for_keyframe = 10;
 
-    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy",  "dummy",            Sm, params);
-    auto pm = P->installProcessor("ProcessorOdom3d",               "odom integrator",  "other odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
+
+    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root});
+    auto pm = P->installProcessor("ProcessorOdom3d", St->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root});
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();
@@ -381,13 +382,13 @@ TEST(Problem, perturb)
 
     // make a sensor first
     auto param = std::make_shared<ParamsSensorDiffDrive>();
-    param->ticks_per_wheel_revolution    = 100;
-    auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", 
-                                                                                   "sensor diff drive", 
-                                                                                   param,
-                                                                                   Priors({{'P',Prior("P",Vector2d::Zero())},
-                                                                                           {'O',Prior("O",Vector1d::Zero())},
-                                                                                           {'I',Prior("StateBlock",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})));
+    param->name = "sensor diff drive 1";
+    param->ticks_per_wheel_revolution = 100;
+    auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
+                                                       param,
+                                                       Priors({{'P',Prior("P",Vector2d::Zero())},
+                                                               {'O',Prior("O",Vector1d::Zero())},
+                                                               {'I',Prior("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
 
     Vector3d pose; pose << 0,0,0;
 
@@ -467,14 +468,13 @@ TEST(Problem, check)
 
     // make a sensor first
     auto param = std::make_shared<ParamsSensorDiffDrive>();
-    param->ticks_per_wheel_revolution    = 100;
-    auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", 
-                                                                                   "sensor diff drive", 
-                                                                                   param,
-                                                                                   Priors({{'P',Prior("P",Vector2d::Zero())},
-                                                                                           {'O',Prior("O",Vector1d::Zero())},
-                                                                                           {'I',Prior("StateBlock",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})));
-
+    param->name = "sensor diff drive 1";
+    param->ticks_per_wheel_revolution = 100;
+    auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
+                                                       param,
+                                                       Priors({{'P',Prior("P",Vector2d::Zero())},
+                                                               {'O',Prior("O",Vector1d::Zero())},
+                                                               {'I',Prior("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
     Vector3d pose; pose << 0,0,0;
 
     int i = 0;
@@ -526,36 +526,21 @@ TEST(Problem, check)
 
 TEST(Problem, autoSetupMap)
 {
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup.yaml");
-    auto server = ParamsServer(parser.getParams());
-
-    auto P = Problem::autoSetup(server);
+    auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup.yaml", {wolf_root});
 
     ASSERT_TRUE(P->check(true, std::cout));
 }
 
 TEST(Problem, autoSetupNoMap)
 {
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup_no_map.yaml");
-    auto server = ParamsServer(parser.getParams());
-
-    auto P = Problem::autoSetup(server);
+    auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup_no_map.yaml", {wolf_root});
 
     ASSERT_TRUE(P->check(true, std::cout));
 }
 
 TEST(Problem, getState)
 {
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup.yaml");
-    auto server = ParamsServer(parser.getParams());
-
-    auto P = Problem::autoSetup(server);
+    auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup.yaml", {wolf_root});
 
     auto S = P->getHardware()->getSensorList().front();
 
@@ -620,12 +605,7 @@ TEST(Problem, getState)
 
 TEST(Problem, transform)
 {
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
-    auto server = ParamsServer(parser.getParams());
-
-    auto P = Problem::autoSetup(server);
+    auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup.yaml", {wolf_root});
 
     auto S = P->getHardware()->getSensorList().front();
 
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 613857d10..81c3ac989 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -63,7 +63,7 @@ TEST(ProcessorBase, MotionProvider)
     ProblemPtr problem = Problem::create("PO", 2);
 
     // Install tracker (sensor and processor)
-    auto sens_trk = problem->installSensor("SensorDummy",
+    auto sens_trk = problem->installSensor("SensorDummy2d",
                                            wolf_root + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml",
                                            {wolf_root});
     auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
@@ -71,7 +71,7 @@ TEST(ProcessorBase, MotionProvider)
     auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
 
     // Install odometer (sensor and processor)
-    auto sens_odo = problem->installSensor("SensorOdom",
+    auto sens_odo = problem->installSensor("SensorOdom2d",
                                            wolf_root + "/test/yaml/sensor_odom_2d.yaml",
                                            {wolf_root});
     auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
@@ -101,7 +101,7 @@ TEST(ProcessorBase, KeyFrameCallback)
     ProblemPtr problem = Problem::create("PO", 2);
 
     // Install tracker (sensor and processor)
-    auto sens_trk = problem->installSensor("SensorDummy",
+    auto sens_trk = problem->installSensor("SensorDummy2d",
                                            wolf_root + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml",
                                            {wolf_root});
     auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
@@ -109,7 +109,7 @@ TEST(ProcessorBase, KeyFrameCallback)
     auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
 
     // Install odometer (sensor and processor)
-    auto sens_odo = problem->installSensor("SensorOdom",
+    auto sens_odo = problem->installSensor("SensorOdom2d",
                                            wolf_root + "/test/yaml/sensor_odom_2d.yaml",
                                            {wolf_root});
     auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index c4b37a73f..8b157d5ee 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -69,7 +69,7 @@ class ProcessorMotion_test : public testing::Test{
 
             dt                      = 1.0;
             problem = Problem::create("PO", 2);
-            sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom", 
+            sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", 
                                                                             wolf_root + "/test/yaml/sensor_odom_2d.yaml",
                                                                             {wolf_root}));
             ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 5280195fd..1ec8217d3 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -335,7 +335,7 @@ TEST(SensorBase, makeshared_priors_POIA_mixed)
   auto S = std::make_shared<SensorDummy3d>(params, 
                                            Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
                                                    {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                   {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
+                                                   {'I',Prior("StateParams5", i_state_3D, "initial_guess")},
                                                    {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
      
   // noise
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 463b82987..ea5ab4946 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -48,8 +48,10 @@ TEST(TreeManager, createNode)
 {
     ProblemPtr P = Problem::create("PO", 2);
 
-    auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root + "/schemas"}, 
-                                                   wolf_root + "/test/yaml/params_tree_manager1.yaml");
+    auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root}, 
+                                                   wolf_root + "/test/yaml/tree_manager_dummy.yaml");
+    
+    WOLF_INFO_COND(not yaml_server.applySchema("TreeManagerDummy"), yaml_server.getLog().str());
     ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy"));
 
     auto GM = TreeManagerDummy::create(yaml_server.getNode());
@@ -66,7 +68,7 @@ TEST(TreeManager, createYaml)
 {
     ProblemPtr P = Problem::create("PO", 2);
 
-    auto GM = TreeManagerDummy::create(wolf_root + "/test/yaml/tree_manager_dummy.yaml", {wolf_root + "/schemas"});
+    auto GM = TreeManagerDummy::create(wolf_root + "/test/yaml/tree_manager_dummy.yaml", {wolf_root});
 
     ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
 
@@ -80,8 +82,8 @@ TEST(TreeManager, FactoryParam)
 {
     ProblemPtr P = Problem::create("PO", 2);
 
-    auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root + "/schemas"}, 
-                                                   wolf_root + "/test/yaml/params_tree_manager1.yaml");
+    auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root}, 
+                                                   wolf_root + "/test/yaml/tree_manager_dummy.yaml");
     ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy"));
 
     auto GM = FactoryTreeManager::create("TreeManagerDummy", yaml_server.getNode());
@@ -100,7 +102,7 @@ TEST(TreeManager, FactoryYaml)
 
     auto GM = FactoryTreeManagerYaml::create("TreeManagerDummy", 
                                              wolf_root + "/test/yaml/tree_manager_dummy.yaml", 
-                                             {wolf_root + "/schemas"});
+                                             {wolf_root});
 
     ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
 
@@ -112,7 +114,7 @@ TEST(TreeManager, FactoryYaml)
 
 TEST(TreeManager, autoConf)
 {
-    ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager1.yaml", {wolf_root + "/schemas"});
+    ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager1.yaml", {wolf_root});
     P->applyPriorOptions(0);
 
     ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr);
@@ -121,7 +123,7 @@ TEST(TreeManager, autoConf)
 
 TEST(TreeManager, autoConfNone)
 {
-    ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager2.yaml", {wolf_root + "/schemas"});
+    ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager2.yaml", {wolf_root});
     P->applyPriorOptions(0);
 
     ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
index 4d0487bde..14c6271cd 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -18,4 +18,5 @@ config:
       name: "my_proc_test"
       plugin: "core"
       sensor_name: "odom"
-      extern_params: "@params3.1.yaml"
\ No newline at end of file
+      extern_params: 
+        follow: "params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml
index dbdc6c608..e6cea5317 100644
--- a/test/yaml/params_prior.yaml
+++ b/test/yaml/params_prior.yaml
@@ -244,40 +244,40 @@ O_3D_factor_dynamic:
 
 # I
 I_initial_guess:
-  type: StateParam4
+  type: StateParams4
   mode: initial_guess
   state: [1, 2, 3, 4]
   dynamic: false
 
 I_fix:
-  type: StateParam4
+  type: StateParams4
   mode: fix
   state: [1, 2, 3, 4]
   dynamic: false
 
 I_factor:
-  type: StateParam4
+  type: StateParams4
   mode: factor
   state: [1, 2, 3, 4]
   noise_std: [0.1, 0.2, 0.3, 0.4]
   dynamic: false
 
 I_initial_guess_dynamic_drift:
-  type: StateParam4
+  type: StateParams4
   mode: initial_guess
   state: [1, 2, 3, 4]
   dynamic: true
   drift_std: [0.1, 0.2, 0.3, 0.4]
 
 I_fix_dynamic_drift:
-  type: StateParam4
+  type: StateParams4
   mode: fix
   state: [1, 2, 3, 4]
   dynamic: true
   drift_std: [0.1, 0.2, 0.3, 0.4]
 
 I_factor_dynamic_drift:
-  type: StateParam4
+  type: StateParams4
   mode: factor
   state: [1, 2, 3, 4]
   noise_std: [0.1, 0.2, 0.3, 0.4]
@@ -285,19 +285,19 @@ I_factor_dynamic_drift:
   drift_std: [0.1, 0.2, 0.3, 0.4]
 
 I_initial_guess_dynamic:
-  type: StateParam4
+  type: StateParams4
   mode: initial_guess
   state: [1, 2, 3, 4]
   dynamic: true
 
 I_fix_dynamic:
-  type: StateParam4
+  type: StateParams4
   mode: fix
   state: [1, 2, 3, 4]
   dynamic: true
 
 I_factor_dynamic:
-  type: StateParam4
+  type: StateParams4
   mode: factor
   state: [1, 2, 3, 4]
   noise_std: [0.1, 0.2, 0.3, 0.4]
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
index 6da054449..12d338a33 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -11,4 +11,5 @@ config:
         P: [0.31, 0.31, 0.31]
         O: [0.31, 0.31, 0.31]
       time_tolerance: 0.1
-    tree_manager: "@tree_manager_sliding_window_dual_rate1.yaml"
\ No newline at end of file
+    tree_manager: 
+      follow: "tree_manager_sliding_window_dual_rate1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index 79f9a97fa..35cd9bc7d 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -1,5 +1,6 @@
 config:
-  solver: "@solver.yaml"
+  solver: 
+    follow: "solver.yaml"
   problem:
     frame_structure: "PO"
     dimension: 3
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index 46e4eeff5..769d7376a 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -1,5 +1,6 @@
 config:
-  solver: "@solver.yaml"
+  solver: 
+    follow: "solver.yaml"
   problem:
     frame_structure: "PO"
     dimension: 3
diff --git a/test/yaml/processor_tracker_feature_dummy.yaml b/test/yaml/processor_tracker_feature_dummy.yaml
new file mode 100644
index 000000000..ea6f0af32
--- /dev/null
+++ b/test/yaml/processor_tracker_feature_dummy.yaml
@@ -0,0 +1,4 @@
+name: cool dummy processor
+min_features_for_keyframe: 1
+max_new_features: 0
+n_tracks_lost: 1
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml
index ea22ca723..d3965dfe4 100644
--- a/test/yaml/sensor_odom_3d.yaml
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -1,3 +1,4 @@
+name: odometer 3d
 states:
   P:
     mode: fix
diff --git a/test/yaml/sensor_odom_3d_other.yaml b/test/yaml/sensor_odom_3d_other.yaml
new file mode 100644
index 000000000..98b573c9e
--- /dev/null
+++ b/test/yaml/sensor_odom_3d_other.yaml
@@ -0,0 +1,2 @@
+follow: sensor_odom_3d.yaml
+name: another odometer 3d
\ No newline at end of file
-- 
GitLab