From 2addf05245c9479780390bca7edffe7aa1b5f672 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 4 Jul 2022 17:55:59 +0200
Subject: [PATCH] [skip ci] working on gtests

---
 include/core/problem/problem.h                |  19 -
 include/core/processor/factory_processor.h    |  10 -
 include/core/processor/processor_base.h       |  14 +-
 include/core/sensor/factory_sensor.h          |  26 +-
 include/core/sensor/sensor_base.h             |  14 -
 include/core/sensor/sensor_odom.h             |  20 +-
 include/core/state_block/prior.h              |   8 +-
 .../core/tree_manager/factory_tree_manager.h  |  10 -
 include/core/tree_manager/tree_manager_base.h |   6 -
 schema/PriorModeDynamicNoiseDrift.schema      |  26 ++
 schema/PriorO2d.schema                        |  27 +-
 schema/PriorO3d.schema                        |  13 +
 schema/PriorP2d.schema                        |  27 +-
 schema/PriorP3d.schema                        |  13 +
 src/problem/problem.cpp                       |  36 --
 src/sensor/sensor_base.cpp                    |  12 +-
 src/sensor/sensor_odom.cpp                    |  20 +-
 src/state_block/prior.cpp                     |  19 +-
 ...ensorDummy.schema => SensorDummy2d.schema} |   0
 test/dummy/SensorDummy3d.schema               |  16 +
 test/dummy/SensorDummyPoia3d.schema           |  44 +++
 test/dummy/sensor_dummy.h                     |   5 +-
 test/dummy/sensor_dummy_poia.h                |  87 ----
 test/gtest_processor_base.cpp                 |  12 +-
 test/gtest_sensor_base.cpp                    | 372 ++----------------
 test/gtest_sensor_odom.cpp                    | 292 --------------
 test/gtest_tree_manager.cpp                   |  32 --
 test/yaml/sensor_tests/sensor_POIA_3D.yaml    |   4 +-
 .../sensor_tests/sensor_POIA_3D_wrong.yaml    |   3 +-
 .../sensor_tests/sensor_PO_2D_factor.yaml     |   3 +-
 .../sensor_PO_2D_factor_dynamic.yaml          |   3 +-
 .../sensor_PO_2D_factor_dynamic_drift.yaml    |   2 +
 ...nsor_PO_2D_factor_dynamic_drift_wrong.yaml |   2 +
 .../sensor_PO_2D_factor_dynamic_wrong.yaml    |   2 +
 .../sensor_PO_2D_factor_wrong.yaml            |   3 +-
 test/yaml/sensor_tests/sensor_PO_2D_fix.yaml  |   3 +-
 .../sensor_PO_2D_fix_dynamic.yaml             |   3 +-
 .../sensor_PO_2D_fix_dynamic_drift.yaml       |   3 +-
 .../sensor_PO_2D_fix_dynamic_drift_wrong.yaml |   3 +-
 .../sensor_PO_2D_fix_dynamic_wrong.yaml       |   3 +-
 .../sensor_tests/sensor_PO_2D_fix_wrong.yaml  |   3 +-
 .../sensor_PO_2D_initial_guess.yaml           |   3 +-
 .../sensor_PO_2D_initial_guess_dynamic.yaml   |   3 +-
 ...sor_PO_2D_initial_guess_dynamic_drift.yaml |   3 +-
 ..._2D_initial_guess_dynamic_drift_wrong.yaml |   3 +-
 ...sor_PO_2D_initial_guess_dynamic_wrong.yaml |   3 +-
 .../sensor_PO_2D_initial_guess_wrong.yaml     |   3 +-
 .../sensor_tests/sensor_PO_3D_factor.yaml     |   3 +-
 .../sensor_PO_3D_factor_dynamic.yaml          |   3 +-
 .../sensor_PO_3D_factor_dynamic_drift.yaml    |   3 +-
 ...nsor_PO_3D_factor_dynamic_drift_wrong.yaml |   3 +-
 .../sensor_PO_3D_factor_dynamic_wrong.yaml    |   3 +-
 .../sensor_PO_3D_factor_wrong.yaml            |   3 +-
 test/yaml/sensor_tests/sensor_PO_3D_fix.yaml  |   3 +-
 .../sensor_PO_3D_fix_dynamic.yaml             |   3 +-
 .../sensor_PO_3D_fix_dynamic_drift.yaml       |   3 +-
 .../sensor_PO_3D_fix_dynamic_drift_wrong.yaml |   3 +-
 .../sensor_PO_3D_fix_dynamic_wrong.yaml       |   3 +-
 .../sensor_tests/sensor_PO_3D_fix_wrong.yaml  |   3 +-
 .../sensor_PO_3D_initial_guess.yaml           |   3 +-
 .../sensor_PO_3D_initial_guess_dynamic.yaml   |   3 +-
 ...sor_PO_3D_initial_guess_dynamic_drift.yaml |   3 +-
 ..._3D_initial_guess_dynamic_drift_wrong.yaml |   3 +-
 ...sor_PO_3D_initial_guess_dynamic_wrong.yaml |   3 +-
 .../sensor_PO_3D_initial_guess_wrong.yaml     |   3 +-
 65 files changed, 261 insertions(+), 1031 deletions(-)
 create mode 100644 schema/PriorModeDynamicNoiseDrift.schema
 create mode 100644 schema/PriorO3d.schema
 create mode 100644 schema/PriorP3d.schema
 rename test/dummy/{SensorDummy.schema => SensorDummy2d.schema} (100%)
 create mode 100644 test/dummy/SensorDummy3d.schema
 create mode 100644 test/dummy/SensorDummyPoia3d.schema
 delete mode 100644 test/dummy/sensor_dummy_poia.h

diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 2a779c07e..027b1b4e8 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -146,14 +146,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         SensorBasePtr installSensor(const std::string& _sen_type,
                                     const std::string& _params_yaml_filename,
                                     const std::vector<std::string>& _folders_schema);
-        /** \brief Factory method to install (create and add) sensors only from its properties
-         * \param _sen_type type of sensor
-         * \param _params a base-pointer to a derived struct defining the intrinsic parameters.
-         * \param _priors an unordered map of the states priors indexed by their key.
-         */
-        SensorBasePtr installSensor(const std::string& _sen_type,
-                                    ParamsSensorBasePtr _params,
-                                    const Priors& _priors);
 
         /** \brief Factory method to install (create, and add to sensor) processors only from a YAML node -- Helper method loading parameters from file
          * \param _processor_node YAML node containing all necessary information to call the factory and create the processor.
@@ -175,17 +167,6 @@ class Problem : public std::enable_shared_from_this<Problem>
                                           const std::string& _params_yaml_filename,
                                           const std::vector<std::string>& _folders_schema);
 
-        /** \brief Factory method to install (create, and add to sensor) processors only from its properties
-         *
-         * This method creates a Processor, and adds it to the specified sensor's list of processors
-         * \param _prc_type type of processor
-         * \param _corresponding_sensor_ptr pointer to the sensor where the processor will be installed.
-         * \param _prc_params a base-pointer to a derived struct defining the processor parameters.
-         */
-        ProcessorBasePtr installProcessor(const std::string& _prc_type,
-                                          SensorBasePtr _corresponding_sensor_ptr,
-                                          ParamsProcessorBasePtr _prc_params);
-
         /** \brief get a sensor pointer by its name
          * \param _sensor_name The sensor name, as it was installed with installSensor()
          */
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 253ccb936..b432824fb 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -134,20 +134,10 @@ inline std::string FactoryProcessorYaml::getClass() const
     return "FactoryProcessorYaml";
 }
 
-typedef Factory<ProcessorBase,
-                std::shared_ptr<ParamsProcessorBase>> FactoryProcessorParams;
-template<>
-inline std::string FactoryProcessorParams::getClass() const
-{
-    return "FactoryProcessorParams";
-}
-
 #define WOLF_REGISTER_PROCESSOR(ProcessorType)                                                \
   namespace{ const bool WOLF_UNUSED ProcessorType##Registered =                               \
     wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); }         \
   namespace{ const bool WOLF_UNUSED ProcessorType##YamlRegistered =                           \
     wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); }     \
