From 21b87f3d7c9e8c143dee8b97d53ce8a5f2f8e0b8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 13 Jul 2022 18:15:59 +0200
Subject: [PATCH] [skip ci] wip

---
 CMakeLists.txt                              |   3 +
 include/core/frame/frame_base.h             |  51 ++--
 include/core/math/SE2.h                     |   1 +
 include/core/math/SE3.h                     |   1 +
 include/core/problem/problem.h              |  82 +++---
 include/core/processor/motion_provider.h    |  45 +--
 include/core/sensor/spec_state_sensor.h     |   2 +-
 include/core/state_block/composite.h        | 116 ++++++++
 include/core/state_block/has_state_blocks.h | 106 +++----
 include/core/state_block/spec_composite.h   |  85 ++----
 include/core/state_block/state_block.h      |   2 +-
 include/core/state_block/state_composite.h  |  57 +---
 include/core/state_block/vector_composite.h |  86 ++++++
 include/core/trajectory/trajectory_base.h   |   2 +-
 include/core/utils/converter.h              |   1 +
 schema/processor/MotionProvider.schema      |   6 +-
 src/frame/frame_base.cpp                    |  96 ++++---
 src/problem/problem.cpp                     | 295 ++++++++++++--------
 src/processor/motion_provider.cpp           |   4 +-
 src/processor/processor_motion.cpp          |   6 +-
 src/processor/processor_odom_2d.cpp         |   2 +-
 src/sensor/sensor_base.cpp                  |   6 +-
 src/state_block/has_state_blocks.cpp        |  40 ++-
 src/state_block/spec_composite.cpp          |   9 +-
 src/state_block/state_composite.cpp         | 152 ----------
 src/state_block/vector_composite.cpp        | 167 +++++++++++
 test/gtest_motion_provider.cpp              |  20 +-
 test/gtest_problem.cpp                      |   8 +-
 test/gtest_state_block.cpp                  |   2 +-
 test/gtest_state_composite.cpp              |  66 ++---
 30 files changed, 912 insertions(+), 607 deletions(-)
 create mode 100644 include/core/state_block/composite.h
 create mode 100644 include/core/state_block/vector_composite.h
 create mode 100644 src/state_block/vector_composite.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 261e7816b..0b833cbbf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -204,6 +204,7 @@ SET(HDRS_SOLVER
   include/${PROJECT_NAME}/solver/factory_solver.h
   )
 SET(HDRS_STATE_BLOCK
+  include/${PROJECT_NAME}/state_block/composite.h
   include/${PROJECT_NAME}/state_block/factory_state_block.h
   include/${PROJECT_NAME}/state_block/has_state_blocks.h
   include/${PROJECT_NAME}/state_block/local_parametrization_angle.h
@@ -217,6 +218,7 @@ SET(HDRS_STATE_BLOCK
   include/${PROJECT_NAME}/state_block/state_composite.h
   include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
   include/${PROJECT_NAME}/state_block/state_quaternion.h
+  include/${PROJECT_NAME}/state_block/vector_composite.h
   )
 SET(HDRS_TRAJECTORY
   include/${PROJECT_NAME}/trajectory/trajectory_base.h
@@ -319,6 +321,7 @@ SET(SRCS_STATE_BLOCK
   src/state_block/state_block.cpp
   src/state_block/state_block_derived.cpp
   src/state_block/state_composite.cpp
+  src/state_block/vector_composite.cpp
   )
 SET(SRCS_TRAJECTORY
   src/trajectory/trajectory_base.cpp
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 2a468a08b..1ff268871 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -70,36 +70,47 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
                   StateBlockPtr _o_ptr = nullptr,
                   StateBlockPtr _v_ptr = nullptr);
 
-        /** \brief Constructor with type, time stamp and state pointer
-         * 
-         * Constructor with type, time stamp and state pointer
-         * \param _ts is the time stamp associated to this frame, provided in seconds
-         * \param _frame_structure StateStructure (string) with the state keys
-         * \param _state VectorComposite containing the state of each key
-         **/   
-        FrameBase(const TimeStamp& _ts,
-                  const StateStructure& _frame_structure,
-                  const VectorComposite& _state);
-
-        /** \brief Constructor with type, time stamp and state pointer
+        // /** \brief Constructor with type, time stamp and state pointer
+        //  * 
+        //  * Constructor with type, time stamp and state pointer
+        //  * \param _ts is the time stamp associated to this frame, provided in seconds
+        //  * \param _frame_structure StateStructure (string) with the state keys
+        //  * \param _state VectorComposite containing the state of each key
+        //  **/   
+        // FrameBase(const TimeStamp& _ts,
+        //           const StateStructure& _frame_structure,
+        //           const VectorComposite& _state);
+
+        // /** \brief Constructor with type, time stamp and state pointer
+        //  * 
+        //  * Constructor with type, time stamp and state pointer
+        //  * \param _ts is the time stamp associated to this frame, provided in seconds
+        //  * \param _frame_structure StateStructure (string) with the state keys
+        //  * \param _vectors std::list of VectorXd containing the state of each key
+        //  **/   
+        // FrameBase(const TimeStamp& _ts,
+        //           const StateStructure& _frame_structure,
+        //           const std::list<VectorXd>& _vectors);
+        
+        /** \brief Constructor time stamp and specs composite
          * 
-         * Constructor with type, time stamp and state pointer
+         * Constructor with time stamp and specs composite
          * \param _ts is the time stamp associated to this frame, provided in seconds
-         * \param _frame_structure StateStructure (string) with the state keys
-         * \param _vectors std::list of VectorXd containing the state of each key
-         **/   
+         * \param _frame_specs SpecComposite containing all information needed to create the state blocs.
+         **/             
         FrameBase(const TimeStamp& _ts,
-                  const StateStructure& _frame_structure,
-                  const std::list<VectorXd>& _vectors);
+                  const SpecComposite& _frame_specs);
         
         /** \brief Constructor time stamp and specs composite
          * 
          * Constructor with time stamp and specs composite
          * \param _ts is the time stamp associated to this frame, provided in seconds
-         * \param _frame_specs SpecComposite containing all information needed to create the state blocs.
+         * \param _frame_types TypeComposite containing the types of the state blocs.
+         * \param _frame_vectors VectorComposite containing the vectors of the the state blocs.
          **/             
         FrameBase(const TimeStamp& _ts,
-                  const SpecComposite& _frame_specs);
+                  const TypeComposite& _frame_types,
+                  const VectorComposite& _frame_vectors);
 
         ~FrameBase() override;
 
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 32dbd2fc4..63d07cbee 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -23,6 +23,7 @@
 
 #include "core/common/wolf.h"
 #include "core/math/rotations.h"
+#include "core/state_block/vector_composite.h"
 #include "core/state_block/state_composite.h"
 
 #include <Eigen/Geometry>
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 701dfebf4..9ae6c2bf3 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -23,6 +23,7 @@
 
 #include "core/common/wolf.h"
 #include "core/math/rotations.h"
