From 9b799315e4da9098203485f626be04c2aa8557ef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 27 Jul 2022 15:50:35 +0200
Subject: [PATCH] finally working everything

---
 demos/hello_wolf/hello_wolf.cpp               |  41 +++---
 demos/hello_wolf/hello_wolf_autoconf.cpp      |  19 +--
 .../schema/ProcessorRangeBearing.schema       |   1 +
 .../schema/SensorRangeBearing.schema          |  19 +++
 demos/hello_wolf/sensor_range_bearing.cpp     |   6 +-
 demos/hello_wolf/sensor_range_bearing.h       |   3 +-
 demos/hello_wolf/yaml/hello_wolf_config.yaml  | 105 +++++++--------
 demos/hello_wolf/yaml/sensor_odom_2d.yaml     |   3 +
 include/core/capture/capture_base.h           |   2 +
 include/core/problem/problem.h                |  31 ++---
 include/core/sensor/sensor_base.h             |   4 +-
 include/core/state_block/has_state_blocks.h   |   3 +-
 schema/none.schema                            |   6 +
 schema/processor/ProcessorOdom2d.schema       |   7 +
 src/capture/capture_base.cpp                  |   5 +
 src/frame/frame_base.cpp                      |   2 +-
 src/landmark/landmark_base.cpp                |   2 +-
 src/problem/problem.cpp                       | 126 +++++++-----------
 src/sensor/sensor_base.cpp                    |   5 +
 test/gtest_odom_2d.cpp                        |  12 +-
 test/gtest_problem.cpp                        |   2 +-
 21 files changed, 213 insertions(+), 191 deletions(-)
 create mode 100644 demos/hello_wolf/schema/ProcessorRangeBearing.schema
 create mode 100644 demos/hello_wolf/schema/SensorRangeBearing.schema
 create mode 100644 schema/none.schema
 create mode 100644 schema/processor/ProcessorOdom2d.schema

diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index b64421fd8..6eb046afc 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -132,7 +132,9 @@ int main()
     ParamsSensorOdomPtr intrinsics_odo      = std::make_shared<ParamsSensorOdom>();
     SpecSensorComposite priors_odo          = {{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())},
                                                {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}};
-    SensorBasePtr sensor_odo                = problem->installSensor("SensorOdom", "sensor odo", intrinsics_odo, priors_odo);
+    SensorBasePtr sensor_odo                = SensorBase::emplace<SensorOdom2d>(problem->getHardware(), 
+                                                                                intrinsics_odo, 
+                                                                                priors_odo);
 
     // processor odometer 2d
     ParamsProcessorOdom2dPtr params_odo     = std::make_shared<ParamsProcessorOdom2d>();
@@ -143,29 +145,35 @@ int main()
     params_odo->angle_turned                = 999;
     params_odo->cov_det                     = 999;
     params_odo->unmeasured_perturbation_std = 0.001;
-    ProcessorBasePtr processor              = problem->installProcessor("ProcessorOdom2d", "processor odo", sensor_odo, params_odo);
+    ProcessorBasePtr processor              = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odo, params_odo);
 
     // sensor Range and Bearing
     ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
-    SpecSensorComposite priors_rb                          = {{'P',SpecStateSensor("P", Vector2d(1,1))},
-                                                 {'O',SpecStateSensor("O", Vector1d::Zero())}};
+    SpecSensorComposite priors_rb             = {{'P',SpecStateSensor("StatePoint2d", Vector2d(1,1))},
+                                                 {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}};
     intrinsics_rb->noise_range_metres_std     = 0.1;
     intrinsics_rb->noise_bearing_degrees_std  = 1.0;
-    SensorBasePtr sensor_rb                   = problem->installSensor("SensorRangeBearing", "sensor RB", intrinsics_rb, priors_rb);
+    SensorBasePtr sensor_rb                   = SensorBase::emplace<SensorRangeBearing>(problem->getHardware(), 
+                                                                                        intrinsics_rb, 
+                                                                                        priors_rb);
 
     // processor Range and Bearing
     ParamsProcessorRangeBearingPtr params_rb = std::make_shared<ParamsProcessorRangeBearing>();
     params_rb->voting_active                = false;
     params_rb->time_tolerance               = 0.01;