-  namespace{ const bool WOLF_UNUSED ProcessorType##ParamsRegistered =                         \
-    wolf::FactoryProcessorParams::registerCreator(#ProcessorType, ProcessorType::create); }   \
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 124744ce3..adc03e088 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -80,19 +80,6 @@ static ProcessorBasePtr create(const std::string& _yaml_filepath,
                                                                                     \
     return std::make_shared<ProcessorClass>(params);                                \
 }                                                                                   \
-static ProcessorBasePtr create(const ParamsProcessorBasePtr _params)                \
-{                                                                                   \
-    auto params_derived = std::dynamic_pointer_cast<ParamsProcessorClass>(_params); \
-    if (not params_derived)                                                         \
-    {                                                                               \
-        WOLF_ERROR("In " #ProcessorClass " creator:",                               \
-                   " _params is not of type " #ParamsProcessorClass "!");           \
-        return nullptr;                                                             \
-    }                                                                               \
-                                                                                    \
-    return std::make_shared<ProcessorClass>(params_derived);                        \
-}                                                                                   \
-
 
 /** \brief Buffer for arbitrary type objects
  *
@@ -468,6 +455,7 @@ template<typename classType, typename... T>
 std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
 {
     std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...);
+    prc->configure(_sen_ptr);
     prc->link(_sen_ptr);
     return prc;
 }
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index fa8cc69e3..976097d45 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -218,11 +218,6 @@ typedef Factory<SensorBase,
                 const std::string&,
                 const std::vector<std::string>&> FactorySensorYaml;
 
-typedef Factory<SensorBase,
-                SizeEigen,
-                std::shared_ptr<ParamsSensorBase>,
-                const Priors& > FactorySensorPriors;
-
 template<>
 inline std::string FactorySensor::getClass() const
 {
@@ -233,18 +228,15 @@ inline std::string FactorySensorYaml::getClass() const
 {
   return "FactorySensorYaml";
 }
-template<>
-inline std::string FactorySensorPriors::getClass() const
-{
-  return "FactorySensorPriors";
-}
 
-#define WOLF_REGISTER_SENSOR(SensorType)                                     \
-  namespace{ const bool WOLF_UNUSED SensorType##Registered =                 \
-     FactorySensor::registerCreator(#SensorType, SensorType::create);}       \
-  namespace{ const bool WOLF_UNUSED SensorType##YamlRegistered =             \
-     FactorySensorYaml::registerCreator(#SensorType, SensorType::create);}   \
-  namespace{ const bool WOLF_UNUSED SensorType##PriorsRegistered =           \
-     FactorySensorPriors::registerCreator(#SensorType, SensorType::create);} \
+
+
+#define WOLF_REGISTER_SENSOR_WITH_KEY(SensorClassName, SensorType)                \
+  namespace{ const bool WOLF_UNUSED SensorClassName##Registered =                 \
+     FactorySensor::registerCreator(#SensorClassName, SensorType::create);}       \
+  namespace{ const bool WOLF_UNUSED SensorClassName##YamlRegistered =             \
+     FactorySensorYaml::registerCreator(#SensorClassName, SensorType::create);}   \
+
+#define WOLF_REGISTER_SENSOR(SensorType) WOLF_REGISTER_SENSOR_WITH_KEY(SensorType, SensorType) \
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 5910fe3c4..cc1f5d408 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -86,20 +86,6 @@ static SensorBasePtr create(SizeEigen _dim,
                                                                                  \
     return std::make_shared<SensorClass>(_dim, params, priors);                  \
 }                                                                                \
-static SensorBasePtr create(SizeEigen _dim,                                      \
-                            ParamsSensorBasePtr _params,                         \
-                            const Priors& _priors)                               \
-{                                                                                \
-    auto params_derived = std::dynamic_pointer_cast<ParamsSensorClass>(_params); \
-    if (not params_derived)                                                      \
-    {                                                                            \
-        WOLF_ERROR("In " #SensorClass " creator:",                               \
-                   " _params is not of type " #ParamsSensorClass "!");           \
-        return nullptr;                                                          \
-    }                                                                            \
-                                                                                 \
-    return std::make_shared<SensorClass>(_dim, params_derived, _priors);         \
-}                                                                                \
 
 /** \brief base struct for intrinsic sensor parameters
  *
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index b73c95ce1..6cb43d327 100644
--- a/include/core/sensor/sensor_odom.h
+++ b/include/core/sensor/sensor_odom.h
@@ -67,10 +67,10 @@ class SensorOdom : public SensorBase
         SizeEigen dim_;
         ParamsSensorOdomPtr params_odom_;
         
-        // protected constructor to avoid creation of SensorOdom without dimension
+	public:
         SensorOdom(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors);
+        WOLF_SENSOR_CREATE(SensorOdom, ParamsSensorOdom);
 
-	public:
         ~SensorOdom() override = default;
 
         double getDispVarToDispNoiseFactor() const;
@@ -83,22 +83,6 @@ class SensorOdom : public SensorBase
 
 };
 
-WOLF_PTR_TYPEDEFS(SensorOdom2d);
-class SensorOdom2d : public SensorOdom
-{
-    public:
-        SensorOdom2d(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors);
-        WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom);
-};
-
-WOLF_PTR_TYPEDEFS(SensorOdom3d);
-class SensorOdom3d : public SensorOdom
-{
-    public:
-        SensorOdom3d(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors);
-        WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom);
-};
-
 inline double SensorOdom::getDispVarToDispNoiseFactor() const
 {
     return params_odom_->k_disp_to_disp;
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 0864028a4..67da9837d 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -74,13 +74,7 @@ class Priors : public PriorMap
     public:
         using PriorMap::PriorMap;
 
-        Priors(const YAML::Node& priors_node)
-        {
-            for (auto prior_pair : priors_node)
-            {
-                this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second));
-            }
-        }
+        Priors(const YAML::Node& priors_node);
         virtual ~Priors() = default;
 };
 
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index edd9a44ef..c4f4e4d56 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -52,20 +52,10 @@ inline std::string FactoryTreeManagerYaml::getClass() const
     return "FactoryTreeManagerYaml";
 }
 
-typedef Factory<TreeManagerBase,
-                ParamsTreeManagerBasePtr> FactoryTreeManagerParams;
-template<>
-inline std::string FactoryTreeManagerParams::getClass() const
-{
-  return "FactoryTreeManagerParams";
-}
-
 #define WOLF_REGISTER_TREE_MANAGER(TreeManagerType)                                               \
   namespace{ const bool WOLF_UNUSED TreeManagerType##Registered =                                 \
     wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); }       \
   namespace{ const bool WOLF_UNUSED TreeManagerType##YamlRegistered =                             \
     wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); }   \
-  namespace{ const bool WOLF_UNUSED TreeManagerType##ParamsRegistered =                           \
-    wolf::FactoryTreeManagerParams::registerCreator(#TreeManagerType, TreeManagerType::create); } \
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 508f48438..588f735c9 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -64,12 +64,6 @@ static TreeManagerBasePtr create(const std::string& _yaml_filepath,
                                                                                     \
     return std::make_shared<TreeManagerClass>(params);                              \
 }                                                                                   \
-static TreeManagerBasePtr create(ParamsTreeManagerBasePtr _params)                  \
-{                                                                                   \
-    auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params);        \
-                                                                                    \
-    return std::make_shared<TreeManagerClass>(params);                              \
-}                                                                                   \
 
 struct ParamsTreeManagerBase : public ParamsBase
 {
diff --git a/schema/PriorModeDynamicNoiseDrift.schema b/schema/PriorModeDynamicNoiseDrift.schema
new file mode 100644
index 000000000..4c14a5652
--- /dev/null
+++ b/schema/PriorModeDynamicNoiseDrift.schema
@@ -0,0 +1,26 @@
+mode:
+  type: string
+  yaml_type: scalar
+  mandatory: true
+  options:
+    - "fix"
+    - "factor"
+    - "initial_guess"
+  doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
+dynamic:
+  type: bool
+  yaml_type: scalar
+  mandatory: true
+  doc: If the state is dynamic, i.e. it changes along time.
+noise_std:
+  type: VectorXd
+  yaml_type: scalar
+  mandatory: false
+  default: []
+  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
+drift_std:
+  type: VectorXd
+  yaml_type: scalar
+  mandatory: false
+  default: []
+  doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/PriorO2d.schema b/schema/PriorO2d.schema
index 08173f33b..402d0c37e 100644
--- a/schema/PriorO2d.schema
+++ b/schema/PriorO2d.schema
@@ -10,29 +10,4 @@ state:
   yaml_type: scalar
   mandatory: true
   doc: A vector containing the state values
-mode:
-  type: string
-  yaml_type: scalar
-  mandatory: true
-  options:
-    - "fix"
-    - "factor"
-    - "initial_guess"
-  doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-dynamic:
-  type: bool
-  yaml_type: scalar
-  mandatory: true
-  doc: If the state is dynamic, i.e. it changes along time.
-noise_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-drift_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/schema/PriorO3d.schema b/schema/PriorO3d.schema
new file mode 100644
index 000000000..e7b1dd38b
--- /dev/null
+++ b/schema/PriorO3d.schema
@@ -0,0 +1,13 @@
+type:
+  type: string
+  yaml_type: scalar
+  mandatory: false
+  default: StateQuaternion
+  options: [StateQuaternion]
+  doc: The derived type of the State in 'O'
+state:
+  type: Vector4d
+  yaml_type: scalar
+  mandatory: true
+  doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized)
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/schema/PriorP2d.schema b/schema/PriorP2d.schema
index 3e17abd5f..d8f19390f 100644
--- a/schema/PriorP2d.schema
+++ b/schema/PriorP2d.schema
@@ -10,29 +10,4 @@ state:
   yaml_type: scalar
   mandatory: true
   doc: A vector containing the state values
-mode:
-  type: string
-  yaml_type: scalar
-  mandatory: true
-  options:
-    - "fix"
-    - "factor"
-    - "initial_guess"
-  doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-dynamic:
-  type: bool
-  yaml_type: scalar
-  mandatory: true
-  doc: If the state is dynamic, i.e. it changes along time.
-noise_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-drift_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/schema/PriorP3d.schema b/schema/PriorP3d.schema
new file mode 100644
index 000000000..d413a9029
--- /dev/null
+++ b/schema/PriorP3d.schema
@@ -0,0 +1,13 @@
+type:
+  type: string
+  yaml_type: scalar
+  mandatory: false
+  default: StatePoint3d
+  options: [StatePoint3d]
+  doc: The derived type of the state in 'P'
+state:
+  type: Vector3d
+  yaml_type: scalar
+  mandatory: true
+  doc: A vector containing the state 'P' values
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 98a12bb81..0442805fe 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -259,18 +259,6 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type,
     return sen_ptr;
 }
 
-SensorBasePtr Problem::installSensor(const std::string& _sen_type,
-                                     ParamsSensorBasePtr _params,
-                                     const Priors& _priors)
-{
-    SensorBasePtr sen_ptr = FactorySensorPriors::create(_sen_type, 
-                                                        dim_, 
-                                                        _params, 
-                                                        _priors);
-    sen_ptr->link(getHardware());
-    return sen_ptr;
-}
-
 ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node)
 {
     auto corresponding_sensor_name = _processor_node["sensor_name"].as<std::string>();
@@ -316,30 +304,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type,
     return prc_ptr;
 }
 
-ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type,
-                                           SensorBasePtr _corresponding_sensor_ptr,
-                                           ParamsProcessorBasePtr _prc_params)
-{
-    if (_corresponding_sensor_ptr == nullptr)
-    {
-      WOLF_ERROR("Cannot install processor '", _prc_params->name,
-                 "' since the associated sensor does not exist !");
-      return ProcessorBasePtr();
-    }
-
-    ProcessorBasePtr prc_ptr = FactoryProcessorParams::create(_prc_type, _prc_params);
-
-    //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(_corresponding_sensor_ptr);
-    prc_ptr->link(_corresponding_sensor_ptr);
-
-    return prc_ptr;
-}
-
 SensorBaseConstPtr Problem::findSensor(const std::string& _sensor_name) const
 {
     return getHardware()->getSensor(_sensor_name);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 9ecb93796..d6ad0ad51 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -69,10 +69,14 @@ void SensorBase::loadPriors(const Priors& _priors,
         auto prior = state_pair.second;
 
         // type
-        if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock")
-            throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'");
-        if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock")
-            throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'");
+        if (key == 'P' and _dim == 2 and prior.getType() != "P" and prior.getType() != "StatePoint2d")
+            throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint2d'");
+        if (key == 'P' and _dim == 3 and prior.getType() != "P" and prior.getType() != "StatePoint3d")
+            throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint3d'");
+        if (key == 'V' and _dim == 2 and prior.getType() != "V" and prior.getType() != "StatePoint2d")
+            throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint2d'");
+        if (key == 'V' and _dim == 3 and prior.getType() != "V" and prior.getType() != "StatePoint3d")
+            throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint3d'");
         if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle")
             throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'");
         if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion")
diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp
index 1b18d951b..c3ac836ca 100644
--- a/src/sensor/sensor_odom.cpp
+++ b/src/sensor/sensor_odom.cpp
@@ -35,22 +35,6 @@ SensorOdom::SensorOdom(const SizeEigen& _dim,
 {
 }
 
-SensorOdom2d::SensorOdom2d(const SizeEigen& _dim,
-                           ParamsSensorOdomPtr _params,
-                           const Priors& _priors) :
-        SensorOdom(_dim,_params,_priors)
-{
-    assert(_dim == 2);
-}
-
-SensorOdom3d::SensorOdom3d(const SizeEigen& _dim,
-                           ParamsSensorOdomPtr _params,
-                           const Priors& _priors) :
-        SensorOdom(_dim,_params,_priors)
-{
-    assert(_dim == 3);
-}
-
 Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const
 {
     double d; // displacement
@@ -101,6 +85,6 @@ Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const
 // Register in the FactorySensor
 #include "core/sensor/factory_sensor.h"
 namespace wolf {
-WOLF_REGISTER_SENSOR(SensorOdom2d);
-WOLF_REGISTER_SENSOR(SensorOdom3d);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom2d, SensorOdom);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom3d, SensorOdom);
 } // namespace wolf
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 098855b99..40fcb72e3 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -26,6 +26,20 @@
 
 namespace wolf
 {
+
+Priors::Priors(const YAML::Node& priors_node)
+{
+    WOLF_INFO("Priors: \n", priors_node);
+    if (not priors_node.IsMap())
+    {
+        throw std::runtime_error("Priors: constructor with a non-map yaml node");
+    }
+    for (auto prior_pair : priors_node)
+    {
+        this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second));
+    }
+}
+
 Prior::Prior(const std::string&     _type,
              const Eigen::VectorXd& _state,
              const std::string&     _mode,
@@ -42,7 +56,6 @@ Prior::Prior(const YAML::Node& prior_node)
     type_  = prior_node["type"].as<std::string>();
     state_ = prior_node["state"].as<Eigen::VectorXd>();
     mode_  = prior_node["mode"].as<std::string>();
-
     if (mode_ == "factor") 
       noise_std_ = prior_node["noise_std"].as<Eigen::VectorXd>();
     else 
@@ -86,12 +99,12 @@ void Prior::check() const
     if (state_.size() != sb->getSize())
         throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of StateBlock size. " + print());
 
-    // check sigma size
+    // check factor sigma size
     if (mode_ == "factor" and noise_std_.size() != sb->getLocalSize())
         throw std::runtime_error("Prior::check() Prior " + type_ + " noise_std size different of StateBlock local size. " +
                                  print());
 
-    // check sigma size
+    // check drift sigma size
     if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize())
         throw std::runtime_error("Prior::check() Prior " + type_ +
                                  " drift_std size different of StateBlock local size. " + print());
diff --git a/test/dummy/SensorDummy.schema b/test/dummy/SensorDummy2d.schema
similarity index 100%
rename from test/dummy/SensorDummy.schema
rename to test/dummy/SensorDummy2d.schema
diff --git a/test/dummy/SensorDummy3d.schema b/test/dummy/SensorDummy3d.schema
new file mode 100644
index 000000000..941cb53d0
--- /dev/null
+++ b/test/dummy/SensorDummy3d.schema
@@ -0,0 +1,16 @@
+follow: SensorBase.schema
+states:
+  P:
+    follow: PriorP3d.schema
+  O:
+    follow: PriorO3d.schema
+noise_p_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in P.
+noise_o_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in O.
\ No newline at end of file
diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema
new file mode 100644
index 000000000..ac24447e5
--- /dev/null
+++ b/test/dummy/SensorDummyPoia3d.schema
@@ -0,0 +1,44 @@
+follow: SensorBase.schema
+states:
+  P:
+    follow: PriorP3d.schema
+  O:
+    follow: PriorO3d.schema
+  I:
+    type:
+      type: string
+      yaml_type: scalar
+      mandatory: false
+      default: StateParam5
+      options: [StateParam5]
+      doc: The derived type of the state in 'I'
+    state:
+      type: Vector5d
+      yaml_type: scalar
+      mandatory: true
+      doc: A vector containing the state 'I' values
+    follow: PriorModeDynamicNoiseDrift.schema
+  A:
+    type:
+      type: string
+      yaml_type: scalar
+      mandatory: false
+      default: StateQuaternion
+      options: [StateQuaternion]
+      doc: The derived type of the state in 'A'
+    state:
+      type: Vector4d
+      yaml_type: scalar
+      mandatory: true
+      doc: A vector containing the state 'A' values
+    follow: PriorModeDynamicNoiseDrift.schema
+noise_p_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in p.
+noise_o_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in o.
\ No newline at end of file
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index 96e8b5777..23ba2a36c 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy.h
@@ -86,5 +86,8 @@ class SensorDummy : public SensorBase
 // Register in the FactorySensor
 #include "core/sensor/factory_sensor.h"
 namespace wolf {
-WOLF_REGISTER_SENSOR(SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy2d, SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy3d, SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia2d, SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia3d, SensorDummy);
 } // namespace wolf
\ No newline at end of file
diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h
deleted file mode 100644
index f81eca9ef..000000000
--- a/test/dummy/sensor_dummy_poia.h
+++ /dev/null
@@ -1,87 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#pragma once
-
-/**************************
- *      WOLF includes     *
- **************************/
-#include "core/sensor/sensor_base.h"
-#include "sensor_dummy.h"
-
-namespace wolf {
-
-// SpecStates specs_states_dummy_poia({{'I',SpecState("StateBlock", 5, "some doc for state I")},
-                                    // {'A',SpecState("StateQuaternion", 4, "some doc for state A")}});
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummyPoia);
-
-struct ParamsSensorDummyPoia : public ParamsSensorDummy
-{
-    ParamsSensorDummyPoia() = default;    
-    ParamsSensorDummyPoia(const YAML::Node& _n):
-        ParamsSensorDummy(_n)
-    {
-    }
-    ~ParamsSensorDummyPoia() override = default;
-    std::string print() const override
-    {
-        return ParamsSensorDummy::print();
-    }
-};
-
-WOLF_PTR_TYPEDEFS(SensorDummyPoia);
-
-class SensorDummyPoia : public SensorBase
-{
-    private:
-        ParamsSensorDummyPoiaPtr params_dummy_poia_;
-        SizeEigen dim_;
-
-    public:
-        SensorDummyPoia(SizeEigen _dim, 
-                        ParamsSensorDummyPoiaPtr _params,
-                        const Priors& _priors) :
-            SensorBase("SensorDummyPoia",
-                       _dim,
-                       _params,
-                       _priors),
-            params_dummy_poia_(_params)
-        {
-        }
-        WOLF_SENSOR_CREATE(SensorDummyPoia, ParamsSensorDummyPoia);
-
-        virtual ~SensorDummyPoia(){};
-
-        Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override
-        {
-            return (Eigen::Vector2d() << params_dummy_poia_->noise_p_std,
-                                         params_dummy_poia_->noise_o_std).finished().cwiseAbs2().asDiagonal();
-        }
-};
-
-}
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR(SensorDummyPoia);
-} // namespace wolf
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index e71a0a1e7..fa60723ff 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -66,7 +66,7 @@ TEST(ProcessorBase, MotionProvider)
                                            {wolf_root});
     auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
     proc_trk_params->name = "proc tracker";