+#include "core/state_block/vector_composite.h"
 #include "core/state_block/state_composite.h"
 
 /*
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 3b2799e31..2e77bda2f 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -40,7 +40,7 @@ class Loader;
 #include "core/frame/frame_base.h"
 #include "core/state_block/state_block.h"
 #include "core/state_block/spec_composite.h"
-#include "core/state_block/state_composite.h"
+#include "core/state_block/vector_composite.h"
 #include "core/processor/motion_provider.h"
 
 // std includes
@@ -73,12 +73,14 @@ class Problem : public std::enable_shared_from_this<Problem>
         MapBasePtr          map_ptr_;
         std::map<int, MotionProviderPtr>  motion_provider_map_;
         std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
-        SizeEigen state_size_, state_cov_size_, dim_;
+        // SizeEigen state_size_, state_cov_size_;
+        SizeEigen dim_;
         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_;
-        StateStructure frame_structure_;
+        //StateStructure frame_structure_;
+        TypeComposite frame_structure_;
         SpecComposite prior_options_;
         bool prior_applied_;
 
@@ -87,10 +89,14 @@ class Problem : public std::enable_shared_from_this<Problem>
         mutable std::mutex mut_transform_;
 
       private:  // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
+        Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
         Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
         void setup();
 
     public:
+        static ProblemPtr create(const TypeComposite& _frame_structure, 
+                                 SizeEigen _dim, 
+                                 MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
         static ProblemPtr create(const std::string& _frame_structure, 
                                  SizeEigen _dim, 
                                  MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
@@ -109,7 +115,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         const StateStructure& getFrameStructure() const;
 
     protected:
-        void appendToStructure(const StateStructure& _structure);
+        //void appendToStructure(const StateStructure& _structure);
+        void appendToStructure(const TypeComposite& _structure);
 
 
 
@@ -194,21 +201,21 @@ class Problem : public std::enable_shared_from_this<Problem>
         FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
         FrameBasePtr setPrior(const SpecComposite& priors, const TimeStamp& _ts);
 
-        /** \brief Emplace frame from string frame_structure, dimension and vector
-         * \param _time_stamp Time stamp of the frame
-         * \param _frame_structure String indicating the frame structure.
-         * \param _dim variable indicating the dimension of the problem
-         * \param _frame_state State vector; must match the size and format of the chosen frame structure
-         *
-         * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
-         *   - Create a Frame
-         *   - Add it to Trajectory
-         *   - If it is key-frame, update state-block lists in Problem
-         */
-        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-                                  const StateStructure& _frame_structure, //
-                                  const SizeEigen _dim, //
-                                  const Eigen::VectorXd& _frame_state);
+        // /** \brief Emplace frame from string frame_structure, dimension and vector
+        //  * \param _time_stamp Time stamp of the frame
+        //  * \param _frame_structure String indicating the frame structure.
+        //  * \param _dim variable indicating the dimension of the problem
+        //  * \param _frame_state State vector; must match the size and format of the chosen frame structure
+        //  *
+        //  * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+        //  *   - Create a Frame
+        //  *   - Add it to Trajectory
+        //  *   - If it is key-frame, update state-block lists in Problem
+        //  */
+        // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+        //                           const StateStructure& _frame_structure, //
+        //                           const SizeEigen _dim, //
+        //                           const Eigen::VectorXd& _frame_state);
 
         /** \brief Emplace frame from string frame_structure and state
          * \param _time_stamp Time stamp of the frame
@@ -222,8 +229,11 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
+        // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+        //                           const StateStructure& _frame_structure, //
+        //                           const VectorComposite& _frame_state);
         FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-                                  const StateStructure& _frame_structure, //
+                                  const TypeComposite& _frame_structure, //
                                   const VectorComposite& _frame_state);
 
         /** \brief Emplace frame from state
@@ -241,22 +251,22 @@ class Problem : public std::enable_shared_from_this<Problem>
         FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
                                   const VectorComposite& _frame_state);
 
-        /** \brief Emplace frame from string frame_structure and dimension
-         * \param _time_stamp Time stamp of the frame
-         * \param _frame_structure String indicating the frame structure.
-         * \param _dim variable indicating the dimension of the problem
-         *
-         * - The dimension is taken from Problem
-         * - The state is taken from Problem
-         *
-         * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
-         *   - Create a Frame
-         *   - Add it to Trajectory
-         *   - If it is key-frame, update state-block lists in Problem
-         */
-        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-                                  const StateStructure& _frame_structure, //
-                                  const SizeEigen _dim);
+        // /** \brief Emplace frame from string frame_structure and dimension
+        //  * \param _time_stamp Time stamp of the frame
+        //  * \param _frame_structure String indicating the frame structure.
+        //  * \param _dim variable indicating the dimension of the problem
+        //  *
+        //  * - The dimension is taken from Problem
+        //  * - The state is taken from Problem
+        //  *
+        //  * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+        //  *   - Create a Frame
+        //  *   - Add it to Trajectory
+        //  *   - If it is key-frame, update state-block lists in Problem
+        //  */
+        // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+        //                           const StateStructure& _frame_structure, //
+        //                           const SizeEigen _dim);
 
         /** \brief Emplace frame from state vector
          * \param _time_stamp Time stamp of the frame
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index 9cec9025a..faa85b47e 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -22,7 +22,7 @@
 #pragma once
 
 #include "core/common/wolf.h"
-#include "core/state_block/state_composite.h"
+#include "core/state_block/vector_composite.h"
 #include "core/common/params_base.h" // for toString
 #include "yaml-cpp/yaml.h"
 
@@ -33,20 +33,20 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider);
 
 struct ParamsMotionProvider
 {
-    bool state_getter = true;
-    int state_priority = 1;
+    bool state_provider = true;
+    int state_order = 1;
 
     ParamsMotionProvider() = default;
     ParamsMotionProvider(const YAML::Node& _n)
     {
-        state_getter    = _n["state_getter"].as<bool>();
-        if (state_getter)
-            state_priority  = _n["state_priority"].as<double>();
+        state_provider    = _n["state_provider"].as<bool>();
+        if (state_provider)
+            state_order   = _n["state_order"].as<double>();
     }
     std::string print() const
     {
-      return  "state_getter: "   + toString(state_getter)   + "\n"
-            + "state_priority: " + toString(state_priority) + "\n";
+      return  "state_provider: "   + toString(state_provider)   + "\n"
+            + "state_provider_order: " + toString(state_order) + "\n";
     }
 
 };
@@ -58,7 +58,8 @@ class MotionProvider
 {
     public:
 
-        MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params);
+        //MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params);
+        MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params);
         virtual ~MotionProvider();
 
         // Queries to the processor:
@@ -70,21 +71,25 @@ class MotionProvider
         void setOdometry(const VectorComposite&);
 
         bool isStateGetter() const;
-        int getStatePriority() const;
-        void setStatePriority(int);
+        int getOrder() const;
+        void setOrder(int);
 
     public:
-        const StateStructure& getStateStructure ( ) const { return state_structure_; };
-        void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
+        //const StateStructure& getStateStructure ( ) const { return state_structure_; };
+        const TypeComposite& getStateStructure ( ) const { return state_structure_; };
+        //void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
+        void setStateStructure(TypeComposite _state_structure) { state_structure_ = _state_structure; };
         void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
 
     protected:
-        StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+        //StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+        TypeComposite state_structure_;
         VectorComposite odometry_;
         ParamsMotionProviderPtr params_motion_provider_;
 };
 
-inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) :
+// inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) :
+inline MotionProvider::MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params) :
         state_structure_(_structure),
         params_motion_provider_(_params)
 {
@@ -107,17 +112,17 @@ inline MotionProvider::~MotionProvider()
 
 inline bool MotionProvider::isStateGetter() const
 {
-    return params_motion_provider_->state_getter;
+    return params_motion_provider_->state_provider;
 }
 
-inline int MotionProvider::getStatePriority() const
+inline int MotionProvider::getOrder() const
 {
-    return params_motion_provider_->state_priority;
+    return params_motion_provider_->state_order;
 }
 
-inline void MotionProvider::setStatePriority(int _priority)
+inline void MotionProvider::setOrder(int _order)
 {
-    params_motion_provider_->state_priority = _priority;
+    params_motion_provider_->state_order = _order;
 }
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/sensor/spec_state_sensor.h b/include/core/sensor/spec_state_sensor.h
index c1391a534..5773fbc7d 100644
--- a/include/core/sensor/spec_state_sensor.h
+++ b/include/core/sensor/spec_state_sensor.h
@@ -53,7 +53,7 @@ class SpecStateSensor : public SpecState
         std::string print(const std::string& _spaces = "") const override;
 };
 
-typedef TemplateComposite<SpecStateSensor> SpecSensorComposite;
+typedef Composite<SpecStateSensor> SpecSensorComposite;
 
 inline bool SpecStateSensor::isDynamic() const { return dynamic_; }
 
diff --git a/include/core/state_block/composite.h b/include/core/state_block/composite.h
new file mode 100644
index 000000000..83f73f5ab
--- /dev/null
+++ b/include/core/state_block/composite.h
@@ -0,0 +1,116 @@
+//--------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
+
+#include <string>
+#include <unordered_map>
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+
+typedef std::string StateStructure;
+using std::unordered_map;
+
+template <typename T>
+class Composite : public unordered_map<char, T>
+{
+    public:
+        using CompositeType = T;
+        using unordered_map<char, T>::unordered_map;
+
+        Composite(const YAML::Node& _n);
+        virtual ~Composite() = default;
+
+        template <typename CompositeOther>
+        CompositeOther cast() const;
+
+        bool has(char _key) const;
+        bool has(const StateStructure &_structure) const;
+        StateStructure getStructure() const;
+        
+        friend std::ostream& operator<<(std::ostream &_os, const Composite<T> &_x)
+        {
+            for (const auto &pair : _x)
+            {
+                _os << pair.first << ": " << pair.second;
+            }
+            return _os;
+        }
+};
+
+template <typename T>
+template <typename CompositeOther>
+inline CompositeOther Composite<T>::cast() const
+{
+    CompositeOther casted_composite;
+    for (auto pair : *this)
+    {
+        casted_composite.emplace(pair.first, static_cast<typename CompositeOther::CompositeType>(pair.second));
+    }
+    return casted_composite;
+}
+
+template <typename T>
+inline Composite<T>::Composite(const YAML::Node& _n)
+{
+    if (not _n.IsMap())
+    {
+        throw std::runtime_error("SpecComposite: constructor with a non-map yaml node");
+    }
+    for (auto spec_pair : _n)
+    {
+        this->emplace(spec_pair.first.as<char>(), T(spec_pair.second));
+    }
+}
+
+template <typename T>
+inline bool Composite<T>::has(char _key) const
+{
+    return this->count(_key) != 0;
+}
+
+template <typename T>
+inline bool Composite<T>::has(const StateStructure &_structure) const
+{
+    for (auto key : _structure)
+    {
+        if (not has(key))
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
+template <typename T>
+inline std::string Composite<T>::getStructure() const
+{
+    std::string structure;
+    for (auto pair : *this)
+    {
+        structure.push_back(pair.first);
+    }
+    return structure;
+}
+
+}  // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index a61e16055..f728227f9 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -22,7 +22,7 @@
 #pragma once
 
 #include "core/common/wolf.h"
-#include "core/state_block/state_composite.h"
+#include "core/state_block/vector_composite.h"
 #include "core/state_block/spec_composite.h"
 
 #include <map>
@@ -39,14 +39,15 @@ class HasStateBlocks
         HasStateBlocks();
         HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
         HasStateBlocks(const SpecComposite& _specs);
+        HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors);
         virtual ~HasStateBlocks();
 
         const StateStructure& getStructure() const { return structure_; }
         void appendToStructure(const char& _new_key){ structure_ += _new_key; }
         bool isInStructure(const char& _sb_key) const { return structure_.find(_sb_key) != std::string::npos; }
 
-        const std::unordered_map<char, StateBlockConstPtr>& getStateBlockMap() const;
-        const std::unordered_map<char, StateBlockPtr>& getStateBlockMap();
+        std::unordered_map<char, StateBlockConstPtr> getStateBlockMap() const;
+        std::unordered_map<char, StateBlockPtr> getStateBlockMap();
         std::vector<StateBlockConstPtr> getStateBlockVec() const;
         std::vector<StateBlockPtr> getStateBlockVec();
 
@@ -69,8 +70,8 @@ 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, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
-        std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
+        //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>
@@ -103,7 +104,6 @@ class HasStateBlocks
     private:
         StateStructure structure_;
         std::unordered_map<char, StateBlockPtr> state_block_map_;
-        std::unordered_map<char, StateBlockConstPtr> state_block_const_map_;
 
 };
 
@@ -118,8 +118,7 @@ namespace wolf{
 
 inline HasStateBlocks::HasStateBlocks() :
         structure_(std::string("")),
-        state_block_map_(),
-        state_block_const_map_()
+        state_block_map_()
 {
     //
 }
@@ -129,12 +128,15 @@ inline HasStateBlocks::~HasStateBlocks()
     //
 }
 
-inline const std::unordered_map<char, StateBlockConstPtr>& HasStateBlocks::getStateBlockMap() const
+inline std::unordered_map<char, StateBlockConstPtr> HasStateBlocks::getStateBlockMap() const
 {
-    return state_block_const_map_;
+    std::unordered_map<char, StateBlockConstPtr> map_const;
+    for (auto&& pair : state_block_map_)
+        map_const[pair.first] = pair.second;
+    return map_const;
 }
 
-inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap()
+inline std::unordered_map<char, StateBlockPtr> HasStateBlocks::getStateBlockMap()
 {
     return state_block_map_;
 }
@@ -162,13 +164,12 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
 inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_key)
 {
     return state_block_map_.erase(_sb_key);
-    return state_block_const_map_.erase(_sb_key);
 }
 
 template<typename SB, typename ... Args>
 inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
 {
-    assert(state_block_map_.count(_sb_key) == 0 && state_block_const_map_.count(_sb_key) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+    assert(state_block_map_.count(_sb_key) == 0 and "Trying to add a state block with an existing type! Use setStateBlock instead.");
     std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
 
     addStateBlock(_sb_key, sb, _problem);
@@ -178,17 +179,16 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key
 
 inline bool HasStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb)
 {
-    assert (structure_.find(_sb_key) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
-    assert ( state_block_map_.count(_sb_key) > 0 &&  state_block_const_map_.count(_sb_key) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
+    assert (structure_.find(_sb_key) > 0 and "Cannot set a state block out of the state structure! Use addStateBlock instead.");
+    assert ( state_block_map_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead.");
     state_block_map_.at(_sb_key) = _sb;
-    state_block_const_map_.at(_sb_key) = _sb;
     return true; // success
 }
 
 inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_key) const
 {
-    auto it = state_block_const_map_.find(_sb_key);
-    if (it != state_block_const_map_.end())
+    auto it = state_block_map_.find(_sb_key);
+    if (it != state_block_map_.end())
         return it->second;
     else
         return nullptr;
@@ -257,7 +257,7 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _
         const auto& sb = getStateBlock(key);
         if (!sb)
             WOLF_ERROR("Stateblock key ", key, " not in the structure");
-        assert (sb && "Stateblock key not in the structure");
+        assert (sb and "Stateblock key not in the structure");
 
         sb->setState(vec, _notify);
     }
@@ -272,13 +272,13 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS
         structure = _structure;
 
     int size = getSize(structure);
-    assert(_state.size() == size && "In FrameBase::setState wrong state size");
+    assert(_state.size() == size and "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
     for (const char key : structure)
     {
         const auto& sb  = getStateBlock(key);
-        assert (sb && "Stateblock key not in the structure");
+        assert (sb and "Stateblock key not in the structure");
 
         sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
         index += sb->getSize();
@@ -296,8 +296,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
     for (const auto& key : _structure)
     {
         const auto& sb = getStateBlock(key);
-        assert (sb && "Stateblock key not in the structure");
-        assert(*size_it == sb->getSize() && "State block size mismatch");
+        assert (sb and "Stateblock key not in the structure");
+        assert(*size_it == sb->getSize() and "State block size mismatch");
 
         sb->setState(_state.segment(index, *size_it), _notify);
         index += *size_it;
@@ -312,8 +312,8 @@ inline void HasStateBlocks::setState(const StateStructure& _structure, const std
     for (const auto key : _structure)
     {
         const auto& sb = getStateBlock(key);
-        assert (sb && "Stateblock key not in the structure");
-        assert(vec_it->size() == sb->getSize() && "State block size mismatch");
+        assert (sb and "Stateblock key not in the structure");
+        assert(vec_it->size() == sb->getSize() and "State block size mismatch");
 
         sb->setState(*vec_it, _notify);
         vec_it ++;
@@ -337,7 +337,7 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure)
     {
         const auto& sb = getStateBlock(key);
 
-        assert(sb != nullptr && "Requested StateBlock key not in the structure");
+        assert(sb != nullptr and "Requested StateBlock key not in the structure");
 
         state.segment(index,sb->getSize()) = sb->getState();
         index += sb->getSize();
@@ -380,20 +380,46 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co
     return size;
 }
 
-inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockConstPtr& _sb) const
+// inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockConstPtr& _sb) const
+// {
+//     const auto& it = std::find_if(state_block_map_.begin(),
+//                                   state_block_map_.end(),
+//                                   [_sb](const std::pair<char, StateBlockConstPtr>& pair)
+//                                   {
+//                                     return pair.second == _sb;
+//                                   }
+//                                   );
+
+//     return it;
+// }
+
+// inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb)
+// {
+//     const auto& it = std::find_if(state_block_map_.begin(),
+//                                   state_block_map_.end(),
+//                                   [_sb](const std::pair<char, StateBlockPtr>& pair)
+//                                   {
+//                                     return pair.second == _sb;
+//                                   }
+//                                   );
+
+//     return it;
+// }
+
+inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
 {
-    const auto& it = std::find_if(state_block_const_map_.begin(),
-                                  state_block_const_map_.end(),
-                                  [_sb](const std::pair<char, StateBlockConstPtr>& pair)
+    const auto& it = std::find_if(state_block_map_.begin(),
+                                  state_block_map_.end(),
+                                  [_sb](const std::pair<char, StateBlockPtr>& pair)
                                   {
                                     return pair.second == _sb;
                                   }
                                   );
 
-    return it;
+    return it != state_block_map_.end();
 }
 
-inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb)
+inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const
 {
     const auto& it = std::find_if(state_block_map_.begin(),
                                   state_block_map_.end(),
@@ -403,21 +429,7 @@ inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::f
                                   }
                                   );
 
-    return it;
-}
-
-inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
-{
-    return this->find(_sb) != state_block_const_map_.end();
-}
-
-inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const
-{
-    const auto& it = this->find(_sb);
-
-    bool found = (it != state_block_const_map_.end());
-
-    if (found)
+    if (it != state_block_map_.end())
     {
         _key = it->first;
         return true;
diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h
index f0bda4d63..05fa48c3a 100644
--- a/include/core/state_block/spec_composite.h
+++ b/include/core/state_block/spec_composite.h
@@ -21,7 +21,8 @@
 //--------LICENSE_END--------
 #pragma once
 
-#include "core/state_block/state_composite.h"
+#include "core/state_block/composite.h"
+#include "core/state_block/vector_composite.h"
 
 #include <string>
 #include <unordered_map>
@@ -63,88 +64,59 @@ class SpecState
         virtual void check() const;
 
         virtual std::string print(const std::string& _spaces = "") const;
+        friend std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x);
 };
 
+typedef Composite<std::string> TypeComposite;
+
 template <typename T>
-class TemplateComposite : public std::unordered_map<char, T>
+class SpecBaseComposite : public Composite<T>
 {
     public:
-        using unordered_map<char, T>::unordered_map;
-
-        TemplateComposite(const YAML::Node& _n);
-        virtual ~TemplateComposite() = default;
-
-        VectorComposite getState() const;
+        using Composite<T>::Composite;
+        virtual ~SpecBaseComposite() = default;
 
-        template <typename OTHER_T>
-        TemplateComposite<OTHER_T> cast() const
-        {
-            TemplateComposite<OTHER_T> casted_composite;
-            for (auto pair : *this)
-            {
-                casted_composite.emplace(pair.first, static_cast<OTHER_T>(pair.second));
-            }
-            return casted_composite;
-        }
-
-        void emplace(char _key, const T& _spec);
-
-        std::string print() const;
+        VectorComposite getVectorComposite() const;
+        TypeComposite getTypeComposite() const;
 };
 
-typedef TemplateComposite<SpecState> SpecComposite;
-
+typedef SpecBaseComposite<SpecState> SpecComposite;
 
-template <typename T>
-TemplateComposite<T>::TemplateComposite(const YAML::Node& _n)
+template <>
+inline VectorComposite SpecBaseComposite<SpecState>::getVectorComposite() const
 {
-    if (not _n.IsMap())
-    {
-        throw std::runtime_error("SpecComposite: constructor with a non-map yaml node");
-    }
-    for (auto spec_pair : _n)
+    VectorComposite states;
+    for (auto spec_pair : *this)
     {
-        this->emplace(spec_pair.first.as<char>(), T(spec_pair.second));
+        states.emplace(spec_pair.first, spec_pair.second.getState());
     }
+    return states;
 }
 
 template <typename T>
-void TemplateComposite<T>::emplace(char _key, const T& _spec)
+inline VectorComposite SpecBaseComposite<T>::getVectorComposite() const
 {
-    this->insert({_key,_spec});
+    throw std::runtime_error("Impossible to build a VectorComposite from this Composite");
 }
 
-template <typename T>
-VectorComposite TemplateComposite<T>::getState() const
+template <>
+inline TypeComposite SpecBaseComposite<SpecState>::getTypeComposite() const
 {
-    VectorComposite states;
+    TypeComposite types;
     for (auto spec_pair : *this)
     {
-        states.emplace(spec_pair.first, spec_pair.second.getState());
+        types.emplace(spec_pair.first, spec_pair.second.getType());
     }
-    return states;
+    return types;
 }
 
-// template <typename T, typename OTHER_T>
-// TemplateComposite<OTHER_T> TemplateComposite<T>::cast() const
-// {
-//     return TemplateComposite<OTHER_T>();
-// }
-
 template <typename T>
-std::string TemplateComposite<T>::print() const
+inline TypeComposite SpecBaseComposite<T>::getTypeComposite() const
 {
-    std::string output;
-    
-    for (auto spec_pair : *this)
-    {
-        output += std::string(1,spec_pair.first) + ":\n";
-        output += spec_pair.second.print("\t");
-    }
-
-    return output;
+    throw std::runtime_error("Impossible to build a TypeComposite from this Composite");
 }
 
+
 inline const std::string& SpecState::getType() const { return type_; }
 
 inline const std::string& SpecState::getMode() const { return mode_; }
@@ -159,4 +131,5 @@ inline bool SpecState::isInitialGuess() const { return mode_ == "initial_guess";
 
 inline bool SpecState::isFactor() const { return mode_ == "factor"; }
 
-}  // namespace wolf
\ No newline at end of file
+}  // namespace wolf
+
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 2f967122c..ca355e307 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -29,7 +29,7 @@ class LocalParametrizationBase;
 
 //Wolf includes
 #include "core/common/wolf.h"
-#include "core/state_block/state_composite.h"
+#include "core/state_block/vector_composite.h"
 
 //std includes
 #include <iostream>
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 83c07d4c4..7307b8afb 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -22,74 +22,21 @@
 #pragma once
 
 #include "core/common/wolf.h"
+#include "core/state_block/vector_composite.h"
 
 #include <unordered_map>
-
 #include <iostream>
 
-
 namespace wolf
 {
 
 using std::string;
+using std::unordered_map;
 using namespace Eigen;
 
-typedef std::string StateStructure;
 typedef std::unordered_map < char, StateBlockPtr   > StateBlockMap;
 typedef StateBlockMap::const_iterator StateBlockMapCIter;
 
-class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
-{
-    public:
-        VectorComposite() {};
-        VectorComposite(const StateStructure& _s);
-        VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
-        /**
-         * \brief Construct from Eigen::VectorXd and structure
-         *
-         * Usage:
-         *
-         *   VectorXd vec_eigen(1,2,3,4,5);
-         *
-         *   VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
-         *
-         * result:
-         *
-         *   vec_comp["a"].transpose(); // = (1,2);
-         *   vec_comp["b"].transpose(); // = (3,4,5);
-         */
-        VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
-        VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
-
-        Eigen::VectorXd vector(const StateStructure& _structure) const;
-
-        /**
-         * \brief set from Eigen::VectorXd and structure
-         *
-         * Usage:
-         *
-         *   Eigen::VectorXd        vec_eigen;
-         *   wolf::VectorComposite  vec_comp;
-         *
-         *   vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
-         *
-         * result:
-         *
-         *   vec_comp["a"].transpose(); // = (1,2);
-         *   vec_comp["b"].transpose(); // = (3,4,5);
-         */
-        void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
-        void setZero();
-
-        bool includesStructure(const StateStructure &_structure) const;
-
-        friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
-        friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
-        friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
-        friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x);
-};
-
-
 class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >
 {
 
diff --git a/include/core/state_block/vector_composite.h b/include/core/state_block/vector_composite.h
new file mode 100644
index 000000000..f5a87bcea
--- /dev/null
+++ b/include/core/state_block/vector_composite.h
@@ -0,0 +1,86 @@
+//--------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
+
+#include "core/common/wolf.h"
+#include "core/state_block/composite.h"
+
+#include <unordered_map>
+#include <iostream>
+
+namespace wolf
+{
+
+using namespace Eigen;
+
+class VectorComposite : public Composite<Eigen::VectorXd>
+{
+    public:
+        using Composite<Eigen::VectorXd>::Composite;
+        
+        VectorComposite() {};
+        VectorComposite(const StateStructure& _s);
+        VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
+        /**
+         * \brief Construct from Eigen::VectorXd and structure
+         *
+         * Usage:
+         *
+         *   VectorXd vec_eigen(1,2,3,4,5);
+         *
+         *   VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+         *
+         * result:
+         *
+         *   vec_comp["a"].transpose(); // = (1,2);
+         *   vec_comp["b"].transpose(); // = (3,4,5);
+         */
+        VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+        VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
+
+        Eigen::VectorXd vector(const StateStructure& _structure) const;
+
+        /**
+         * \brief set from Eigen::VectorXd and structure
+         *
+         * Usage:
+         *
+         *   Eigen::VectorXd        vec_eigen;
+         *   wolf::VectorComposite  vec_comp;
+         *
+         *   vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+         *
+         * result:
+         *
+         *   vec_comp["a"].transpose(); // = (1,2);
+         *   vec_comp["b"].transpose(); // = (3,4,5);
+         */
+        void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+        void setZero();
+
+        friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
+        // friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+        // friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+        // friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x);
+};
+
+}
\ No newline at end of file
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 2362b88ce..194c942d7 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -31,7 +31,7 @@ class FrameBase;
 #include "core/common/wolf.h"
 #include "core/common/node_base.h"
 #include "core/common/time_stamp.h"
-#include "core/state_block/state_composite.h"
+#include "core/state_block/vector_composite.h"
 
 namespace wolf {
 
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index f989149fb..9fd1d9a3e 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -24,6 +24,7 @@
 // wolf
 #include "core/common/wolf.h"
 #include "core/utils/converter_utils.h"
+#include "core/state_block/vector_composite.h"
 #include "core/state_block/state_composite.h"
 
 // Eigen
diff --git a/schema/processor/MotionProvider.schema b/schema/processor/MotionProvider.schema
index a557389eb..6fe9328c1 100644
--- a/schema/processor/MotionProvider.schema
+++ b/schema/processor/MotionProvider.schema
@@ -1,10 +1,10 @@
-state_getter:
+state_provider:
   mandatory: true
   yaml_type: scalar
   type: bool
   doc: If the processor is used by the problem as provider of state.
-state_priority:
+state_provider_order:
   mandatory: false
   yaml_type: scalar
   type: double
-  doc: The priority of this processor when problem gets the state (only used if state_getter = true). Two processors cannot have the same priority (if so, when installing the second is increased).
\ No newline at end of file
+  doc: The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased).
\ No newline at end of file
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 47da4c53d..a681f73a2 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -33,48 +33,48 @@ namespace wolf {
 
 unsigned int FrameBase::frame_id_count_ = 0;
 
-FrameBase::FrameBase(const TimeStamp& _ts,
-                     const StateStructure& _frame_structure,
-                     const VectorComposite& _state) :
-        NodeBase("FRAME", "FrameBase"),
-        HasStateBlocks(_frame_structure),
-        trajectory_ptr_(),
-        frame_id_(++frame_id_count_),
-        time_stamp_(_ts)
-{
-    assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure");
-
-    for (auto key : getStructure())
-    {
-        StateBlockPtr sb = FactoryStateBlock::create(string(1,key), _state.at(key), false); // false = non fixed
-        addStateBlock(key, sb, getProblem());
-    }
-}
-
-
-FrameBase::FrameBase(const TimeStamp& _ts,
-                     const StateStructure& _frame_structure,
-                     const std::list<VectorXd>& _vectors) :
-                NodeBase("FRAME", "FrameBase"),
-                HasStateBlocks(_frame_structure),
-                trajectory_ptr_(),
-                frame_id_(++frame_id_count_),
-                time_stamp_(_ts)
-{
-    assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
-
-    auto vec_it = _vectors.begin();
-    for (const auto key : _frame_structure)
-    {
-        const auto& vec = *vec_it;
-
-        StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
-
-        addStateBlock(key, sb, getProblem());
-
-        vec_it++;
-    }
-}
+// FrameBase::FrameBase(const TimeStamp& _ts,
+//                      const StateStructure& _frame_structure,
+//                      const VectorComposite& _state) :
+//         NodeBase("FRAME", "FrameBase"),
+//         HasStateBlocks(_frame_structure),
+//         trajectory_ptr_(),
+//         frame_id_(++frame_id_count_),
+//         time_stamp_(_ts)
+// {
+//     assert(_state.has(_frame_structure) && "_state does not include all keys of _frame_structure");
+
+//     for (auto key : getStructure())
+//     {
+//         StateBlockPtr sb = FactoryStateBlock::create(std::string(1,key), _state.at(key), false); // false = non fixed
+//         addStateBlock(key, sb, getProblem());
+//     }
+// }
+
+
+// FrameBase::FrameBase(const TimeStamp& _ts,
+//                      const StateStructure& _frame_structure,
+//                      const std::list<VectorXd>& _vectors) :
+//                 NodeBase("FRAME", "FrameBase"),
+//                 HasStateBlocks(_frame_structure),
+//                 trajectory_ptr_(),
+//                 frame_id_(++frame_id_count_),
+//                 time_stamp_(_ts)
+// {
+//     assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
+
+//     auto vec_it = _vectors.begin();
+//     for (const auto key : _frame_structure)
+//     {
+//         const auto& vec = *vec_it;
+
+//         StateBlockPtr sb = FactoryStateBlock::create(std::string(1,key), vec, false); // false = non fixed
+
+//         addStateBlock(key, sb, getProblem());
+
+//         vec_it++;
+//     }
+// }
 
 
 FrameBase::FrameBase(const TimeStamp& _ts,
@@ -113,6 +113,18 @@ FrameBase::FrameBase(const TimeStamp& _ts,
 }
 
 
+FrameBase::FrameBase(const TimeStamp& _ts,
+                     const TypeComposite& _frame_types,
+                     const VectorComposite& _frame_vectors) :
+            NodeBase("FRAME", "FrameBase"),
+            HasStateBlocks(_frame_types, _frame_vectors),
+            trajectory_ptr_(),
+            frame_id_(++frame_id_count_),
+            time_stamp_(_ts)
+{
+}
+
+
 FrameBase::~FrameBase()
 {
 }
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index dc7720fe5..acab5763a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -41,7 +41,7 @@
 namespace wolf
 {
 
-Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
+Problem::Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
         tree_manager_(nullptr),
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
@@ -51,27 +51,61 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
         prior_options_(),
         prior_applied_(false),
         transformed_(false)
+{
+}
+
+Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
+        tree_manager_(nullptr),
+        hardware_ptr_(std::make_shared<HardwareBase>()),
+        trajectory_ptr_(std::make_shared<TrajectoryBase>()),
+        map_ptr_(_map),
+        motion_provider_map_(),
+        //frame_structure_(_frame_structure),
+        frame_structure_(),
+        prior_options_(),
+        prior_applied_(false),
+        transformed_(false)
 {
     dim_ = _dim;
-    if (_frame_structure == "PO" and _dim == 2)
-    {
-        state_size_ = 3;
-        state_cov_size_ = 3;
-    }else if (_frame_structure == "PO" and _dim == 3)
-    {
-        state_size_ = 7;
-        state_cov_size_ = 6;
-    }else if (_frame_structure == "POV" and _dim == 3)
-    {
-        state_size_ = 10;
-        state_cov_size_ = 9;
-    }else if (_frame_structure == "POVCDL" and _dim == 3)
+    for (auto key : _frame_structure)
     {
-        state_size_ = 19;
-        state_cov_size_ = 18;
+        if (key == 'P')
+        {
+            appendToStructure({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")}});
+        }
+        else if (key == 'O')
+        {
+            appendToStructure({{'P', (dim_ == 2 ? "StateAngle" : "StateQuaternion")}});
+        }
+        else if (key == 'V')
+        {
+            appendToStructure({{'P', (dim_ == 2 ? "StateVector2d" : "StateVector3d")}});
+        }
+        else
+        {
+            throw std::runtime_error("Problem::Problem: Unknown type for key " + std::string(1,key) + " use the constructor via TypeComposite");
+        }
     }
-    else std::runtime_error(
-            "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
+
+    // if (_frame_structure == "PO" and _dim == 2)
+    // {
+    //     state_size_ = 3;
+    //     state_cov_size_ = 3;
+    // }else if (_frame_structure == "PO" and _dim == 3)
+    // {
+    //     state_size_ = 7;
+    //     state_cov_size_ = 6;
+    // }else if (_frame_structure == "POV" and _dim == 3)
+    // {
+    //     state_size_ = 10;
+    //     state_cov_size_ = 9;
+    // }else if (_frame_structure == "POVCDL" and _dim == 3)
+    // {
+    //     state_size_ = 19;
+    //     state_cov_size_ = 18;
+    // }
+    // else std::runtime_error(
+    //         "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
 }
 
 void Problem::setup()
@@ -82,6 +116,13 @@ void Problem::setup()
         map_ptr_   -> setProblem(shared_from_this());
 }
 
+ProblemPtr Problem::create(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map)
+{
+    ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+    p->setup();
+    return p->shared_from_this();
+}
+
 ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
 {
     ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
@@ -91,7 +132,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;
@@ -148,9 +188,14 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     // structure and dimension
     WOLF_INFO("Loading problem parameters");
     YAML::Node problem_node = param_node["problem"];
-    std::string frame_structure = problem_node["frame_structure"].as<std::string>();
-    int         dim             = problem_node["dimension"].as<int>();
-    auto        problem         = Problem::create(frame_structure, dim, nullptr);
+    std::string frame_structure;
+    if (problem_node["frame_structure"])
+    {
+        // TODO: accept types!
+        frame_structure = problem_node["frame_structure"].as<std::string>();
+    }
+    int  dim     = problem_node["dimension"].as<int>();
+    auto problem = Problem::create(frame_structure, dim, nullptr);
     
     // Tree manager
     WOLF_INFO("Loading tree manager parameters");
@@ -167,7 +212,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     WOLF_INFO("Loading problem first frame parameters");
     WOLF_INFO(problem_node["first_frame"])
     SpecComposite priors(problem_node["first_frame"]);
-    WOLF_INFO(priors.print());
+    WOLF_INFO(priors);
     problem->setPriorOptions(priors);
 
     // SENSORS =============================================================================== 
@@ -348,56 +393,57 @@ ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name)
     return nullptr;
 }
 
-FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
-                                   const StateStructure& _frame_structure, //
-                                   const SizeEigen _dim, //
-                                   const Eigen::VectorXd& _frame_state)
-{
-    VectorComposite state;
-    SizeEigen index = 0;
-    SizeEigen size = 0;
-    for (auto key : _frame_structure)
-    {
-        if (key == 'O')
-        {
-            if (_dim == 2) size = 1;
-            else size = 4;
-        }
-        else
-            size = _dim;
-
-        assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");
-
-        state.emplace(key, _frame_state.segment(index, size));
-
-        // append new key to frame structure
-        if (frame_structure_.find(key) == std::string::npos) // not found
-            frame_structure_ += std::string(1,key);
-
-        index += size;
-    }
-
-    assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
-
-    auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
-                                             _time_stamp,
-                                             _frame_structure,
-                                             state);
-
-    return frm;
-}
-
-FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
-                                   const StateStructure& _frame_structure, //
-                                   const SizeEigen _dim)
-{
-    return emplaceFrame(_time_stamp,
-                        _frame_structure,
-                        getState(_time_stamp));
-}
+// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+//                                    const StateStructure& _frame_structure, //
+//                                    const SizeEigen _dim, //
+//                                    const Eigen::VectorXd& _frame_state)
+// {
+//     VectorComposite state;
+//     SizeEigen index = 0;
+//     SizeEigen size = 0;
+//     for (auto key : _frame_structure)
+//     {
+//         if (key == 'O')
+//         {
+//             if (_dim == 2) size = 1;
+//             else size = 4;
+//         }
+//         else
+//             size = _dim;
+
+//         assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");
+
+//         state.emplace(key, _frame_state.segment(index, size));
+
+//         // append new key to frame structure
+//         if (frame_structure_.find(key) == std::string::npos) // not found
+//             frame_structure_ += std::string(1,key);
+
+//         index += size;
+//     }
+
+//     assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
+
+//     auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
+//                                              _time_stamp,
+//                                              _frame_structure,
+//                                              state);
+
+//     return frm;
+// }
+
+// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+//                                    const StateStructure& _frame_structure, //
+//                                    const SizeEigen _dim)
+// {
+//     return emplaceFrame(_time_stamp,
+//                         _frame_structure,
+//                         getState(_time_stamp));
+// }
 
 FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
-                                    const StateStructure& _frame_structure, //
+                                    //const StateStructure& _frame_structure, //
+                                    const TypeComposite& _frame_structure, //
                                     const VectorComposite& _frame_state)
 {
     return FrameBase::emplace<FrameBase>(getTrajectory(),
@@ -409,12 +455,18 @@ FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
 FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
                                     const VectorComposite& _frame_state)
 {
-    // append new keys to frame structure
-    for (auto pair_key_vec : _frame_state)
+    // // append new keys to frame structure
+    // for (auto pair_key_vec : _frame_state)
+    // {
+    //     if (not frame_structure_.has(pair_key_vec.first))
+    //     auto key = pair_key_vec.first;
+    //     if (frame_structure_.find(key) == std::string::npos) // not found
+    //         frame_structure_ += std::string(1,key);
+    // }
+
+    if (not frame_structure_.has(_frame_state.getStructure()))
     {
-        auto key = pair_key_vec.first;
-        if (frame_structure_.find(key) == std::string::npos) // not found
-            frame_structure_ += std::string(1,key);
+        throw std::runtime_error("Problem::emplaceFrame: the given VectorComposite has a state with an unknown key");
     }
 
     return FrameBase::emplace<FrameBase>(getTrajectory(),
@@ -423,21 +475,25 @@ FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
                                          _frame_state);
 }
 
-FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
-                                   const Eigen::VectorXd& _frame_state)
-{
-    return emplaceFrame(_time_stamp,
-                        this->getFrameStructure(),
-                        this->getDim(),
-                        _frame_state);
-}
-
-FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
-{
-    return emplaceFrame(_time_stamp,
-                        this->getFrameStructure(),
-                        this->getDim());
-}
+// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+//                                    const Eigen::VectorXd& _frame_state)
+// {
+//     // return emplaceFrame(_time_stamp,
+//     //                     this->getFrameStructure(),
+//     //                     this->getDim(),
+//     //                     _frame_state);
+//     return emplaceFrame(_time_stamp,
+//                         this->getFrameStructure(),
+//                         this->getDim(),
+//                         _frame_state);
+// }
+
+// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
+// {
+//     return emplaceFrame(_time_stamp,
+//                         this->getFrameStructure(),
+//                         this->getDim());
+// }
 
 FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
                                    const SpecComposite& _frame_spec_composite)