-    ProcessorBasePtr processor_rb           = problem->installProcessor("ProcessorRangeBearing", "processor RB", sensor_rb, params_rb);
+    ProcessorBasePtr processor_rb           = ProcessorBase::emplace<ProcessorRangeBearing>(sensor_rb, params_rb);
 
     // initialize
-    TimeStamp   t(0.0);                          // t : 0.0
-    // Vector3d    x(0,0,0);
-    VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
-    // Matrix3d    P = Matrix3d::Identity() * 0.1;
-    VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
-    FrameBasePtr KF1 = problem->setPriorFactor(x, P, t);             // KF1 : (0,0,0)
+    TimeStamp   t(0.0);                              // t : 0.0
+    SpecComposite prior;
+    prior.emplace('P',SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(sqrt(0.1))));
+    prior.emplace('O',SpecState("StateAngle",   Vector1d::Zero(), "factor", Vector1d::Constant(sqrt(0.1))));
+    FrameBasePtr KF1 = problem->setPrior(prior, t);  // KF1 : (0,0,0)
+    // // Vector3d    x(0,0,0);
+    // VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+    // // Matrix3d    P = Matrix3d::Identity() * 0.1;
+    // VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+    // FrameBasePtr KF1 = problem->setPriorFactor(x, P, t);             // KF1 : (0,0,0)
     std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
 
     // SELF CALIBRATION ===================================================
@@ -256,17 +264,20 @@ int main()
 
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
-    ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    bool succeed = ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    WOLF_ERROR_COND(not succeed, "Covariances could not be computed.");
     for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
     {
         Eigen::MatrixXd cov;
-        kf_pair.second->getCovariance(cov);
+        succeed = kf_pair.second->getCovariance("PO", cov);
+        WOLF_ERROR_COND(not succeed, "Covariance matrix for frame ", kf_pair.second->id(), " could not be recovered.");
         WOLF_TRACE("KF", kf_pair.second->id(), "_cov = \n", cov);
     }
     for (auto& lmk : problem->getMap()->getLandmarkList())
     {
         Eigen::MatrixXd cov;
-        lmk->getCovariance(cov);
+        succeed = lmk->getCovariance("P", cov);
+        WOLF_ERROR_COND(not succeed, "Covariance matrix for landmark ", lmk->id(), " could not be recovered.");
         WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
     }
     std::cout << std::endl;
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 13e92f792..b8cc00e1e 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -33,7 +33,6 @@
 #include "core/common/wolf.h"
 #include "core/capture/capture_odom_2d.h"
 #include "core/processor/processor_motion.h"
-#include "core/yaml/parser_yaml.h"
 #include "capture_range_bearing.h"
 
 
@@ -123,16 +122,20 @@ int main()
     std::string wolf_path   = _WOLF_ROOT_DIR;
 
     // parse file into params server: each param will be retrievable from this params server:
-    ParserYaml parser       = ParserYaml(wolf_path + "/" + config_file);
-    ParamsServer server     = ParamsServer(parser.getParams());
+    yaml_schema_cpp::YamlServer server({wolf_path}, wolf_path + "/" + config_file);
+    if (not server.applySchema("Problem2d"))
+    {
+        WOLF_ERROR(server.getLog());
+        throw std::runtime_error("Couldn't load and validate the yaml file " + config_file);
+    }
     // Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
-    ProblemPtr problem      = Problem::autoSetup(server);
-
+    ProblemPtr problem      = Problem::autoSetup(server.getNode());
+    
     // Print problem to see its status before processing any sensor data
     problem->print(4,0,1,0);
 
     // Solver. Configure a Ceres solver
-    SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
+    SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server.getNode()["solver"]);
 
     // recover sensor pointers and other stuff for later use (access by sensor name)
     SensorBasePtr sensor_odo    = problem->findSensor("sen odom");
@@ -243,13 +246,13 @@ int main()
     for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
     {
         Eigen::MatrixXd cov;
-        kf_pair.second->getCovariance(cov);
+        kf_pair.second->getCovariance("PO", cov);
         WOLF_INFO("KF", kf_pair.second->id(), "_cov = \n", cov);
     }
     for (auto& lmk : problem->getMap()->getLandmarkList())
     {
         Eigen::MatrixXd cov;
-        lmk->getCovariance(cov);
+        lmk->getCovariance("P", cov);
         WOLF_INFO("L", lmk->id(), "_cov = \n", cov);
     }
     std::cout << std::endl;