-    auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", sens_trk, proc_trk_params);
+    auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
 
     // Install odometer (sensor and processor)
     auto sens_odo = problem->installSensor("SensorOdom",
@@ -75,9 +75,7 @@ TEST(ProcessorBase, MotionProvider)
     auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
     proc_odo_params->time_tolerance = dt/2;
     proc_odo_params->name = "odom processor";
-    auto proc_odo = problem->installProcessor("ProcessorOdom2d",
-                                              sens_odo,
-                                              proc_odo_params);
+    auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
 
     ASSERT_FALSE(proc_trk->isMotionProvider());
     ASSERT_TRUE (proc_odo->isMotionProvider());
@@ -106,7 +104,7 @@ TEST(ProcessorBase, KeyFrameCallback)
                                            {wolf_root});
     auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
     proc_trk_params->name = "proc tracker";
-    auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", sens_trk, proc_trk_params);
+    auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
 
     // Install odometer (sensor and processor)
     auto sens_odo = problem->installSensor("SensorOdom",
@@ -115,9 +113,7 @@ TEST(ProcessorBase, KeyFrameCallback)
     auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
     proc_odo_params->time_tolerance = dt/2;
     proc_odo_params->name = "odom processor";
-    auto proc_odo = problem->installProcessor("ProcessorOdom2d",
-                                              sens_odo,
-                                              proc_odo_params);
+    auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
 
     std::cout << "sensor & processor created and added to wolf problem" << std::endl;
 
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 2154adcb7..0771d6b26 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -23,7 +23,6 @@
 #include "core/sensor/sensor_base.h"
 #include "core/utils/utils_gtest.h"
 #include "dummy/sensor_dummy.h"
-#include "dummy/sensor_dummy_poia.h"
 #include "yaml-schema-cpp/yaml_server.hpp"
 
 using namespace wolf;
@@ -326,18 +325,18 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
 // 3D POIA mixed
 TEST(SensorBase, makeshared_priors_POIA_mixed)
 {
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
+  auto params = std::make_shared<ParamsSensorDummy>();
   params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   VectorXd i_state_3D = VectorXd::Random(5);
 
-  auto S = std::make_shared<SensorDummyPoia>(3, 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")},
-                                                     {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
+  auto S = std::make_shared<SensorDummy>(3, 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")},
+                                                 {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
      
   // noise
   ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -352,41 +351,6 @@ TEST(SensorBase, makeshared_priors_POIA_mixed)
   checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
 }
 
-// 3D POIA wrong
-TEST(SensorBase, makeshared_priors_POIA_wrong)
-{
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
-  params->name = "sensor_1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  VectorXd i_state_3D = VectorXd::Random(5);
-
-  // missing I
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, 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")},
-                                                         {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})),
-               std::runtime_error);
-
-  // missing A
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, 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")},
-                                                         /*{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}*/})),
-               std::runtime_error);
-
-  // wrong A type (expected StateQuaternion)
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, 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")},
-                                                         {'A',Prior("StateBlock", p_state_3D, "factor", p_std_3D)}})),
-               std::runtime_error);
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////// FactorySensor ///////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -428,12 +392,15 @@ TEST(SensorBase, factory)
 
             WOLF_INFO("Creating sensor from ", name, ".yaml");
 