@@ -445,7 +501,7 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
     // Checks
     if (_frame_spec_composite.count('P') == 0 or _frame_spec_composite.count('O') == 0)
     {
-        WOLF_INFO(_frame_spec_composite.print());
+        WOLF_INFO(_frame_spec_composite);
         throw std::runtime_error("Problem::emplaceFrame: emplacing a frame without P or O");
     }
     if (_frame_spec_composite.at('P').getType() != "P" and 
@@ -533,7 +589,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
         state_last = last_kf->getState(structure);
 
     else if (not prior_options_.empty())
-        state_last = prior_options_.getState();
+        state_last = prior_options_.getVectorComposite();
 
     for (auto key : structure)
     {
@@ -545,7 +601,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
                 state.emplace(key, state_last_it->second);
 
             else
-                state.emplace(key, stateZero(string(1,key)).at(key));
+                state.emplace(key, stateZero(std::string(1,key)).at(key));
         }
     }
 
@@ -587,7 +643,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
         state_last = last_kf->getState(structure);
 
     else if (not prior_options_.empty())
-        state_last = prior_options_.getState();
+        state_last = prior_options_.getVectorComposite();
 
     for (auto key : structure)
     {
@@ -599,7 +655,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
                 state.emplace(key, state_last_it->second);
 
             else
-                state.emplace(key, stateZero(string(1,key)).at(key));
+                state.emplace(key, stateZero(std::string(1,key)).at(key));
         }
     }
 