diff --git a/demos/hello_wolf/schema/ProcessorRangeBearing.schema b/demos/hello_wolf/schema/ProcessorRangeBearing.schema
new file mode 100644
index 000000000..3c97f7469
--- /dev/null
+++ b/demos/hello_wolf/schema/ProcessorRangeBearing.schema
@@ -0,0 +1 @@
+follow: ProcessorBase.schema
\ No newline at end of file
diff --git a/demos/hello_wolf/schema/SensorRangeBearing.schema b/demos/hello_wolf/schema/SensorRangeBearing.schema
new file mode 100644
index 000000000..bc40eef77
--- /dev/null
+++ b/demos/hello_wolf/schema/SensorRangeBearing.schema
@@ -0,0 +1,19 @@
+follow: SensorBase.schema
+
+noise_range_metres_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: Standard deviation of the noise of the range measurements (meters)
+
+noise_bearing_degrees_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: Standard deviation of the noise of the bearing measurements (degrees)
+
+states:
+  P:
+    follow: SpecStateSensorP2d.schema
+  O:
+    follow: SpecStateSensorO2d.schema
\ No newline at end of file
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 0738c4418..b7d62c95d 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -25,13 +25,11 @@ namespace wolf{
 
 WOLF_PTR_TYPEDEFS(SensorRangeBearing);
 
-SensorRangeBearing::SensorRangeBearing(const SizeEigen& _dim,
-                                       ParamsSensorRangeBearingPtr _params,
+SensorRangeBearing::SensorRangeBearing(ParamsSensorRangeBearingPtr _params,
                                        const SpecSensorComposite& _priors) :
-        SensorBase("SensorRangeBearing", _dim, _params, _priors),
+        SensorBase("SensorRangeBearing", 2, _params, _priors("PO")),
         params_rb_(_params)
 {
-    assert(_dim == 2 && "SensorRangeBearing only for 2D");
 }
 
 SensorRangeBearing::~SensorRangeBearing()
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 75d6e7731..69e0ea53c 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -56,8 +56,7 @@ class SensorRangeBearing : public SensorBase
         ParamsSensorRangeBearingPtr params_rb_;
 
     public:
-        SensorRangeBearing(const SizeEigen& _dim,
-                           ParamsSensorRangeBearingPtr _params,
+        SensorRangeBearing(ParamsSensorRangeBearingPtr _params,
                            const SpecSensorComposite& _priors);
         WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing);
         
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 33889188f..267616859 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -1,59 +1,56 @@
-config:
+problem:
 
-  problem:
-  
-    dimension:            2               # space is 2d
-    frame_structure:      "PO"            # keyframes have position and orientation
-  
-    first_frame:
+  dimension:            2               # space is 2d
+  frame_structure:      "PO"            # keyframes have position and orientation
+
+  first_frame:
+    P:
+      mode:               "factor"
+      state: [0,0]
+      noise_std: [0.31, 0.31]
+    O:
       mode:               "factor"
-      $state:
-        P: [0,0]
-        O: [0]
-      $sigma:
-        P: [0.31, 0.31]
-        O: [0.31]
-      time_tolerance:     0.5
+      state: [0]
+      noise_std: [0.31]
 
-    tree_manager:
-      type: "none"
-      plugin: "core"
+  tree_manager:
+    type: "none"
+
+solver:
+  max_num_iterations: 100
+  verbose: 0
+  period: 0.2
+  interrupt_on_problem_change: false
+  n_threads: 2
+  compute_cov: false
+  minimizer: LEVENBERG_MARQUARDT
+  use_nonmonotonic_steps: false         # only for LEVENBERG_MARQUARDT and DOGLEG
+  function_tolerance: 0.000001
+  gradient_tolerance: 0.0000000001
+  
+sensors:
   
-  solver:
-    max_num_iterations: 100
-    verbose: 0
-    period: 0.2
-    interrupt_on_problem_change: false
-    n_threads: 2
-    compute_cov: false
-    minimizer: LEVENBERG_MARQUARDT
-    use_nonmonotonic_steps: false         # only for LEVENBERG_MARQUARDT and DOGLEG
-    function_tolerance: 0.000001
-    gradient_tolerance: 0.0000000001
-    
-  sensors:
-    
-    - type:               "SensorOdom2d"
-      plugin:             "core"
-      name:               "sen odom"
-      follow:             "demos/hello_wolf/yaml/sensor_odom_2d.yaml"         # config parameters in this file
+  - type:               "SensorOdom2d"
+    plugin:             "core"
+    name:               "sen odom"
+    follow:             "sensor_odom_2d.yaml"         # config parameters in this file
 
-    - type:               "SensorRangeBearing"
-      plugin:             "core"
-      name:               "sen rb"  
-      noise_range_metres_std: 0.2                                       # parser only considers first appearence so the following file parsing will not overwrite this param
-      follow:             demos/hello_wolf/yaml/sensor_range_bearing.yaml     # config parameters in this file
-          
-  processors:
-    
-    - type:               "ProcessorOdom2d"
-      plugin:             "core"
-      name:               "prc odom"
-      sensor_name:        "sen odom"                                    # attach processor to this sensor
-      follow:             demos/hello_wolf/yaml/processor_odom_2d.yaml        # config parameters in this file
-    
-    - type:               "ProcessorRangeBearing"
-      plugin:             "core"
-      name:               "prc rb"
-      sensor_name:        "sen rb"                                      # attach processor to this sensor
-      follow:             demos/hello_wolf/yaml/processor_range_bearing.yaml  # config parameters in this file
+  - type:               "SensorRangeBearing"
+    plugin:             "core"
+    name:               "sen rb"  
+    noise_range_metres_std: 0.2                       # parser only considers first appearence so the following file parsing will not overwrite this param
+    follow:             sensor_range_bearing.yaml     # config parameters in this file
+        
+processors:
+  
+  - type:               "ProcessorOdom2d"
+    plugin:             "core"
+    name:               "prc odom"
+    sensor_name:        "sen odom"                    # attach processor to this sensor
+    follow:             processor_odom_2d.yaml        # config parameters in this file
+  
+  - type:               "ProcessorRangeBearing"
+    plugin:             "core"
+    name:               "prc rb"
+    sensor_name:        "sen rb"                      # attach processor to this sensor
+    follow:             processor_range_bearing.yaml  # config parameters in this file
diff --git a/demos/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
index 1480abfef..260829e09 100644
--- a/demos/hello_wolf/yaml/sensor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -2,6 +2,9 @@
 
 k_disp_to_disp:   0.1  # m^2   / m
 k_rot_to_rot:     0.1  # rad^2 / rad
+k_disp_to_rot:    0.0  # m^2   / rad
+min_disp_var:     0.0
+min_rot_var:      0.0
 apply_loss_function: true
 
 states:
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index b7f24d4a0..31092a824 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -119,6 +119,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         StateBlockConstPtr  getSensorIntrinsic() const;
         StateBlockPtr       getSensorIntrinsic();
 
+        bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
+
         void fix() override;
         void unfix() override;
 
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 33cc7d029..45aef3cff 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -39,6 +39,7 @@ class Loader;
 #include "core/common/wolf.h"
 #include "core/frame/frame_base.h"
 #include "core/state_block/state_block.h"
+#include "core/state_block/has_state_blocks.h"
 #include "core/state_block/spec_composite.h"
 #include "core/state_block/vector_composite.h"
 #include "core/processor/motion_provider.h"
@@ -67,28 +68,32 @@ class Problem : public std::enable_shared_from_this<Problem>
     friend MotionProvider;
 
     protected:
-        TreeManagerBasePtr  tree_manager_;
         HardwareBasePtr     hardware_ptr_;
         TrajectoryBasePtr   trajectory_ptr_;
         MapBasePtr          map_ptr_;
+        TreeManagerBasePtr  tree_manager_;
         std::map<int, MotionProviderPtr>  motion_provider_map_;
-        std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
-        // SizeEigen state_size_, state_cov_size_;
+
         SizeEigen dim_;
+        TypeComposite frame_types_;
+        SpecComposite prior_options_;
+        bool prior_applied_;
+
+
+        std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
+
         std::map<FactorBasePtr, Notification> factor_notification_map_;
         std::map<StateBlockPtr, Notification> state_block_notification_map_;
         mutable std::mutex mut_notifications_;
         mutable std::mutex mut_covariances_;
-        //StateKeys frame_structure_;
-        TypeComposite frame_types_;
-        SpecComposite prior_options_;
-        bool prior_applied_;
 
         std::atomic_bool transformed_;
         VectorComposite  transformation_;
         mutable std::mutex mut_transform_;
 
-      private:  // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
+        std::list<std::shared_ptr<Loader>> loaders_;
+
+    private:  // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
         Problem(const TypeComposite& _frame_structure, SizeEigen _dim); // USE create() below !!
         Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
         void setup();
@@ -100,12 +105,11 @@ class Problem : public std::enable_shared_from_this<Problem>
                                  SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
         static ProblemPtr autoSetup(const std::string& _input_yaml_file, 
                                     const std::vector<std::string>& _primary_schema_folders={});
+        static ProblemPtr autoSetup(YAML::Node _param_node);
         virtual ~Problem();
 
     protected:
-        static void loadPlugin(const std::string& plugin_name, 
-                               const std::string& plugins_path, 
-                               std::list<std::shared_ptr<Loader>>& loaders);
+        void loadPlugin(const std::string& plugin_name);
 
         // Properties -----------------------------------------
     public:
@@ -114,7 +118,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         TypeComposite getFrameTypes(StateKeys _keys = "") const;
 
     protected:
-        //void appendToFrameTypes(const StateKeys& _structure);
         void appendToFrameTypes(const TypeComposite& _structure);
 
 
@@ -335,10 +338,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         bool getCovarianceBlock(StateBlockConstPtr _state1, StateBlockConstPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
         bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
         bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
-        bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
+        bool getCovariance(HasStateBlocksConstPtr _has_states_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
         bool getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
-        bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
-
 
 
         // Solver management ----------------------------------------
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 9cc033cb0..0627f903b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -186,6 +186,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
         StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts);
 
+        bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
+
         // Declare a state block as dynamic or static (with _dymanic = false)
         void setStateBlockDynamic(const char& _key, bool _dynamic = true);
 
@@ -282,7 +284,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
 
 }
 
-#include "core/problem/problem.h"
+// #include "core/problem/problem.h"
 #include "core/hardware/hardware_base.h"
 #include "core/capture/capture_base.h"
 #include "core/processor/processor_base.h"
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 8ea5e94a6..a5c97899f 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -30,6 +30,7 @@
 namespace wolf
 {
 
+WOLF_PTR_TYPEDEFS(HasStateBlocks)
 
 class HasStateBlocks
 {
@@ -72,8 +73,6 @@ class HasStateBlocks
         bool            stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
         StateBlockConstPtr  getStateBlock(const char& _sb_key) const;
         StateBlockPtr       getStateBlock(const char& _sb_key);
-        //std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
-        //std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
 
         // Emplace derived state blocks (angle, quaternion, etc).
         template<typename SB, typename ... Args>
diff --git a/schema/none.schema b/schema/none.schema
new file mode 100644
index 000000000..d3ba158a2
--- /dev/null
+++ b/schema/none.schema
@@ -0,0 +1,6 @@
+type:
+  mandatory: false
+  options: ["none"]
+  type: string
+  yaml_type: scalar
+  doc: Nothing to say here
diff --git a/schema/processor/ProcessorOdom2d.schema b/schema/processor/ProcessorOdom2d.schema
new file mode 100644
index 000000000..15f0939de
--- /dev/null
+++ b/schema/processor/ProcessorOdom2d.schema
@@ -0,0 +1,7 @@
+follow: ProcessorMotion.schema
+keyframe_vote:
+  cov_det:
+    mandatory: true
+    type: double
+    yaml_type: scalar
+    doc: The determinant threshold of the covariance matrix of the integrated delta, to vote for a keyframe.
\ No newline at end of file
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index e6d4a1e66..0eb2cc216 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -218,6 +218,11 @@ StateBlockPtr CaptureBase::getStateBlock(const char& _key)
         return HasStateBlocks::getStateBlock(_key);
 }
 
+bool CaptureBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
+{
+    return getProblem()->getCovariance(shared_from_this(), _keys, _cov);
+}
+
 void CaptureBase::fix()
 {
     HasStateBlocks::fix();
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index fa3c402a9..b4c0ffb24 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -132,7 +132,7 @@ void FrameBase::link(ProblemPtr _prb)
 
 bool FrameBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
 {
-    return getProblem()->getFrameCovariance(shared_from_this(), _keys, _cov);
+    return getProblem()->getCovariance(shared_from_this(), _keys, _cov);
 }
 
 FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index f17dc8769..f697d7482 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -102,7 +102,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
 
 bool LandmarkBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
 {
-    return getProblem()->getLandmarkCovariance(shared_from_this(), _keys, _cov);
+    return getProblem()->getCovariance(shared_from_this(), _keys,_cov);
 }
 
 YAML::Node LandmarkBase::toYaml() const
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6fa2388c1..d5139bd35 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -43,31 +43,33 @@ namespace wolf
 {
 
 Problem::Problem(const TypeComposite& _frame_types, SizeEigen _dim) :
-        tree_manager_(nullptr),
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
         map_ptr_(std::make_shared<MapBase>()),
+        tree_manager_(nullptr),
         motion_provider_map_(),
         dim_(_dim),
         frame_types_(_frame_types),
         prior_options_(),
         prior_applied_(false),
-        transformed_(false)
+        transformed_(false),
+        loaders_()
 {
     checkTypeComposite(frame_types_, dim_);
 }
 
 Problem::Problem(const StateKeys& _frame_keys, SizeEigen _dim) :
-        tree_manager_(nullptr),
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
         map_ptr_(std::make_shared<MapBase>()),
+        tree_manager_(nullptr),
         motion_provider_map_(),
         dim_(_dim),
         frame_types_(),
         prior_options_(),
         prior_applied_(false),
-        transformed_(false)
+        transformed_(false),
+        loaders_()
 {
     for (auto key : _frame_keys)
     {
@@ -114,30 +116,6 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
 
 ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::vector<std::string>& _primary_schema_folders)
 {
-    // Loaders
-    auto loaders = std::list<std::shared_ptr<Loader>>();
-    std::string plugins_path = _WOLF_LIB_DIR;
-    if (plugins_path.back() != '/')
-    {
-        plugins_path += '/';  // only works for UNIX systems
-    }
-    // // get all plugins
-    // auto plugins_nodes = yaml_schema_cpp::findNodesWithKey(server.getNode(),"plugins");
-    // std::set<std::string> plugins;
-    // for (auto plugin_n : plugins_nodes)
-    // {
-    //     if (not plugin_n.IsScalar() and not yaml_schema_cpp::checkType(plugin_n, "string"))
-    //     {
-    //         throw std::runtime_error("Problem::autoSetup: there is one 'plugin' param that is not a string.")
-    //     }
-    //     // plugin string
-    //     auto plugin_str = plugin_n.as<std::string>();
-    //     if (plugins.count(plugin_str) == 0)
-    //     {
-    //         plugins.insert(plugin_str);
-    //     }
-    // }
-
     // Schema folders
     std::vector<std::string> schema_folders = _primary_schema_folders;
     schema_folders.push_back(_WOLF_SCHEMAS_DIR);
@@ -164,12 +142,15 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     WOLF_INFO("schema applied");
 
     // Get param node
-    YAML::Node param_node = server.getNode();
+    return Problem::autoSetup(server.getNode());
+}
 
+ProblemPtr Problem::autoSetup(YAML::Node _param_node)
+{
     // PROBLEM =============================================================================== 
     // structure and dimension
     WOLF_INFO("Loading problem parameters");
-    YAML::Node problem_node = param_node["problem"];
+    YAML::Node problem_node = _param_node["problem"];
     int  dim     = problem_node["dimension"].as<int>();
     TypeComposite frame_types;
     if (problem_node["frame_types"])
@@ -184,6 +165,10 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
         tree_manager_type != "None" and 
         tree_manager_type != "NONE")
     {
+        if (not FactoryTreeManager::isCreatorRegistered(tree_manager_type))
+        {
+            problem->loadPlugin(tree_manager_node["plugin"].as<std::string>());
+        }
         problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node));
     }
 
@@ -197,13 +182,13 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     // SENSORS =============================================================================== 
     // load plugin if sensor is not registered
     WOLF_INFO("Loading sensors parameters");
-    YAML::Node sensors_node = param_node["sensors"];
+    YAML::Node sensors_node = _param_node["sensors"];
     for (auto sensor_n : sensors_node)
     {
         auto sensor_type = sensor_n["type"].as<std::string>();
         if (not FactorySensor::isCreatorRegistered(sensor_type))
         {
-            loadPlugin(sensor_n["plugin"].as<std::string>(), plugins_path, loaders);
+            problem->loadPlugin(sensor_n["plugin"].as<std::string>());
         }
         problem->installSensor(sensor_n);
     }
@@ -211,13 +196,13 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     // PROCESSORS =============================================================================== 
     // load plugin if processor is not registered
     WOLF_INFO("Loading processors parameters");
-    YAML::Node processors_node = param_node["processors"];
+    YAML::Node processors_node = _param_node["processors"];
     for (auto processor_n : processors_node)
     {
         auto processor_type = processor_n["type"].as<std::string>();
         if (not FactoryProcessor::isCreatorRegistered(processor_type))
         {
-            loadPlugin(processor_n["plugin"].as<std::string>(), plugins_path, loaders);
+            problem->loadPlugin(processor_n["plugin"].as<std::string>());
         }
         problem->installProcessor(processor_n);
     }
@@ -225,16 +210,16 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     // MAP (optional) ===============================================================================
     // Default MapBase
     WOLF_INFO("Loading map parameters");
-    if (not param_node["map"])
+    if (not _param_node["map"])
     {
-        param_node["map"]["type"] = "MapBase";
+        _param_node["map"]["type"] = "MapBase";
     }
     // load plugin if map is not registered
-    YAML::Node map_node = param_node["map"];
+    YAML::Node map_node = _param_node["map"];
     auto map_type = map_node["type"].as<std::string>();
     if (not FactoryMap::isCreatorRegistered(map_type))
     {
-        loadPlugin(map_node["plugin"].as<std::string>(), plugins_path, loaders);
+        problem->loadPlugin(map_node["plugin"].as<std::string>());
     }
     WOLF_TRACE("Map Type: ", map_type);
     auto map = FactoryMap::create(map_type, map_node);
@@ -246,21 +231,25 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     return problem;
 }
 
-void Problem::loadPlugin(const std::string& plugin_name, 
-                         const std::string& plugins_path, 
-                         std::list<std::shared_ptr<Loader>>& loaders)
+void Problem::loadPlugin(const std::string& plugin_name)
 {
 #if __APPLE__
     std::string lib_extension = ".dylib";
 #else
     std::string lib_extension = ".so";
 #endif
-    
+
+    std::string plugins_path = _WOLF_LIB_DIR;
+    if (plugins_path.back() != '/')
+    {
+        plugins_path += '/';  // only works for UNIX systems
+    }
+
     std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
-    WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
+    WOLF_TRACE("Loading plugin " + plugin_name + " in " + plugins_path);
     auto l = std::make_shared<LoaderRaw>(plugin);
     l->load();
-    loaders.push_back(l);
+    loaders_.push_back(l);
 }
 
 Problem::~Problem()
@@ -1004,28 +993,34 @@ bool Problem::getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _co
     return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
 }
 
-bool Problem::getFrameCovariance(FrameBaseConstPtr _frame, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const
+bool Problem::getCovariance(HasStateBlocksConstPtr _has_blocks, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const
 {
-    if (not _frame->hasKeys(_keys))
+    if (not _has_blocks)
+    {
+        WOLF_WARN("Problem::getCovariance: called with a nullptr");
+        return false;
+    }
+    if (not _has_blocks->hasKeys(_keys))
     {
+        WOLF_WARN(" does not have the keys ", _keys, ". Available keys: ", _has_blocks->getKeys());
         return false;
     }
 
     bool success(true);
 
     // resizing
-    SizeEigen sz = _frame->getLocalSize();
+    SizeEigen sz = _has_blocks->getLocalSize();
     _covariance.resize(sz, sz);
 
     // filling covariance
     int i = 0, j = 0;
     for (auto key_i : _keys)
     {
-        auto sb_i = _frame->getStateBlock(key_i);
+        auto sb_i = _has_blocks->getStateBlock(key_i);
         j = 0;
         for (auto key_j : _keys)
         {
-            auto sb_j = _frame->getStateBlock(key_j);
+            auto sb_j = _has_blocks->getStateBlock(key_j);
             success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
             j += sb_j->getLocalSize();
         }
@@ -1037,38 +1032,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame, const StateKeys& _key
 
 bool Problem::getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& cov) const
 {
-    auto frm = getLastFrame();
-    return getFrameCovariance(frm, _keys, cov);
-}
-
-bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const
-{
-    if (not _landmark->hasKeys(_keys))
-    {
-        return false;
-    }
-    bool success(true);
-
-    // resizing
-    SizeEigen sz = _landmark->getLocalSize();
-    _covariance.resize(sz, sz);
-
-    // filling covariance
-    int i = 0, j = 0;
-    for (auto key_i : _keys)
-    {
-        auto sb_i = _landmark->getStateBlock(key_i);
-        j = 0;
-        for (auto key_j : _keys)
-        {
-            auto sb_j = _landmark->getStateBlock(key_j);
-            success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-            j += sb_j->getLocalSize();
-        }
-        i += sb_i->getLocalSize();
-    }
-
-    return success;
+    return getCovariance(getLastFrame(), _keys, cov);
 }
 
 MapBaseConstPtr Problem::getMap() const
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 10503f263..0db3b19e0 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -365,6 +365,11 @@ StateBlockPtr SensorBase::getIntrinsic()
     return getStateBlockDynamic('I');
 }
 
+bool SensorBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
+{
+    return getProblem()->getCovariance(shared_from_this(), _keys, _cov);
+}
+
 bool SensorBase::process(const CaptureBasePtr capture_ptr)
 {
     capture_ptr->setSensor(shared_from_this());
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 9d46f7b2a..10b60efa2 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -107,7 +107,7 @@ void show(const ProblemPtr& problem)
         }
         cout << "  state: \n" << F->getStateVector("PO").transpose() << endl;
         Eigen::MatrixXd cov;
-        problem->getFrameCovariance(F, "PO", cov);
+        F->getCovariance("PO", cov);
         cout << "  covariance: \n" << cov << endl;
     }
 }
@@ -181,9 +181,9 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
 
     // get covariances
     MatrixXd P0_solver, P1_solver, P2_solver;
-    ASSERT_TRUE(Pr->getFrameCovariance(F0, "PO", P0_solver));
-    ASSERT_TRUE(Pr->getFrameCovariance(F1, "PO", P1_solver));
-    ASSERT_TRUE(Pr->getFrameCovariance(F2, "PO", P2_solver));
+    ASSERT_TRUE(F0->getCovariance("PO", P0_solver));
+    ASSERT_TRUE(F1->getCovariance("PO", P1_solver));
+    ASSERT_TRUE(F2->getCovariance("PO", P2_solver));
 
     ASSERT_POSE2d_APPROX(F0->getStateVector("PO"), Vector3d(0,0,0), 1e-6);
     auto s0_vector = s0.vector("PO");
@@ -508,13 +508,13 @@ TEST(Odom2d, KF_callback)
 
     // check the split KF
     MatrixXd KF1_cov;
-    ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, "PO", KF1_cov));
+    ASSERT_TRUE(keyframe_1->getCovariance("PO", KF1_cov));
     ASSERT_POSE2d_APPROX(keyframe_1->getStateVector("PO"), integrated_pose_vector[m_split], 1e-6);
     ASSERT_MATRIX_APPROX(KF1_cov,                      integrated_cov_vector [m_split], 1e-6);
 
     // check other KF in the future of the split KF
     MatrixXd KF2_cov;
-    ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, "PO", KF2_cov));
+    ASSERT_TRUE(keyframe_2->getCovariance("PO", KF2_cov));
     ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO"), integrated_pose_vector[n_split], 1e-6);
     ASSERT_MATRIX_APPROX(KF2_cov,                                integrated_cov_vector [n_split], 1e-6);
 
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 23cf399b4..a70aa857d 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -413,7 +413,7 @@ TEST(Problem, Covariances)
 
     // get covariance
     Eigen::MatrixXd Cov;
-    ASSERT_TRUE(P->getFrameCovariance(F, "PO", Cov));
+    ASSERT_TRUE(F->getCovariance("PO", Cov));
 
     ASSERT_EQ(Cov.cols() , 6);
     ASSERT_EQ(Cov.rows() , 6);
-- 
GitLab