-            ASSERT_EQ(server.applySchema("SensorDummy"), not wrong);
+            auto valid = server.applySchema("SensorDummy"+toString(dim)+"d");
+            WOLF_WARN_COND(not valid and not wrong, server.getLog().str());
 
             // CORRECT YAML
             if (not wrong)
             {
-              auto S = FactorySensor::create("SensorDummy", dim, server.getNode());
+              ASSERT_TRUE(valid);
+
+              auto S = FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode());
 
               auto p_size = dim;
               auto o_size = dim == 2 ? 1 : 4;
@@ -444,8 +411,8 @@ TEST(SensorBase, factory)
 
               // noise
               ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
-                                  noise_cov_dummy, 
-                                  Constants::EPS);
+                                   noise_cov_dummy, 
+                                   Constants::EPS);
 
               // factors
               ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
@@ -457,7 +424,11 @@ TEST(SensorBase, factory)
             // INCORRECT YAML
             else
             {
-              ASSERT_THROW(FactorySensor::create("SensorDummy", dim, server.getNode()),std::runtime_error);
+              // either is not valid (schema) or it throws an error
+              if (valid)
+              {
+                ASSERT_THROW(FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode()),std::runtime_error);
+              }
             }
           }
 
@@ -469,7 +440,7 @@ TEST(SensorBase, factory)
     YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml");
 
     // create sensor