@@ -630,7 +686,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
     VectorComposite state_last;
 
     if (not prior_options_.empty())
-        state_last = prior_options_.getState();
+        state_last = prior_options_.getVectorComposite();
 
     for (auto key : structure)
     {
@@ -642,7 +698,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
                 odom_state.emplace(key, state_last_it->second);
 
             else
-                odom_state.emplace(key, stateZero(string(1,key)).at(key));
+                odom_state.emplace(key, stateZero(std::string(1,key)).at(key));
         }
     }
 
@@ -656,14 +712,22 @@ SizeEigen Problem::getDim() const
 
 const StateStructure& Problem::getFrameStructure() const
 {
-    return frame_structure_;
+    // return frame_structure_;
+    return frame_structure_.getStructure();
 }
 
-void Problem::appendToStructure(const StateStructure& _structure)
+// void Problem::appendToStructure(const StateStructure& _structure)
+// {
+//     for (auto key : _structure)
+//         if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
+//             frame_structure_ += key;
+// }
+
+void Problem::appendToStructure(const TypeComposite& _types)
 {
-    for (auto key : _structure)
-        if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
-            frame_structure_ += key;
+    for (auto pair : _types)
+        if (not frame_structure_.has(pair.first)) // now key not found in problem structure -> append!
+            frame_structure_.emplace(pair.first, pair.second);
 }
 
 void Problem::setTreeManager(TreeManagerBasePtr _gm)