-    auto S = FactorySensor::create("SensorDummyPoia", 3, server.getNode());
+    auto S = FactorySensor::create("SensorDummyPoia3d", 3, server.getNode());
 
     // noise
     ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
@@ -493,7 +464,7 @@ TEST(SensorBase, factory)
     YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml");
 
     // create sensor
-    ASSERT_THROW(FactorySensor::create("SensorDummyPoia", 3, server.getNode()),std::runtime_error);
+    ASSERT_THROW(FactorySensor::create("SensorDummyPoia3d", 3, server.getNode()),std::runtime_error);
   }
 }
 
@@ -542,7 +513,7 @@ TEST(SensorBase, factory_yaml)
             // CORRECT YAML
             if (not wrong)
             {
-              auto S = FactorySensorYaml::create("SensorDummy", dim, yaml_filepath, {wolf_root});
+              auto S = FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root});
 
               auto p_size = dim;
               auto o_size = dim == 2 ? 1 : 4;
@@ -566,7 +537,7 @@ TEST(SensorBase, factory_yaml)
             // INCORRECT YAML
             else
             {
-              ASSERT_THROW(FactorySensorYaml::create("SensorDummy", dim, yaml_filepath, {wolf_root}),std::runtime_error);
+              ASSERT_THROW(FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root}),std::runtime_error);
             }
           }
 