@@ -677,33 +741,36 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
 
 void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
 {
+    // Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key
+    // TODO
+
     // Check if is state getter
     if (not _motion_provider_ptr->isStateGetter())
     {
-        WOLF_WARN("Problem::addMotionProvider: adding a MotionProvider processor with state_getter=false. Not adding this processor");
+        WOLF_WARN("Problem::addMotionProvider: adding a MotionProvider processor with state_provider=false. Not adding this processor");
         return;
     }
 
     // check duplicated priority
-    while (motion_provider_map_.count(_motion_provider_ptr->getStatePriority()) == 1)
+    while (motion_provider_map_.count(_motion_provider_ptr->getOrder()) == 1)
     {
         WOLF_ERROR("Problem::addMotionProvider: adding a MotionProvider processor with priority = ",
-                   _motion_provider_ptr->getStatePriority(),
+                   _motion_provider_ptr->getOrder(),
                    " which is already taken. Trying to add it with priority = ",
-                   _motion_provider_ptr->getStatePriority()+1);
-        _motion_provider_ptr->setStatePriority(_motion_provider_ptr->getStatePriority()+1);
+                   _motion_provider_ptr->getOrder()+1);
+        _motion_provider_ptr->setOrder(_motion_provider_ptr->getOrder()+1);
     }
 
     // add to map ordered by priority