@@ -575,7 +546,7 @@ TEST(SensorBase, factory_yaml)
     WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
 
     // create sensor
-    auto S = FactorySensorYaml::create("SensorDummyPoia",
+    auto S = FactorySensorYaml::create("SensorDummy3d",
                                        3,
                                        wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml",
                                        {wolf_root});
@@ -599,7 +570,7 @@ TEST(SensorBase, factory_yaml)
     WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml");
 
     // create sensor
-    ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia",
+    ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia3d",
                                            3,
                                            wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml",
                                            {wolf_root}),
@@ -607,301 +578,6 @@ TEST(SensorBase, factory_yaml)
   }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////// FactorySensorPriors ////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// 2D Fix
-TEST(SensorBase, factory_priors_POfix2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_2D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // checks
-  checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
-}
-
-// 3D Fix
-TEST(SensorBase, factory_priors_POfix3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_3D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
-}
-
-// 2D Initial guess
-TEST(SensorBase, factory_priors_POinitial_guess2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
-                                               {'O',Prior("O", o_state_2D, "initial_guess")}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
-}
-
-// 3D Initial guess
-TEST(SensorBase, factory_priors_POinitial_guess3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
-                                               {'O',Prior("O", o_state_3D, "initial_guess")}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
-}
-
-// 2D Factor
-TEST(SensorBase, factory_priors_POfactor2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
-}
-
-// 3D Factor
-TEST(SensorBase, factory_priors_POfactor3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 2D Initial guess dynamic
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
-}
-
-// 3D Initial guess dynamic
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
-}
-
-// 2D Initial guess dynamic drift
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D_drift)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
-}
-
-// 3D Initial guess dynamic drift
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D_drift)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
-}
-
-// 3D POIA mixed
-TEST(SensorBase, factory_priors_POIA_mixed)
-{
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  VectorXd i_state_3D = VectorXd::Random(5);
-
-  auto S = FactorySensorPriors::create("SensorDummyPoia", 3, 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")},
-                                               {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
-     
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, true, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_3D, false, o_std_3D, true, o_std_3D);
-  checkSensor(S, 'I', i_state_3D, false, vector0, false, vector0);
-  checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 3D POIA wrong
-TEST(SensorBase, factory_priors_POIA_wrong)
-{
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  VectorXd i_state_3D = VectorXd::Random(5);
-
-  // missing I
-  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia", 3, 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")},
-                                                   {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})),
-               std::runtime_error);
-
-  // missing A
-  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia", 3, 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")},
-                                                   /*{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}*/})),
-               std::runtime_error);
-
-  // wrong A type (expected StateQuaternion)
-  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia", 3, 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")},
-                                                   {'A',Prior("StateBlock", p_state_3D, "factor", p_std_3D)}})),
-               std::runtime_error);
-}
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index f3349edfb..23e6b4604 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -316,77 +316,6 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_3D)
   checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES ////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST(SensorOdom, makeshared_server)
-{
-  std::vector<int> dims({2, 3});
-  std::vector<std::string> modes({"fix", "initial_guess", "factor"});
-  std::vector<bool> dynamics({false, true});
-  std::vector<bool> drifts({false, true});
-  std::vector<bool> wrongs({false, true});
-
-  VectorXd p_state(4), o_state(4), po_std(4);
-  p_state << 1, 2, 3, 4;
-  o_state << 1, 0, 0, 0;
-  po_std << 0.1, 0.2, 0.3, 0.4;
-
-  // P & O
-  for (auto dim : dims)
-    for (auto mode : modes)
-      for (auto dynamic : dynamics)
-        for (auto drift : drifts)
-          for (auto wrong : wrongs)
-          {
-            // nonsense combination
-            if (not dynamic and drift)
-              continue;
-
-            std::string name = "sensor_PO_" + 
-                               toString(dim) + 
-                               "D_" + 
-                               mode + 
-                               (dynamic ? "_dynamic" : "") + 
-                               (drift ? "_drift" : "") + 
-                               (wrong ? "_wrong" : "");
-
-            // Yaml parser
-            ParserYaml parser   = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true);
-            ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
-            WOLF_INFO("Creating sensor from ", name, ".yaml");
-
-            // CORRECT YAML
-            if (not wrong)
-            {
-              auto params = std::make_shared<ParamsSensorOdom>("sensor_1", server);
-              auto S = std::make_shared<SensorOdom>("sensor_1", dim, params, server);
-
-              auto p_size = dim;
-              auto o_size = dim == 2 ? 1 : 4;
-              auto p_size_std = mode == "factor" ? dim : 0;
-              auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
-              auto p_size_std_drift = drift ? dim : 0;
-              auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
-
-              // factors
-              ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
-
-              // check
-              checkSensor(S, 'P', p_state.head(p_size), mode == "fix", po_std.head(p_size_std), dynamic, po_std.head(p_size_std_drift));
-              checkSensor(S, 'O', o_state.head(o_size), mode == "fix", po_std.head(o_size_std), dynamic, po_std.head(o_size_std_drift));
-            }
-            // INCORRECT YAML
-            else
-            {
-              ASSERT_THROW(std::make_shared<SensorOdom>("sensor_1", dim, 
-                                                         std::make_shared<ParamsSensorOdom>("sensor_1", server),
-                                                         server),std::runtime_error);
-            }
-          }
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////// FactorySensor ///////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -521,227 +450,6 @@ TEST(SensorOdom, factory_yaml)
           }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////// FactorySensorPriors ////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// 2D Fix
-TEST(SensorOdom, factory_priors_fix_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_2D)}}));
-
-  // checks
-  checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
-}
-
-// 3D Fix
-TEST(SensorOdom, factory_priors_fix_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_3D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
-}
-
-// 2D Initial guess
-TEST(SensorOdom, factory_priors_initial_guess_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
-                                               {'O',Prior("O", o_state_2D, "initial_guess")}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
-}
-
-// 3D Initial guess
-TEST(SensorOdom, factory_priors_initial_guess_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
-                                               {'O',Prior("O", o_state_3D, "initial_guess")}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
-}
-
-// 2D Factor
-TEST(SensorOdom, factory_priors_factor_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
-}
-
-// 3D Factor
-TEST(SensorOdom, factory_priors_factor_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 2D Initial guess dynamic
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
-}
-
-// 3D Initial guess dynamic
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
-}
-
-// 2D Initial guess dynamic drift
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
-}
-
-// 3D Initial guess dynamic drift
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////// COMPUTE NOISE COV /////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 0c3f031eb..463b82987 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -44,22 +44,6 @@ TEST(TreeManager, make_shared)
     ASSERT_EQ(P->getTreeManager(), GM);
 }
 
-TEST(TreeManager, createParams)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-
-    auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
-    auto GM = TreeManagerDummy::create(ParamsGM);
-
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
-
-    P->setTreeManager(GM);
-
-    ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
-    ASSERT_EQ(P->getTreeManager(), GM);
-}
-
 TEST(TreeManager, createNode)
 {
     ProblemPtr P = Problem::create("PO", 2);
@@ -92,22 +76,6 @@ TEST(TreeManager, createYaml)
     ASSERT_EQ(P->getTreeManager(), GM);
 }
 
-TEST(TreeManager, FactoryParams)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-
-    auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
-    auto GM = FactoryTreeManagerParams::create("TreeManagerDummy",ParamsGM);
-
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
-
-    P->setTreeManager(GM);
-
-    ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
-    ASSERT_EQ(P->getTreeManager(), GM);
-}
-
 TEST(TreeManager, FactoryParam)
 {
     ProblemPtr P = Problem::create("PO", 2);
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D.yaml b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
index abe471720..b2530ce9a 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_POIA_3D:
 states:
   P:
@@ -21,8 +23,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
index 5cf6bffb4..9eec9a201 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_POIA_3D_wrong:
 states:
   P:
@@ -21,7 +23,6 @@ states:
   #   dynamic: true
   #   drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
index 9bfc63bcd..96c51ec98 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
index ce4287ed5..aa801e760 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_dynamic:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
index d4c05d48d..4c703e7da 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_dynamic_drift:
 states:
   P:
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
index a6787204c..a897b3566 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 #  sensor_PO_2D_factor_dynamic_drift_wrong:
 states:
   P:
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
index c66c96442..0f6c0630f 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_dynamic_wrong:
 states:
   P:
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
index b99691f90..1a6591ae9 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
index e96ee1849..296fd0bc3 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
index 715da540e..74f8fd2fb 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
index 4e6680029..2fcf456a4 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
index 0bcee886c..0d257867a 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
index 1c9d81679..381414ba6 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
index 229f89094..8f7b6cd33 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
index f593fc53c..bffef6ad2 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
index 50705368a..0ceb01c8a 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
index d5060eeaf..faeceb548 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
index 9a53f8300..615a03f76 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
index 9b3da538f..0f93bc7ba 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic_wrong:
 states:
   # P: #missing
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
index 65d170426..d2a1cd57a 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
index 23737df98..d1b61af3b 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
index 8c9342ab8..ccd18fe7a 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
index 9f7e14d04..c6840055a 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic_drift:
 states:
   P:
@@ -13,7 +15,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
index a4980b27d..28c0b1745 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic_drift_wrong:
 states:
   P:
@@ -13,7 +15,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
index 104237208..ec9ab37c1 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
index d99e58074..f8440b8c7 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
index 44eef97f1..cd91be67d 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
index 606751a7b..52ea8fc40 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
index f6166c91b..4864053e6 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
index 17467074e..3536ded54 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     #dynamic: true #missing
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
index 4e6da9d21..f7cd8d862 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 1] # not normalized
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
index acc563566..23aae62a0 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 # noise_o_std: 0.01 #missing
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
index 7b5e1398d..86488c0b2 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
index e758e8703..1a0c2d774 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
index 1de0b8d93..1991d7089 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
index 5322cd09f..642749806 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
index 873cf7425..ee02d6607 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
index 5eed19d62..8d74c207d 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
   #   state: [1, 0, 0, 0]
   #   dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
-- 
GitLab