-    motion_provider_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr);
+    motion_provider_map_.emplace(_motion_provider_ptr->getOrder(), _motion_provider_ptr);
     appendToStructure(_motion_provider_ptr->getStateStructure());
 }
 
 void Problem::removeMotionProvider(MotionProviderPtr proc)
 {
-    WOLF_WARN_COND(motion_provider_map_.count(proc->getStatePriority()) == 0, "Problem::clearMotionProvider: missing processor");
+    WOLF_WARN_COND(motion_provider_map_.count(proc->getOrder()) == 0, "Problem::clearMotionProvider: missing processor");
 
-    motion_provider_map_.erase(proc->getStatePriority());
+    motion_provider_map_.erase(proc->getOrder());
 
     // rebuild frame structure with remaining motion processors
     frame_structure_.clear();
@@ -1117,7 +1184,6 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
         // Factor
         else if (prior_pair.second.isFactor())
         {
-
             // Emplace a capture (if not done already)
             if (not prior_cap)
                 prior_cap = CaptureBase::emplace<CaptureVoid>(first_frame, _ts, nullptr);
@@ -1128,7 +1194,10 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
             {
                 throw std::runtime_error("Problem::applyPriorOptions: covariance matrix has wrong size");
             }
-            auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), prior_pair.second.getState(), cov);
+            auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, 
+                                                               "Prior " + std::string(1,key), 
+                                                               prior_pair.second.getState(), 
+                                                               cov);
 
             // Emplace a factor
             if (sb->hasLocalParametrization())
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index e2723db61..4cd155830 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -29,7 +29,7 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion
     setOdometry(_prb_ptr->stateZero(state_structure_));
     if (not isStateGetter())
     {
-        WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_getter=false. Not adding this processor");
+        WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor");
         return;
     }
     _prb_ptr->addMotionProvider(_motion_ptr);
@@ -37,7 +37,7 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion
 
 void MotionProvider::setOdometry(const VectorComposite& _odom)
 {
-    assert(_odom.includesStructure(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom.");
+    assert(_odom.has(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom.");
 
     for (auto key : state_structure_)
         odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index a934fe6b7..2936dfc16 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -284,7 +284,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             for (auto pair_key_vec : proc_state)
                 if (!keyframe_from_callback->isInStructure(pair_key_vec.first))
                     keyframe_from_callback->addStateBlock(pair_key_vec.first,
-                                                          FactoryStateBlock::create(string(1, pair_key_vec.first),
+                                                          FactoryStateBlock::create(std::string(1, pair_key_vec.first),
                                                                                     pair_key_vec.second,
                                                                                     false),
                                                           getProblem());
@@ -375,7 +375,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             for (auto pair_key_vector : proc_state)
                 if (!keyframe_from_callback->isInStructure(pair_key_vector.first))
                     keyframe_from_callback->addStateBlock(pair_key_vector.first,
-                                                          FactoryStateBlock::create(string(1, pair_key_vector.first),
+                                                          FactoryStateBlock::create(std::string(1, pair_key_vector.first),
                                                                                     pair_key_vector.second,
                                                                                     false),
                                                           getProblem());
@@ -428,7 +428,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     for (auto & pair_key_vec : state_propa)
         if (!last_ptr_->getFrame()->isInStructure(pair_key_vec.first))
             last_ptr_->getFrame()->addStateBlock(pair_key_vec.first,
-                                                 FactoryStateBlock::create(string(1, pair_key_vec.first),
+                                                 FactoryStateBlock::create(std::string(1, pair_key_vec.first),
                                                                            pair_key_vec.second,
                                                                            false),
                                                  getProblem());
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index ec6a42639..be37af7ec 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -22,7 +22,7 @@
 #include "core/processor/processor_odom_2d.h"
 #include "core/sensor/sensor_odom.h"
 #include "core/math/covariance.h"
-#include "core/state_block/state_composite.h"
+#include "core/state_block/vector_composite.h"
 #include "core/factor/factor_relative_pose_2d.h"
 
 namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index b2a5f8ee6..725b9f903 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -74,9 +74,9 @@ SensorBase::SensorBase(const std::string& _type,
             setDriftStd(key, prior.getDriftStd());
     }
 
-    WOLF_INFO(_priors.print());
-    auto base_prior = _priors.cast<SpecState>();
-    WOLF_INFO(base_prior.print());
+    WOLF_INFO(_priors);
+    auto base_prior = _priors.cast<SpecComposite>();
+    WOLF_INFO(base_prior);
 }
 
 SensorBase::~SensorBase()
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 2f59aa27e..717fc720e 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -55,14 +55,50 @@ HasStateBlocks::HasStateBlocks(const SpecComposite& _specs)
     }
 }
 
+HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors)
+{
+    if (not _types.has(_vectors.getStructure()) or not _vectors.has(_types.getStructure()))
+    {
+        throw std::runtime_error("HasStateBlocks::HasStateBlocks: provided type and vector composites don't have the same structure");
+    }
+
+    for (auto type_pair : _types)
+    {
+        auto key = type_pair.first;
+        auto type = type_pair.second;
+        auto vector = _vectors.at(key);
+
+        if (key == 'P' and 
+            type != "P" and 
+            type != "StatePoint2d" and 
+            type != "StatePoint3d" )
+        {
+            throw std::runtime_error("HasStateBlocks: in key 'P', the state block should be of type 'P', 'StatePoint2d' or 'StatePoint3d'");
+        }
+        if (key == 'O' and 
+            type != "O" and 
+            type != "StateAngle" and 
+            type != "StateQuaternion" )
+        {
+            throw std::runtime_error("HasStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+        }
+        if (key == 'V' and 
+            type != "V" and 
+            type != "StateVector2d" and 
+            type != "StateVector3d" )
+        {
+            throw std::runtime_error("HasStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+        }
+        addStateBlock(key, FactoryStateBlock::create(type, vector, false), nullptr);
+    }
+}
+
 StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem)
 {
     assert(state_block_map_.count(_sb_key) == 0 and
-           state_block_const_map_.count(_sb_key) == 0 and
            "Trying to add a state block with an existing type! Use setStateBlock instead.");
 
     state_block_map_.emplace(_sb_key, _sb);
-    state_block_const_map_.emplace(_sb_key, _sb);
     if (not isInStructure(_sb_key))
         appendToStructure(_sb_key);
 
diff --git a/src/state_block/spec_composite.cpp b/src/state_block/spec_composite.cpp
index 7a648badb..f6a43c2c8 100644
--- a/src/state_block/spec_composite.cpp
+++ b/src/state_block/spec_composite.cpp
@@ -27,6 +27,7 @@
 namespace wolf
 {
 
+
 SpecState::SpecState(const std::string&     _type,
                      const Eigen::VectorXd& _state,
                      const std::string&     _mode,
@@ -74,10 +75,16 @@ void SpecState::check() const
 
 std::string SpecState::print(const std::string& _spaces) const
 {
-    return _spaces + "SpecState " + type_ + "\n" +
+    return _spaces + "type: " + type_ + "\n" +
            _spaces + "mode: " + mode_ + "\n" + 
            _spaces + "state: " + toString(state_) + "\n" +
            (mode_ == "factor" ? _spaces + "noise_std: " + toString(noise_std_) + "\n" : "");
 }
 
+std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x)
+{
+    _os << _x.print();
+    return _os;
+}
+
 }  // namespace wolf
\ No newline at end of file
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index ff1888db0..a84a74f09 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -20,163 +20,11 @@
 //
 //--------LICENSE_END--------
 
-
-
 #include "core/state_block/state_composite.h"
 #include "core/state_block/state_block.h"
 
 namespace wolf{
 
-////// VECTOR COMPOSITE //////////
-
-VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
-{
-    auto size_it = _sizes.begin();
-    for ( const auto& key : _structure)
-    {
-        const auto& size    = *size_it;
-
-        this->emplace(key, VectorXd(size).setZero());
-
-        size_it ++;
-    }
-}
-
-VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
-{
-    int index = 0;
-    auto size_it = _sizes.begin();
-    for ( const auto& key : _structure)
-    {
-        const auto& size    = *size_it;
-
-        (*this)[key]        = _v.segment(index,size);
-
-        index += size;
-        size_it ++;
-    }
-}
-
-VectorComposite::VectorComposite (const StateStructure& _s)
-{
-    for (const auto& key : _s)
-    {
-        this->emplace(key,VectorXd(0));
-    }
-}
-
-VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors)
-{
-    assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch");
-
-    auto vector_it = _vectors.begin();
-
-    for (const auto& key : _structure)
-    {
-        this->emplace(key, *vector_it);
-        vector_it++;
-    }
-}
-
-
-Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
-{
-    // traverse once with unordered_map access
-    std::vector<const VectorXd*> vp;
-    unsigned int size = 0;
-    for (const auto& key : _structure)
-    {
-        vp.push_back(&(this->at(key)));
-        size +=  vp.back()->size();
-    }
-
-    Eigen::VectorXd x(size);
-
-    // traverse once linearly
-    unsigned int index = 0;
-    for (const auto& blkp : vp)
-    {
-        x.segment(index, blkp->size()) = *blkp;
-        index += blkp->size();
-    }
-    return x;
-}
-
-bool VectorComposite::includesStructure(const StateStructure &_structure) const
-{
-    for (auto key : _structure)
-        if (count(key) == 0)
-            return false;
-    return true;
-}
-
-std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
-{
-    for (const auto &pair_key_vec : _x)
-    {
-        const auto &key = pair_key_vec.first;
-        const auto &vec = pair_key_vec.second;
-        _os << key << ": (" << vec.transpose() << ");  ";
-    }
-    return _os;
-}
-
-wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
-{
-    wolf::VectorComposite xpy;
-    for (const auto& pair_i_xi : _x)
-    {
-        const auto& i = pair_i_xi.first;
-
-        xpy.emplace(i, _x.at(i) + _y.at(i));
-    }
-    return xpy;
-}
-
-VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y)
-{
-    VectorComposite xpy;
-    for (const auto& pair_i_xi : _x)
-    {
-        const auto& i = pair_i_xi.first;
-
-        xpy.emplace(i, _x.at(i) - _y.at(i));
-    }
-    return xpy;
-}
-
-VectorComposite operator -(const wolf::VectorComposite &_x)
-{
-    wolf::VectorComposite m;
-    for (const auto& pair_i_xi : _x)
-    {
-        const auto& i = pair_i_xi.first;
-        m.emplace(i, - _x.at(i));
-    }
-    return m;
-}
-
-void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
-{
-    int index = 0;
-    auto size_it = _sizes.begin();
-    for ( const auto& key : _structure)
-    {
-        const auto& size    = *size_it;
-
-        (*this)[key]        = _v.segment(index,size);
-
-        index += size;
-        size_it ++;
-    }
-}
-
-void VectorComposite::setZero()
-{
-    for (auto& pair_key_vec : *this)
-        pair_key_vec.second.setZero();
-}
-
 ////// MATRIX COMPOSITE //////////
 
 bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk)
diff --git a/src/state_block/vector_composite.cpp b/src/state_block/vector_composite.cpp
new file mode 100644
index 000000000..f01efc485
--- /dev/null
+++ b/src/state_block/vector_composite.cpp
@@ -0,0 +1,167 @@
+//--------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--------
+
+#include "core/state_block/vector_composite.h"
+
+namespace wolf{
+
+VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
+{
+    auto size_it = _sizes.begin();
+    for ( const auto& key : _structure)
+    {
+        const auto& size    = *size_it;
+
+        this->emplace(key, VectorXd(size).setZero());
+
+        size_it ++;
+    }
+}
+
+VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+    int index = 0;
+    auto size_it = _sizes.begin();
+    for ( const auto& key : _structure)
+    {
+        const auto& size    = *size_it;
+
+        (*this)[key]        = _v.segment(index,size);
+
+        index += size;
+        size_it ++;
+    }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _s)
+{
+    for (const auto& key : _s)
+    {
+        this->emplace(key,VectorXd(0));
+    }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors)
+{
+    assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch");
+
+    auto vector_it = _vectors.begin();
+
+    for (const auto& key : _structure)
+    {
+        this->emplace(key, *vector_it);
+        vector_it++;
+    }
+}
+
+
+Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
+{
+    // traverse once with unordered_map access
+    std::vector<const VectorXd*> vp;
+    unsigned int size = 0;
+    for (const auto& key : _structure)
+    {
+        vp.push_back(&(this->at(key)));
+        size +=  vp.back()->size();
+    }
+
+    Eigen::VectorXd x(size);
+
+    // traverse once linearly
+    unsigned int index = 0;
+    for (const auto& blkp : vp)
+    {
+        x.segment(index, blkp->size()) = *blkp;
+        index += blkp->size();
+    }
+    return x;
+}
+
+std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
+{
+    for (const auto &pair_key_vec : _x)
+    {
+        const auto &key = pair_key_vec.first;
+        const auto &vec = pair_key_vec.second;
+        _os << key << ": (" << vec.transpose() << ");  ";
+    }
+    return _os;
+}
+
+// wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
+// {
+//     wolf::VectorComposite xpy;
+//     for (const auto& pair_i_xi : _x)
+//     {
+//         const auto& i = pair_i_xi.first;
+
+//         xpy.emplace(i, _x.at(i) + _y.at(i));
+//     }
+//     return xpy;
+// }
+
+// VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y)
+// {
+//     VectorComposite xpy;
+//     for (const auto& pair_i_xi : _x)
+//     {
+//         const auto& i = pair_i_xi.first;
+
+//         xpy.emplace(i, _x.at(i) - _y.at(i));
+//     }
+//     return xpy;
+// }
+
+// VectorComposite operator -(const wolf::VectorComposite &_x)
+// {
+//     wolf::VectorComposite m;
+//     for (const auto& pair_i_xi : _x)
+//     {
+//         const auto& i = pair_i_xi.first;
+//         m.emplace(i, - _x.at(i));
+//     }
+//     return m;
+// }
+
+void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+    int index = 0;
+    auto size_it = _sizes.begin();
+    for ( const auto& key : _structure)
+    {
+        const auto& size    = *size_it;
+
+        (*this)[key]        = _v.segment(index,size);
+
+        index += size;
+        size_it ++;
+    }
+}
+
+void VectorComposite::setZero()
+{
+    for (auto& pair_key_vec : *this)
+        pair_key_vec.second.setZero();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index bcec7f0ba..b63823360 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -62,7 +62,7 @@ class MotionProviderTest : public testing::Test
             ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>();
             prc1_params->time_tolerance = dt/2;
             prc1_params->state_structure = "PO";
-            prc1_params->state_getter = false;
+            prc1_params->state_provider = false;
             prc1 = problem->installProcessor("MotionProviderDummy",
                                              "not getter processor",
                                              sen,
@@ -72,8 +72,8 @@ class MotionProviderTest : public testing::Test
             ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>();
             prc2_params->time_tolerance = dt/2;
             prc2_params->state_structure = "PO";
-            prc2_params->state_getter = true;
-            prc2_params->state_priority = 1;
+            prc2_params->state_provider = true;
+            prc2_params->state_order = 1;
             prc2 = problem->installProcessor("MotionProviderDummy",
                                              "getter processor",
                                              sen,
@@ -83,8 +83,8 @@ class MotionProviderTest : public testing::Test
             ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>();
             prc3_params->time_tolerance = dt/2;
             prc3_params->state_structure = "POV";
-            prc3_params->state_getter = true;
-            prc3_params->state_priority = 1;
+            prc3_params->state_provider = true;
+            prc3_params->state_order = 1;
             prc3 = problem->installProcessor("MotionProviderDummy",
                                              "getter processor low priority",
                                              sen,
@@ -95,12 +95,12 @@ class MotionProviderTest : public testing::Test
 
 /*
  * Things to be tested:
- * - state_getter
- * - state_priority
+ * - state_provider
+ * - state_order
  * - Duplicated priority (Problem handles this)
  * - setOdometry (stateStructures)
  * - getOdomtry
- * - Problem::getState/getOdometry (state_getter, state_priority)
+ * - Problem::getState/getOdometry (state_provider, state_order)
  */
 
 TEST_F(MotionProviderTest, install)
@@ -117,8 +117,8 @@ TEST_F(MotionProviderTest, install)
     ASSERT_FALSE(im1->isStateGetter());
     ASSERT_TRUE(im2->isStateGetter());
     ASSERT_TRUE(im3->isStateGetter());
-    ASSERT_EQ(im2->getStatePriority(), 1);
-    ASSERT_EQ(im3->getStatePriority(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
+    ASSERT_EQ(im2->getOrder(), 1);
+    ASSERT_EQ(im3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
     ASSERT_EQ(im1->getStateStructure(), "PO");
     ASSERT_EQ(im2->getStateStructure(), "PO");
     ASSERT_EQ(im3->getStateStructure(), "POV");
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 6c8dbf4ac..1b65d7c04 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -151,7 +151,7 @@ TEST(Problem, SetOrigin_PO_2d)
     ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
 
     // check that the state is correct
-    ASSERT_POSE2d_APPROX(prior.getState().vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
+    ASSERT_POSE2d_APPROX(prior.getVectorComposite().vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
 
     // check that we have one frame, one capture, one feature, one factor
     TrajectoryBasePtr T = P->getTrajectory();
@@ -180,7 +180,7 @@ TEST(Problem, SetOrigin_PO_2d)
         ASSERT_FALSE(fac->getCaptureOther());
         ASSERT_FALSE(fac->getFeatureOther());
     }
-    auto x0_vector = prior.getState().vector("PO");
+    auto x0_vector = prior.getVectorComposite().vector("PO");
     auto P0_p = prior.at('P').getNoiseStd().cwiseAbs2().asDiagonal();
     auto P0_o = prior.at('O').getNoiseStd().cwiseAbs2().asDiagonal();
 
@@ -214,7 +214,7 @@ TEST(Problem, SetOrigin_PO_3d)
     ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
 
     // check that the state is correct
-    ASSERT_TRUE((prior.getState().vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
+    ASSERT_TRUE((prior.getVectorComposite().vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
 
     // check that we have one frame, one capture, one feature, one factor
     TrajectoryBasePtr T = P->getTrajectory();
@@ -244,7 +244,7 @@ TEST(Problem, SetOrigin_PO_3d)
         ASSERT_FALSE(fac->getFeatureOther());
     }
 
-    auto x0_vector = prior.getState().vector("PO");
+    auto x0_vector = prior.getVectorComposite().vector("PO");
     auto P0_p = prior.at('P').getNoiseStd().cwiseAbs2().asDiagonal();
     auto P0_o = prior.at('O').getNoiseStd().cwiseAbs2().asDiagonal();
     
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index 17ad54a8c..5a6272fe4 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -32,7 +32,7 @@
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
 #include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_composite.h"
+#include "core/state_block/vector_composite.h"
 
 #include <iostream>
 
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index bcb2c0832..f077c1608 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -308,50 +308,50 @@ TEST(VectorComposite, operatorStream)
     WOLF_DEBUG("X = ", x);
 }
 
-TEST(VectorComposite, operatorPlus)
-{
-    VectorComposite x, y;
+// TEST(VectorComposite, operatorPlus)
+// {
+//     VectorComposite x, y;
 
-    x.emplace('P', Vector2d(1,1));
-    x.emplace('O', Vector3d(2,2,2));
-    y.emplace('P', Vector2d(1,1));
-    y.emplace('O', Vector3d(2,2,2));
+//     x.emplace('P', Vector2d(1,1));
+//     x.emplace('O', Vector3d(2,2,2));
+//     y.emplace('P', Vector2d(1,1));
+//     y.emplace('O', Vector3d(2,2,2));
 
-    WOLF_DEBUG("x + y = ", x + y);
+//     WOLF_DEBUG("x + y = ", x + y);
 
-    ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20);
-    ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20);
-}
+//     ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20);
+//     ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20);
+// }
 
-TEST(VectorComposite, operatorMinus)
-{
-    VectorComposite x, y;
+// TEST(VectorComposite, operatorMinus)
+// {
+//     VectorComposite x, y;
 
-    x.emplace('P', Vector2d(3,3));
-    x.emplace('O', Vector3d(6,6,6));
-    y.emplace('P', Vector2d(1,1));
-    y.emplace('O', Vector3d(2,2,2));
+//     x.emplace('P', Vector2d(3,3));
+//     x.emplace('O', Vector3d(6,6,6));
+//     y.emplace('P', Vector2d(1,1));
+//     y.emplace('O', Vector3d(2,2,2));
 
-    WOLF_DEBUG("x = ", x);
-    WOLF_DEBUG("y = ", y);
-    WOLF_DEBUG("x - y = ", x - y);
+//     WOLF_DEBUG("x = ", x);
+//     WOLF_DEBUG("y = ", y);
+//     WOLF_DEBUG("x - y = ", x - y);
 
-    ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20);
-    ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20);
-}
+//     ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20);
+//     ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20);
+// }
 
-TEST(VectorComposite, unary_Minus)
-{
-    VectorComposite x, y;
+// TEST(VectorComposite, unary_Minus)
+// {
+//     VectorComposite x, y;
 
-    x.emplace('P', Vector2d(1,1));
-    x.emplace('O', Vector3d(2,2,2));
+//     x.emplace('P', Vector2d(1,1));
+//     x.emplace('O', Vector3d(2,2,2));
 
-    WOLF_DEBUG("-x = ", -x);
+//     WOLF_DEBUG("-x = ", -x);
 
-    ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20);
-    ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20);
-}
+//     ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20);
+//     ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20);
+// }
 
 TEST(VectorComposite, stateVector)
 {
-- 
GitLab