diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index c02474f8924ba06224221dc651f08e259bf92095..358c86895c1c8fcfdf026548925e5d290640ca7b 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -398,20 +398,6 @@ inline void Factory<TypeBase, TypeInput...>::printCallbacks()
 namespace wolf
 {
 
-// Some specializations
-//======================
-
-// Landmarks from YAML
-class LandmarkBase;
-typedef Factory<std::shared_ptr<LandmarkBase>,
-        const YAML::Node&>  FactoryLandmark;
-template<>
-inline std::string FactoryLandmark::getClass() const
-{
-    return "FactoryLandmark";
-}
-
-
 #ifdef __GNUC__
     #define WOLF_UNUSED __attribute__((used))
 #elif defined _MSC_VER
diff --git a/include/core/landmark/factory_landmark.h b/include/core/landmark/factory_landmark.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e162739e35601302db94a26fd31c246f5a52b97
--- /dev/null
+++ b/include/core/landmark/factory_landmark.h
@@ -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--------
+#pragma once
+
+namespace wolf
+{
+class LandmarkBase;
+}
+
+#include <unordered_map>
+
+// wolf
+#include "core/common/factory.h"
+
+// yaml
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+
+/** \brief Landmark factory class
+ *
+ * This factory can create objects of classes deriving from LandmarkBase.
+ *
+ * Specific object creation is invoked by `create(TYPE, ... )`, and the TYPE of landmark is identified with a string.
+ *
+ * Find general Factory documentation in class Factory:
+ *   - Access the factory
+ *   - Register/unregister creators
+ *   - Invoke object creation
+ *
+ * This documentation shows you how to use the FactoryLandmark specifically:
+ *   - Write landmark creators.
+ *   - Create landmarks
+ *
+ * #### Write landmark creators
+ * Landmark creators have the following API:
+ *
+ *     \code
+ *     static LandmarkBasePtr create(const YAML::Node&);
+ *     \endcode
+ *
+ * They follow the general implementations shown below:
+ *
+ *     \code
+ *      static LandmarkBasePtr create(const YAML::Node& _node)
+ *      {
+ *          return std::make_shared<LandmarkDerived>("LandmarkDerived", _node);                              
+ *      }
+ *     \endcode
+ *
+ * #### Creating landmarks
+ * Note: Prior to invoking the creation of a landmark of a particular type,
+ * you must register the creator for this type into the factory.
+ *
+ * To create e.g. a LandmarkCorner in 3D, you type:
+ *
+ *     \code
+ *     auto corner_ptr = FactoryLandmark::create("LandmarkCorner", _node);
+ *     \endcode
+ *
+ * We RECOMMEND using the macro WOLF_LANDMARK_CREATE(LandmarkClass) to automatically add the landmark creator, provided in 'landmark_base.h'.
+ * 
+ * To do so, you should implement a constructor with the API:
+ * 
+ *     \code
+ *     LandmarkDerived(const YAML::Node& _node) : 
+ *       LandmarkBase("LandmarkDerived", _node)
+ *     {
+ *        < CODE >
+ *     }
+ *     \endcode
+ *
+ * #### Registering landmark creator into the factory
+ * Registration can be done manually or automatically. It involves the call to static functions.
+ * It is advisable to put these calls within unnamed namespaces.
+ *
+ *   - __Manual registration__: you control registration at application level.
+ *   Put the code either at global scope (you must define a dummy variable for this),
+ *      \code
+ *      namespace {
+ *      const bool registered_corner = FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create);
+ *      }
+ *      main () { ... }
+ *      \endcode
+ *   or inside your main(), where a direct call is possible:
+ *      \code
+ *      main () {
+ *          FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create);
+ *          ...
+ *      }
+ *      \endcode
+ *
+ *   - __Automatic registration__: registration is performed at library level.
+ *   Put the code at the last line of the landmark_xxx.cpp file,
+ *      \code
+ *      namespace {
+ *      const bool registered_corner = FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create);
+ *      }
+ *      \endcode
+ * 
+ *   Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro
+ *   WOLF_REGISTER_LANDMARK(LandmarkType).
+ *   You are free to comment out these lines and place them wherever you consider it more convenient for your design.
+ *
+ * #### Example 2: creating landmarks
+ * We finally provide the necessary steps to create a landmark of class LandmarkCorner in our application:
+ *
+ *  \code
+ *  #include "core/landmark/factory_landmark.h"
+ *  #include "vision/landmark/landmark_corner.h" // provides LandmarkCorner
+ *
+ *  // Note: LandmarkCorner::create() is already registered, automatically.
+ *
+ *  using namespace wolf;
+ *  int main() {
+ *
+ *      // To create a corner, provide:
+ *      //    a type = "LandmarkCorner",
+ *      //    a YAML node with the info
+ *
+ *      LandmarkBasePtr corner_1_ptr =
+ *          FactoryLandmark::create ( "LandmarkCorner", 
+ *                                    node );
+ *
+ *      LandmarkBasePtr corner_2_ptr =
+ *          FactoryLandmark::create( "LandmarkCorner" ,
+ *                                   node2 );
+ *
+ *      return 0;
+ *  }
+ *  \endcode
+ */
+
+typedef Factory<std::shared_ptr<LandmarkBase>,
+                const YAML::Node&> FactoryLandmark;
+
+template<>
+inline std::string FactoryLandmark::getClass() const
+{
+  return "FactoryLandmark";
+}
+
+#define WOLF_REGISTER_LANDMARK(LandmarkType)                                  \
+  namespace{ const bool WOLF_UNUSED LandmarkType##Registered =                \
+     FactoryLandmark::registerCreator(#LandmarkType, LandmarkType::create);}  \
+
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 3f7bd7adb38a3f263361bd206534e6f06a63a691..fc62fc8068317a66ae97f47320d85b532eca672d 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -39,6 +39,22 @@ class StateBlock;
 #include "yaml-cpp/yaml.h"
 
 namespace wolf {
+/*
+ * Macro for defining Autoconf landmark creator.
+ *
+ * Place a call to this macro inside your class declaration (in the landmark_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived landmark class, LandmarkClass,
+ * must have two constructors available with the API:
+ *
+ *   LandmarkClass(const YAML::Node& _node);
+ */
+#define WOLF_LANDMARK_CREATE(LandmarkClass)             \
+static LandmarkBasePtr create(const YAML::Node& _node)  \
+{                                                       \
+    return std::make_shared<LandmarkClass>(_node);      \
+}                                                       \
 
 //class LandmarkBase
 class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<LandmarkBase>
@@ -48,9 +64,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
     private:
         MapBaseWPtr map_ptr_;
         FactorBasePtrList constrained_by_list_;
-        //std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
-        //std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
-
         static unsigned int landmark_id_count_;
 
     protected:
@@ -58,7 +71,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
         void setProblem(ProblemPtr _problem) override final;
         unsigned int landmark_id_; ///< landmark unique id
         TimeStamp stamp_;       ///< stamp of the creation of the landmark
-        Eigen::VectorXd descriptor_;    //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
 
     public:
 
@@ -68,27 +80,37 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
          * \param _tp indicates landmark type.(types defined at wolf.h)
          * \param _p_ptr StateBlock pointer to the position
          * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
-         *
          **/
         LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
+
+        /** \brief Constructor with type and a YAML node (to be used by the derived classes YAML node constructors)
+         *
+         * Constructor with type, and YAML node
+         * \param _tp indicates landmark type.(types defined at wolf.h)
+         * \param _n YAML node containing the necessary information
+         **/
+        LandmarkBase(const std::string& _type, const YAML::Node& _n);
+
+        /** \brief Constructor with YAML node
+         *
+         * Constructor with YAML node (type assumed "LandmarkBase")
+         * \param _n YAML node containing the necessary information
+         **/
+        LandmarkBase(const YAML::Node& _n);
+        WOLF_LANDMARK_CREATE(LandmarkBase);
+
         ~LandmarkBase() override;
         virtual void remove(bool viral_remove_empty_parent=false);
-        virtual YAML::Node saveToYaml() const;
+        virtual YAML::Node toYaml() const;
 
         // Properties
         unsigned int id() const;
         void setId(unsigned int _id);
 
         // State blocks
-        //std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
-        //std::vector<StateBlockPtr> getUsedStateBlockVec();
         bool getCovariance(Eigen::MatrixXd& _cov) const;
 
-        // Descriptor
     public:
-        const Eigen::VectorXd& getDescriptor() const;
-        double getDescriptor(unsigned int _ii) const;
-        void setDescriptor(const Eigen::VectorXd& _descriptor);
 
         unsigned int getHits() const;
         FactorBaseConstPtrList getConstrainedByList() const;
@@ -102,12 +124,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
         template<typename classType, typename... T>
         static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
 
-        /** \brief Creator for Factory<LandmarkBase, YAML::Node>
-         * Caution: This creator does not set the landmark's anchor frame and sensor.
-         * These need to be set afterwards.
-         */
-        static LandmarkBasePtr create(const YAML::Node& _node);
-
         virtual void printHeader(int depth, //
                                  bool constr_by, //
                                  bool metric, //
@@ -193,20 +209,4 @@ inline FactorBasePtrList LandmarkBase::getConstrainedByList()
     return constrained_by_list_;
 }
 
-inline void LandmarkBase::setDescriptor(const Eigen::VectorXd& _descriptor)
-{
-    descriptor_ = _descriptor;
-}
-
-inline double LandmarkBase::getDescriptor(unsigned int _ii) const
-{
-    assert(_ii < descriptor_.size() && "LandmarkBase::getDescriptor: bad index");
-    return descriptor_(_ii);
-}
-
-inline const Eigen::VectorXd& LandmarkBase::getDescriptor() const
-{
-    return descriptor_;
-}
-
 } // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/composite.h b/include/core/state_block/composite.h
index 2e1abc2dd54cb29c2fb96139ff381289cda690b0..6fec7d76a82fed670bbf367237213e1fc195d8fe 100644
--- a/include/core/state_block/composite.h
+++ b/include/core/state_block/composite.h
@@ -21,6 +21,7 @@
 //--------LICENSE_END--------
 #pragma once
 
+#include "core/common/wolf.h"
 #include <string>
 #include <map>
 #include <algorithm>
@@ -57,6 +58,8 @@ class Composite : public map<char, T>
             }
             return _os;
         }
+
+        YAML::Node toYaml() const;
 };
 
 template <typename T>
@@ -74,15 +77,22 @@ inline CompositeOther Composite<T>::cast() const
 template <typename T>
 inline Composite<T>::Composite(const YAML::Node& _n)
 {
-    if (not _n.IsMap())
+    WOLF_INFO("constructor composite...");
+    if (_n.IsDefined())
     {
-        throw std::runtime_error("SpecComposite: constructor with a non-map yaml node");
-    }
+        WOLF_INFO("defined...", _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));
+        for (auto spec_pair : _n)
+        {
+            WOLF_INFO("iterating...", spec_pair.first, spec_pair.second);
+            this->emplace(spec_pair.first.as<char>(), T(spec_pair.second));
+        }
     }
+    WOLF_INFO("constructor composite done");
 }
 
 template <typename T>
@@ -114,4 +124,16 @@ inline StateKeys Composite<T>::getKeys() const
     return keys;
 }
 
+template <typename T>
+inline YAML::Node Composite<T>::toYaml() const
+{
+    YAML::Node n;
+    for (auto&& pair : *this)
+    {
+        n[pair.first] = pair.second.toYaml();
+    }
+
+    return n;
+}
+
 }  // 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 d143262215279bf247464e017a7cb842423c351d..71308a397edb94da2f06b628b6838443495a15cd 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -43,6 +43,7 @@ class HasStateBlocks
         virtual ~HasStateBlocks();
 
         StateKeys getKeys() const { return keys_order_; }
+        SpecComposite getSpecs() const;
         void appendToStructure(const char& _new_key){ keys_order_ += _new_key; }
         bool isInStructure(const char& _sb_key) const { return keys_order_.find(_sb_key) != std::string::npos; }
 
diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h
index 688caea505b38e199f24bbd51c370b21cad76f47..81e38cd10165e352ef431ad7c636fb1666c7492f 100644
--- a/include/core/state_block/spec_composite.h
+++ b/include/core/state_block/spec_composite.h
@@ -26,7 +26,6 @@
 #include "core/state_block/vector_composite.h"
 
 #include <string>
-#include <unordered_map>
 #include <eigen3/Eigen/Dense>
 #include "yaml-cpp/yaml.h"
 
@@ -64,6 +63,8 @@ class SpecState
 
         virtual std::string print(const std::string& _spaces = "") const;
         friend std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x);
+
+        YAML::Node toYaml() const;
 };
 
 template <typename T>
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index d33f53a1a923b1380c8a04c1a4266cb434cf1aa1..160ddbce32e96453eb43572db72bfb8f8761d28b 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -43,13 +43,13 @@ class StateAngle : public StateBlock
 };
 
 inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
-        StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
+        StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
 {
     state_(0) = pi2pi(_angle);
 }
 
 inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable) :
-        StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
+        StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
 {
     if(_angle.size() != 1)
         throw std::runtime_error("The angle state vector must be of size 1!");
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index d19d3155a28d8b95e273a3f8f9e9bbde97a8564b..6a65d6447c1a7591cd0158405cc3cfde86473c4c 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -77,6 +77,7 @@ public:
 
     protected:
 
+        std::string type_;                      ///< Type of the derived class
         std::atomic_bool fixed_;                ///< Key to indicate whether the state is fixed or not
 
         std::atomic<SizeEigen> state_size_;     ///< State vector size
@@ -98,7 +99,7 @@ public:
          * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
          * \param _local_param_ptr pointer to the local parametrization for the block
          */
-        StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
+        StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
 
         /** \brief Constructor from vector
          * 
@@ -106,7 +107,7 @@ public:
          * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
          * \param _local_param_ptr pointer to the local parametrization for the block
          **/
-        StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
+        StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
 
         ///< Explicitly not copyable/movable
         StateBlock(const StateBlock& o) = delete;
@@ -117,6 +118,10 @@ public:
          **/
         virtual ~StateBlock();
 
+        /** \brief Returns the type of the state block
+         **/
+        std::string getType() const;
+
         /** \brief Returns the state vector
          **/
         Eigen::VectorXd getState() const;
@@ -229,7 +234,8 @@ public:
 
 namespace wolf {
 
-inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
+inline StateBlock::StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
+        type_(_type),
         fixed_(_fixed),
         state_size_(_state.size()),
         state_(_state),
@@ -242,7 +248,8 @@ inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalP
 //    std::cout << "constructed           +sb" << std::endl;
 }
 
-inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
+inline StateBlock::StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
+        type_(_type),
         fixed_(_fixed),
         state_size_(_size),
         state_(Eigen::VectorXd::Zero(_size)),
@@ -260,6 +267,11 @@ inline StateBlock::~StateBlock()
 //    std::cout << "destructed            -sb" << std::endl;
 }
 
+inline std::string StateBlock::getType() const
+{
+    return type_;
+}
+
 inline Eigen::VectorXd StateBlock::getState() const
 {
     std::lock_guard<std::mutex> lock(mut_state_);
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
index 6f0c50feeb7301ae6e90ea076cf5d311b07418cd..8e07be2c06de256f2d90524d902fc3851ef1c752 100644
--- a/include/core/state_block/state_block_derived.h
+++ b/include/core/state_block/state_block_derived.h
@@ -40,10 +40,10 @@ class StateParams : public StateBlock
 {
   public:
     StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : 
-        StateBlock(_state, _fixed, nullptr, false) 
+        StateBlock("StateParams" + toString(size), _state, _fixed, nullptr, false) 
     {
         if (_state.size() != size) 
-            throw std::runtime_error("Wrong vector size for StateParams.");
+            throw std::runtime_error("Wrong vector size for StateParams" + toString(size) + ":" + toString(_state.size()));
     }
     static Eigen::VectorXd Identity()
     {
@@ -80,10 +80,10 @@ class StatePoint2d : public StateBlock
 {
   public:
     StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
-        : StateBlock(_state, _fixed, nullptr, _transformable)
+        : StateBlock("StatePoint2d", _state, _fixed, nullptr, _transformable)
     {
         if (_state.size() != 2) 
-            throw std::runtime_error("Wrong vector size for StatePoint2d.");
+            throw std::runtime_error("Wrong vector size for StatePoint2d:" + toString(_state.size()));
     }
     static Eigen::VectorXd Identity()
     {
@@ -110,10 +110,10 @@ class StateVector2d : public StateBlock
 {
   public:
     StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
-        : StateBlock(_state, _fixed, nullptr, _transformable)
+        : StateBlock("StateVector2d", _state, _fixed, nullptr, _transformable)
     {
         if (_state.size() != 2) 
-            throw std::runtime_error("Wrong vector size for StateVector2d.");
+            throw std::runtime_error("Wrong vector size for StateVector2d:" + toString(_state.size()));
     }
     static Eigen::VectorXd Identity()
     {
@@ -141,10 +141,10 @@ class StatePoint3d : public StateBlock
     StatePoint3d(const Eigen::VectorXd& _state, 
                  bool _fixed = false, 
                  bool _transformable = true)
-        : StateBlock(_state, _fixed, nullptr, _transformable)
+        : StateBlock("StatePoint3d", _state, _fixed, nullptr, _transformable)
     {
         if (_state.size() != 3) 
-            throw std::runtime_error("Wrong vector size for StatePoint3d.");
+            throw std::runtime_error("Wrong vector size for StatePoint3d:" + toString(_state.size()));
     }
     static Eigen::VectorXd Identity()
     {
@@ -172,10 +172,10 @@ class StateVector3d : public StateBlock
 {
   public:
     StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
-        : StateBlock(_state, _fixed, nullptr, _transformable)
+        : StateBlock("StateVector3d", _state, _fixed, nullptr, _transformable)
     {
         if (_state.size() != 3) 
-            throw std::runtime_error("Wrong vector size for StateVector3d.");
+            throw std::runtime_error("Wrong vector size for StateVector3d:" + toString(_state.size()));
     }
     static Eigen::VectorXd Identity()
     {
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 357a976044f3574e17d9c405e0119aa338725e22..9d62b312ef2c3c98f2d207725cece7b2ae69ffbc 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -42,7 +42,7 @@ class StateHomogeneous3d : public StateBlock
 };
 
 inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) :
-        StateBlock(_state, _fixed, nullptr, _transformable)
+        StateBlock("StateHomogeneous3d", _state, _fixed, nullptr, _transformable)
 {
     if(_state.size() != 4)
         throw std::runtime_error("Homogeneous 3d must be size 4.");
@@ -50,7 +50,7 @@ inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool
 }
 
 inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) :
-        StateBlock(4, _fixed, nullptr, _transformable)
+        StateBlock("StateHomogeneous3d", 4, _fixed, nullptr, _transformable)
 {
     local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
     state_ << 0, 0, 0, 1; // Set origin
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 13632c59ee2d6f1cd102a9cb7996ffcbff85d1a3..4246e72fb3500bbb5e0ca807566dc2f7eedc54a5 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -43,7 +43,7 @@ class StateQuaternion : public StateBlock
 };
 
 inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) :
-        StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
+        StateBlock("StateQuaternion", _quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
 {
     // TODO: remove these lines after issue #381 is addressed
     if(not isValid(1e-5))
@@ -53,7 +53,7 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b
 }
 
 inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) :
-        StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
+        StateBlock("StateQuaternion", _state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
 {
     if(state_.size() != 4)
         throw std::runtime_error("The quaternion state vector must be of size 4!");
@@ -66,7 +66,7 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix
 }
 
 inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) :
-        StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
+        StateBlock("StateQuaternion", 4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
 {
     state_ = Eigen::Quaterniond::Identity().coeffs();
     //
diff --git a/include/core/state_block/type_composite.h b/include/core/state_block/type_composite.h
index 5835e2e2b232434475249a88ee71feb8afec9079..392f718b57b47dd7b677d10b823e3d352a9c3820 100644
--- a/include/core/state_block/type_composite.h
+++ b/include/core/state_block/type_composite.h
@@ -41,10 +41,24 @@ inline Composite<std::string>::Composite(const YAML::Node& _n)
 
     for (auto spec_pair : _n)
     {
-        this->emplace(spec_pair.first.as<char>(), spec_pair.second.as<std::string>());
+        this->emplace(spec_pair.first.as<char>(), spec_pair.second["type"].as<std::string>());
     }
 }
 
+template <>
+inline YAML::Node Composite<std::string>::toYaml() const
+{
+    YAML::Node n;
+    for (auto&& pair : *this)
+    {
+        YAML::Node n_type;
+        n_type["type"] = pair.second; 
+        n[pair.first] = n_type;
+    }
+
+    return n;
+}
+
 inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim)
 {
     try
diff --git a/include/core/state_block/vector_composite.h b/include/core/state_block/vector_composite.h
index ebed1706395b60e87650c1d621bad052a8c44b45..5e1447f858b63a10610b764c2e3e5bdced073b58 100644
--- a/include/core/state_block/vector_composite.h
+++ b/include/core/state_block/vector_composite.h
@@ -24,6 +24,8 @@
 #include "core/common/wolf.h"
 #include "core/state_block/composite.h"
 
+#include "yaml-schema-cpp/yaml_conversion.hpp"
+
 #include <unordered_map>
 #include <iostream>
 
@@ -81,4 +83,34 @@ class VectorComposite : public Composite<Eigen::VectorXd>
         // friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x);
 };
 
+template <>
+inline Composite<Eigen::VectorXd>::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>(), spec_pair.second["state"].as<Eigen::VectorXd>());
+    }
+}
+
+template <>
+inline YAML::Node Composite<Eigen::VectorXd>::toYaml() const
+{
+    YAML::Node n;
+    for (auto&& pair : *this)
+    {
+        YAML::Node n_type;
+        n_type["state"] = pair.second; 
+        n[pair.first] = n_type;
+    }
+
+    return n;
+}
+
+
+
 }
\ No newline at end of file
diff --git a/schema/sensor/SensorMotionModel.schema b/schema/sensor/SensorMotionModel.schema
new file mode 100644
index 0000000000000000000000000000000000000000..9ff4a8f839cc1bec658f6c111fc6214889d29b27
--- /dev/null
+++ b/schema/sensor/SensorMotionModel.schema
@@ -0,0 +1 @@
+follow: SensorBase.schema
\ No newline at end of file
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 82f91377428bb34ab7b46c29bac5b1adc5890626..bd9a2fe5f13657b8bafd79df7547645647d0e3f4 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -38,7 +38,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
         NodeBase("LANDMARK", _type),
         HasStateBlocks(""),
         map_ptr_(),
-        //state_block_vec_(0), // Resize in derived constructors if needed.
         landmark_id_(++landmark_id_count_)
 {
     if (_p_ptr)
@@ -52,6 +51,22 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
 
 }
 
+LandmarkBase::LandmarkBase(const std::string& _type, const YAML::Node& _n) :
+        NodeBase("LANDMARK", _type),
+        HasStateBlocks(SpecComposite(_n["states"])),
+        map_ptr_(),
+        landmark_id_(_n["id"].as<int>())
+{
+}
+
+LandmarkBase::LandmarkBase(const YAML::Node& _n) :
+        NodeBase("LANDMARK", "LandmarkBase"),
+        HasStateBlocks(SpecComposite(_n["states"])),
+        map_ptr_(),
+        landmark_id_(_n["id"].as<int>())
+{
+}
+
 LandmarkBase::~LandmarkBase()
 {
     removeStateBlocks(getProblem());
@@ -85,66 +100,17 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
     }
 }
 
-// std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
-// {
-//     std::vector<StateBlockConstPtr> used_state_block_vec(0);
-
-//     // normal state blocks in {P,O,V,W}
-//     for (auto key : getKeys())
-//     {
-//         const auto sbp = getStateBlock(key);
-//         if (sbp)
-//             used_state_block_vec.push_back(sbp);
-//     }
-
-//     // // other state blocks in a vector
-//     // for (auto sbp : state_block_vec_)
-//     //     if (sbp)
-//     //         used_state_block_vec.push_back(sbp);
-
-//     return used_state_block_vec;
-// }
-
-// std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec()
-// {
-//     std::vector<StateBlockPtr> used_state_block_vec(0);
-
-//     // normal state blocks in {P,O,V,W}
-//     for (auto key : getKeys())
-//     {
-//         auto sbp = getStateBlock(key);
-//         if (sbp)
-//             used_state_block_vec.push_back(sbp);
-//     }
-
-//     // // other state blocks in a vector
-//     // for (auto sbp : state_block_vec_)
-//     //     if (sbp)
-//     //         used_state_block_vec.push_back(sbp);
-
-//     return used_state_block_vec;
-// }
-
 bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
 {
     return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
 }
 
-YAML::Node LandmarkBase::saveToYaml() const
+YAML::Node LandmarkBase::toYaml() const
 {
     YAML::Node node;
     node["id"] = landmark_id_;
     node["type"] = node_type_;
-    if (getP() != nullptr)
-    {
-        node["position"] = getP()->getState();
-        node["position fixed"] = getP()->isFixed();
-    }
-    if (getO() != nullptr)
-    {
-        node["orientation"] = getO()->getState();
-        node["orientation fixed"] = getO()->isFixed();
-    }
+    node["states"] = HasStateBlocks::getSpecs().toYaml();
     return node;
 }
 
@@ -211,31 +177,6 @@ void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
 {
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
 }
-LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
-{
-    unsigned int    id          = _node["id"]               .as< unsigned int     >();
-    Eigen::VectorXd pos         = _node["position"]         .as< Eigen::VectorXd  >();
-    bool            pos_fixed   = _node["position fixed"]   .as< bool >();
-
-    StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
-    StateBlockPtr ori_sb = nullptr;
-
-    if (_node["orientation"])
-    {
-        Eigen::VectorXd ori       = _node["orientation"].as< Eigen::VectorXd >();
-        bool            ori_fixed = _node["orientation fixed"].as< bool >();
-
-        if (ori.size() == 4)
-            ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed);
-        else
-            ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed);
-    }
-
-    LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("LandmarkBase", pos_sb, ori_sb);
-    lmk->setId(id);
-
-    return lmk;
-}
 
 CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
 {
@@ -324,10 +265,11 @@ bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbo
 
     return _log.is_consistent_;
 }
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create);
-}
 
 } // namespace wolf
+
+// Register landmark creator
+#include "core/landmark/factory_landmark.h"
+namespace wolf {
+WOLF_REGISTER_LANDMARK(LandmarkBase);
+}
\ No newline at end of file
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index ebd6038e83a51604023853e17d78dede44e8e34d..c2a59554ae320987eeb8b71acd30107881b95b5b 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -23,7 +23,7 @@
 // wolf
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
-#include "core/common/factory.h"
+#include "core/landmark/factory_landmark.h"
 
 // YAML
 #include <yaml-cpp/yaml.h>
@@ -79,7 +79,6 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
         LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
         lmk_ptr->link(shared_from_this());
     }
-
 }
 
 void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name)
@@ -96,7 +95,7 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
 
     for (LandmarkBasePtr lmk_ptr : getLandmarkList())
     {
-        emitter << YAML::Flow << lmk_ptr->saveToYaml();
+        emitter << YAML::Flow << lmk_ptr->toYaml();
     }
     emitter << YAML::EndSeq << YAML::EndMap;
 
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 84668cfff6d8ba7f9996255bef11d82d3bc0ad8c..b7e3568c608a36084b3d04ff95e6f4239a0e7dad 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -93,6 +93,18 @@ HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposit
     }
 }
 
+SpecComposite HasStateBlocks::getSpecs() const
+{
+    SpecComposite specs;
+    for (auto && state_pair : state_block_map_)
+    {
+        specs.emplace(state_pair.first, SpecState(state_pair.second->getType(), 
+                                                  state_pair.second->getState(), 
+                                                  state_pair.second->isFixed() ? "fix" : "initial_guess"));
+    }
+    return specs;
+}
+
 StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem)
 {
     assert(state_block_map_.count(_sb_key) == 0 and
diff --git a/src/state_block/spec_composite.cpp b/src/state_block/spec_composite.cpp
index 00234e6c75f5cf26eec05e033402a81637d6a639..7d95b118b27bfddc044e097666e8c59334902bfa 100644
--- a/src/state_block/spec_composite.cpp
+++ b/src/state_block/spec_composite.cpp
@@ -87,4 +87,16 @@ std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x)
     return _os;
 }
 
+YAML::Node SpecState::toYaml() const
+{
+    YAML::Node node;
+    node["type"]    = type_;
+    node["state"]   = state_;
+    node["mode"]    = mode_;
+    if (mode_ == "factor") 
+      node["noise_std"] = noise_std_;
+
+    return node;
+}
+
 }  // namespace wolf
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 3951bf2f092fdcce0c518bcabfb7d9d46e66325f..782a06f05f0cd39dac17e2bdf74047d8161b878d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -159,32 +159,32 @@ wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
 # FactorPose3d class test
 wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
 
-# # FactorRelativePose2d class test
-# wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
+# FactorRelativePose2d class test
+wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
 
-# # FactorRelativePose2dWithExtrinsics class test
-# wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
+# FactorRelativePose2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
 
-# # FactorRelativePose3d class test
-# wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
+# FactorRelativePose3d class test
+wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
 
-# # FactorRelativePose3dWithExtrinsics class test
-# wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+# FactorRelativePose3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
 
-# # FactorVelocityLocalDirection3d class test
-# wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
+# FactorVelocityLocalDirection3d class test
+wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
 
-# # MakePosDef function test
-# wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
+# MakePosDef function test
+wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
 
-# # Map yaml save/load test
-# wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
+# Map yaml save/load test
+wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
 
-# # Parameter prior test
-# wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
+# Parameter prior test
+wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
 
-# # ProcessorFixedWingModel class test
-# wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+# ProcessorFixedWingModel class test
+wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
 
 # # ProcessorDiffDrive class test
 # wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
diff --git a/test/dummy/landmark_dummy.h b/test/dummy/landmark_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..70aa2eb93ecbb1edf9228a0433356e128ca5b6bf
--- /dev/null
+++ b/test/dummy/landmark_dummy.h
@@ -0,0 +1,96 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#pragma once
+
+//Wolf includes
+#include "core/landmark/landmark_base.h"
+#include "core/landmark/factory_landmark.h"
+
+#include "core/state_block/state_quaternion.h"
+
+// Std includes
+
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(LandmarkDummy);
+    
+//class LandmarkDummy
+class LandmarkDummy : public LandmarkBase
+{
+    public:
+		LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param);
+		LandmarkDummy(const YAML::Node& _node);
+        WOLF_LANDMARK_CREATE(LandmarkDummy);
+
+        ~LandmarkDummy() override = default;
+        
+        int getIntParam() const;
+        double getDoubleParam() const;
+
+        YAML::Node toYaml() const override;
+
+    private:
+        const int int_param_;
+        const double double_param_;         
+        
+};
+
+
+inline LandmarkDummy::LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param) :
+    LandmarkBase("LandmarkDummy", _p_ptr, _o_ptr),
+    int_param_(_int_param),
+    double_param_(_double_param)
+{
+}
+
+inline LandmarkDummy::LandmarkDummy(const YAML::Node& _node) :
+    LandmarkBase("LandmarkDummy", _node),
+    int_param_(_node["int_param"].as<int>()),
+    double_param_(_node["double_param"].as<double>())
+{
+}
+
+inline int LandmarkDummy::getIntParam() const
+{
+    return int_param_;
+}
+
+inline double LandmarkDummy::getDoubleParam() const
+{
+    return double_param_;
+}
+
+inline YAML::Node LandmarkDummy::toYaml() const
+{
+    YAML::Node node = LandmarkBase::toYaml();
+
+    node["int_param"] = int_param_;
+    node["double_param"] = double_param_;
+
+    return node;
+}
+
+WOLF_REGISTER_LANDMARK(LandmarkDummy);
+
+} // namespace wolf
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index 63eed7c281b4bfb8c5ae3ba213c2fc49726a151b..15a042316c76bba2fe4c5aefcfb87cad01c31cba 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -40,8 +40,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
 SolverCeres solver(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, "PO", Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, "PO", Vector3d::Zero());
 
 // Capture from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index 86578050a1fb1375fe9e18aa1fb518db07bdb4b4..c15db358f0ec0ef6cade5137841c7faf23f1fdbb 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -43,12 +43,12 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
 SolverCeres solver(problem_ptr);
 
 // Sensor
-auto sensor_odom2d = problem_ptr->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
-auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
+auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+auto processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", Vector3d::Zero() );
 
 // Capture from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 524255e52010730fad55fecdd5851a1317faa98f..a0ebf1390e044af47b9167334b937e9bea2e74ad 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -59,7 +59,7 @@ SolverCeres solver(problem_ptr);
 
 // Two frames
 FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "PO", delta);
 
 // Capture, feature and factor from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index d9be016dd463ede52c70e9213705a563770c387c..d5f2b0b04782ec0e2c45f9a4ccbbee35b2048c9f 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -36,7 +36,7 @@ using namespace Eigen;
 using namespace wolf;
 using std::static_pointer_cast;
 
-string wolf_root = _WOLF_ROOT_DIR;
+std::string wolf_root = _WOLF_ROOT_DIR;
 
 // Use the following in case you want to initialize tests with predefines variables or methods.
 class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
@@ -116,17 +116,16 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
             solver = std::make_shared<SolverCeres>(problem);
 
             // Create sensor to be able to initialize
-            S = problem->installSensor("SensorPose", 
-                                       "sensor_pose_1", 
-                                       std::make_shared<ParamsSensorPose>(),
-                                       SpecSensorComposite{{'P',SpecStateSensor("P",pos_camera)},
-                                              {'O',SpecStateSensor("O",vquat_camera)}});
+            S = SensorBase::emplace<SensorPose3d>(problem->getHardware(), 
+                                                  std::make_shared<ParamsSensorPose>(),
+                                                  SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",pos_camera)},
+                                                                      {'O',SpecStateSensor("StateQuaternion",vquat_camera)}});
 
             // F1 is be origin KF
-            VectorComposite x0(pose_robot, "PO", {3,4});
-            VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
-            F1 = problem->setPriorFactor(x0, s0, 0.0);
-
+            SpecComposite prior;
+            prior.emplace('P',SpecState("StatePoint3d", pos_robot, "factor", Vector3d::Ones()));
+            prior.emplace('O',SpecState("StateQuaternion", vquat_robot, "factor", Vector3d::Ones()));
+            F1 = problem->setPrior(prior, 0.0);
 
             // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world)
             meas_cov = Eigen::Matrix6d::Identity();
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index be2eb8584a83b27504239c8b784119e07c4e9528..85c994571cd1336630891d650e0621946e157f47 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -57,7 +57,7 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
             solver = std::make_shared<SolverCeres>(problem, params_solver);
 
             // Frame
-            frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+            frm = problem->emplaceFrame(0.0, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
             sb_p = frm->getP();
             sb_p->fix();
             sb_o = frm->getO();
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 183d6dfbbc6489dd94da9875e224ef4cbbd072a9..8635bf8b48002c5631ebc1fbdb033847519be214 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -31,6 +31,7 @@
 #include "core/problem/problem.h"
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
+#include "dummy/landmark_dummy.h"
 #include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_angle.h"
 #include "core/state_block/state_quaternion.h"
@@ -39,31 +40,38 @@
 
 #include <iostream>
 using namespace wolf;
+using namespace Eigen;
+
+std::string map_path = std::string(_WOLF_ROOT_DIR) + "/test/yaml";
+
+Vector2d p1_2d = Vector2d::Random();
+Vector2d p2_2d = Vector2d::Random();
+Vector2d p3_2d = Vector2d::Random();
+Vector1d o2_2d = Vector1d::Random();
+Vector1d o3_2d = Vector1d::Random();
+
+Vector3d p1_3d = Vector3d::Random();
+Vector3d p2_3d = Vector3d::Random();
+Vector3d p3_3d = Vector3d::Random();
+Vector4d o2_3d = Vector4d::Random().normalized();
+Vector4d o3_3d = Vector4d::Random().normalized();
+
+int int_param = (int) (Vector1d::Random()(0) * 1e4);
+double double_param = Vector1d::Random()(0) * 1e4;
 
 TEST(MapYaml, save_2d)
 {
     ProblemPtr problem = Problem::create("PO", 2);
 
-    Eigen::Vector2d p1, p2, p3;
-    Eigen::Vector1d o2, o3;
-    p1 << 1, 2;
-    p2 << 3, 4;
-    p3 << 5, 6;
-    o2 << 2;
-    o3 << 3;
-
-    StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1);
-    StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2);
-    StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2(0));
-    StateBlockPtr p3_sb = std::make_shared<StatePoint2d>(p3, true);
-    StateBlockPtr o3_sb = std::make_shared<StateAngle>(o3(0), true);
+    StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1_2d);
+    StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2_2d);
+    StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2_2d(0));
+    StateBlockPtr p3_sb = std::make_shared<StatePoint2d>(p3_2d, true);
+    StateBlockPtr o3_sb = std::make_shared<StateAngle>(o3_2d(0), true);
 
     LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
     LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
-    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, o3_sb);
-
-    std::string wolf_root = _WOLF_ROOT_DIR;
-    std::string map_path  = wolf_root + "/test/yaml";
+    LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param);
 
     // Check Problem functions
     std::string filename  = map_path + "/map_2d_problem.yaml";
@@ -80,113 +88,76 @@ TEST(MapYaml, load_2d)
 {
     ProblemPtr problem = Problem::create("PO", 2);
 
-    std::string wolf_root = _WOLF_ROOT_DIR;
-    std::string map_path  = wolf_root + "/test/yaml";
-
-    // Check Problem functions
-    std::string filename  = map_path + "/map_2d_problem.yaml";
-    std::cout << "Loading map to file: " << filename << std::endl;
-    problem->loadMap(filename);
+    std::list<std::string> maps{"map_2d_problem.yaml", "map_2d_map.yaml"};
 
-    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
-    for (auto lmk : problem->getMap()->getLandmarkList())
+    for (auto map : maps)
     {
-        if (lmk->id() == 1)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() == nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
-        }
-        else if (lmk->id() == 2)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
-            ASSERT_FALSE(lmk->getO()->isFixed());
-        }
-        else if (lmk->id() == 3)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
-            ASSERT_TRUE(lmk->getP()->isFixed());
-            ASSERT_TRUE(lmk->getO()->isFixed());
-        }
-    }
+        std::string filename  = map_path + "/" + map;
+        std::cout << "Loading map to file: " << filename << std::endl;
+        problem->loadMap(filename);
 
-    // empty the map
-    {
-        auto lmk_list = problem->getMap()->getLandmarkList();
-        for (auto lmk : lmk_list)
-            lmk->remove();
-    }
-    ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+        ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
 
-    // Check Map functions
-    filename  = map_path + "/map_2d_map.yaml";
-    std::cout << "Loading map to file: " << filename << std::endl;
-    problem->getMap()->load(filename);
-
-    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
-    for (auto lmk : problem->getMap()->getLandmarkList())
-    {
-        if (lmk->id() == 1)
+        for (auto lmk : problem->getMap()->getLandmarkList())
         {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() == nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
+            if (lmk->id() == 1)
+            {
+                ASSERT_TRUE(lmk->getP() != nullptr);
+                ASSERT_TRUE(lmk->getO() == nullptr);
+                ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p1_2d, 1e-12);
+                ASSERT_FALSE(lmk->getP()->isFixed());
+                ASSERT_EQ(lmk->getType(), "LandmarkBase");
+            }
+            else if (lmk->id() == 2)
+            {
+                ASSERT_TRUE(lmk->getP() != nullptr);
+                ASSERT_TRUE(lmk->getO() != nullptr);
+                ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p2_2d, 1e-12);
+                ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o2_2d, 1e-12);
+                ASSERT_FALSE(lmk->getP()->isFixed());
+                ASSERT_FALSE(lmk->getO()->isFixed());
+                ASSERT_EQ(lmk->getType(), "LandmarkBase");
+            }
+            else if (lmk->id() == 3)
+            {
+                ASSERT_TRUE(lmk->getP() != nullptr);
+                ASSERT_TRUE(lmk->getO() != nullptr);
+                ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p3_2d, 1e-12);
+                ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o3_2d, 1e-12);
+                ASSERT_TRUE(lmk->getP()->isFixed());
+                ASSERT_TRUE(lmk->getO()->isFixed());
+                ASSERT_EQ(lmk->getType(), "LandmarkDummy");
+
+                auto lmk_dummy = std::dynamic_pointer_cast<LandmarkDummy>(lmk);
+                ASSERT_TRUE(lmk_dummy != nullptr);
+                ASSERT_EQ(lmk_dummy->getIntParam(), int_param);
+                ASSERT_NEAR(lmk_dummy->getDoubleParam(), double_param, 1e-12);
+            }
         }
-        else if (lmk->id() == 2)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
-            ASSERT_FALSE(lmk->getO()->isFixed());
-        }
-        else if (lmk->id() == 3)
+
+        // empty the map
         {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
-            ASSERT_TRUE(lmk->getP()->isFixed());
-            ASSERT_TRUE(lmk->getO()->isFixed());
+            auto lmk_list = problem->getMap()->getLandmarkList();
+            for (auto lmk : lmk_list)
+                lmk->remove();
         }
+        ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
     }
 }
+
 TEST(MapYaml, save_3d)
 {
     ProblemPtr problem = Problem::create("PO", 3);
 
-    Eigen::Vector3d p1, p2, p3;
-    Eigen::Vector4d q2, q3;
-    p1 << 1, 2, 3;
-    p2 << 4, 5, 6;
-    p3 << 7, 8, 9;
-    q2 << 0, 1, 0, 0;
-    q3 << 0, 0, 1, 0;
-
-    auto p1_sb = std::make_shared<StatePoint2d>(p1);
-    auto p2_sb = std::make_shared<StatePoint2d>(p2);
-    auto q2_sb = std::make_shared<StateQuaternion>(q2);
-    auto p3_sb = std::make_shared<StatePoint2d>(p3, true);
-    auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
+    auto p1_sb = std::make_shared<StatePoint3d>(p1_3d);
+    auto p2_sb = std::make_shared<StatePoint3d>(p2_3d);
+    auto o2_sb = std::make_shared<StateQuaternion>(o2_3d);
+    auto p3_sb = std::make_shared<StatePoint3d>(p3_3d, true);
+    auto o3_sb = std::make_shared<StateQuaternion>(o3_3d, true);
 
     LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
-    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, q2_sb);
-    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, q3_sb);
-
-    std::string wolf_root = _WOLF_ROOT_DIR;
-    std::string map_path  = wolf_root + "/test/yaml";
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
+    LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param);
 
     // Check Problem functions
     std::string filename  = map_path + "/map_3d_problem.yaml";
@@ -202,100 +173,68 @@ TEST(MapYaml, save_3d)
 TEST(MapYaml, load_3d)
 {
     ProblemPtr problem = Problem::create("PO", 3);
+    
+    std::list<std::string> maps{"map_3d_problem.yaml", "map_3d_map.yaml"};
 
-    std::string wolf_root = _WOLF_ROOT_DIR;
-    std::string map_path  = wolf_root + "/test/yaml";
-
-    // Check Problem functions
-    std::string filename  = map_path + "/map_3d_problem.yaml";
-    std::cout << "Loading map to file: " << filename << std::endl;
-    problem->loadMap(filename);
-
-    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
-    for (auto lmk : problem->getMap()->getLandmarkList())
+    for (auto map : maps)
     {
-        if (lmk->id() == 1)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() == nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
-        }
-        else if (lmk->id() == 2)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
-            ASSERT_FALSE(lmk->getO()->isFixed());
-            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
-        }
-        else if (lmk->id() == 3)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
-            ASSERT_TRUE(lmk->getP()->isFixed());
-            ASSERT_TRUE(lmk->getO()->isFixed());
-            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
-        }
-    }
+        std::string filename  = map_path + "/" + map;
+        std::cout << "Loading map to file: " << filename << std::endl;
+        problem->loadMap(filename);
 
-    // empty the map
-    {
-        auto lmk_list = problem->getMap()->getLandmarkList();
-        for (auto lmk : lmk_list)
-            lmk->remove();
-    }
-    ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
-
-    // Check Map functions
-    filename  = map_path + "/map_3d_map.yaml";
-    std::cout << "Loading map to file: " << filename << std::endl;
-    problem->getMap()->load(filename);
+        ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
 
-    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
-    for (auto lmk : problem->getMap()->getLandmarkList())
-    {
-        if (lmk->id() == 1)
-        {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() == nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
-        }
-        else if (lmk->id() == 2)
+        for (auto lmk : problem->getMap()->getLandmarkList())
         {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
-            ASSERT_FALSE(lmk->getP()->isFixed());
-            ASSERT_FALSE(lmk->getO()->isFixed());
-            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+            if (lmk->id() == 1)
+            {
+                ASSERT_TRUE(lmk->getP() != nullptr);
+                ASSERT_TRUE(lmk->getO() == nullptr);
+                ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p1_3d, 1e-12);
+                ASSERT_FALSE(lmk->getP()->isFixed());
+                ASSERT_EQ(lmk->getType(), "LandmarkBase");
+            }
+            else if (lmk->id() == 2)
+            {
+                ASSERT_TRUE(lmk->getP() != nullptr);
+                ASSERT_TRUE(lmk->getO() != nullptr);
+                ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p2_3d, 1e-12);
+                ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o2_3d, 1e-12);
+                ASSERT_FALSE(lmk->getP()->isFixed());
+                ASSERT_FALSE(lmk->getO()->isFixed());
+                ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+                ASSERT_EQ(lmk->getType(), "LandmarkBase");
+            }
+            else if (lmk->id() == 3)
+            {
+                ASSERT_TRUE(lmk->getP() != nullptr);
+                ASSERT_TRUE(lmk->getO() != nullptr);
+                ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p3_3d, 1e-12);
+                ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o3_3d, 1e-12);
+                ASSERT_TRUE(lmk->getP()->isFixed());
+                ASSERT_TRUE(lmk->getO()->isFixed());
+                ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+                ASSERT_EQ(lmk->getType(), "LandmarkDummy");
+
+                auto lmk_dummy = std::dynamic_pointer_cast<LandmarkDummy>(lmk);
+                ASSERT_TRUE(lmk_dummy != nullptr);
+                ASSERT_EQ(lmk_dummy->getIntParam(), int_param);
+                ASSERT_NEAR(lmk_dummy->getDoubleParam(), double_param, 1e-12);
+            }
         }
-        else if (lmk->id() == 3)
+
+        // empty the map
         {
-            ASSERT_TRUE(lmk->getP() != nullptr);
-            ASSERT_TRUE(lmk->getO() != nullptr);
-            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
-            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
-            ASSERT_TRUE(lmk->getP()->isFixed());
-            ASSERT_TRUE(lmk->getO()->isFixed());
-            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+            auto lmk_list = problem->getMap()->getLandmarkList();
+            for (auto lmk : lmk_list)
+                lmk->remove();
         }
+        ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
     }
 }
 
 TEST(MapYaml, remove_map_files)
 {
-    std::string wolf_root = _WOLF_ROOT_DIR;
-    std::string map_path  = wolf_root + "/test/yaml";
-
     std::string filename  = map_path + "/map_2d_problem.yaml";
     ASSERT_TRUE(std::remove(filename.c_str()) == 0);
     filename  = map_path + "/map_2d_map.yaml";
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 3f4146a10fe9201887f3ab5a0f9c509dd39ae689..caf8b96994445d0b1dede7299d45dcbfdbb9adce 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -46,12 +46,11 @@ Vector7d prior_extrinsics((Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
 Vector7d prior2_extrinsics((Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
 
 ParamsSensorOdomPtr params = std::make_shared<ParamsSensorOdom>();
-SpecSensorComposite priors{{'P',SpecStateSensor("P",initial_extrinsics_p,"initial_guess")},
-              {'O',SpecStateSensor("O",initial_extrinsics_o,"initial_guess")}};
-SensorBasePtr sensor_ptr_ = problem_ptr->installSensor("SensorOdom", 
-                                                       "odometer", 
-                                                       params, 
-                                                       priors);
+SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d",initial_extrinsics_p,"initial_guess")},
+                           {'O',SpecStateSensor("StateQuaternion",initial_extrinsics_o,"initial_guess")}};
+SensorBasePtr sensor_ptr_ = SensorBase::emplace<SensorOdom3d>(problem_ptr->getHardware(),
+                                                              params, 
+                                                              priors);
 
 TEST(ParameterPrior, add_prior_p)
 {
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index 0504619c7489de34b566ab509b569b3f81fa8193..00157a56dd51207f539d1e840a297eb4fc53f68a 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -54,20 +54,15 @@ class ProcessorFixedWingModelTest : public testing::Test
             solver = std::make_shared<SolverCeres>(problem);
 
             // Emplace sensor
-            sensor = problem->installSensor("SensorMotionModel", "sensor_1", wolf_root + "/test/yaml/sensor_pose_3d.yaml");
+            sensor = problem->installSensor("SensorMotionModel", wolf_root + "/test/yaml/sensor_pose_3d.yaml", {wolf_root});
 
             // Emplace processor
             ParamsProcessorFixedWingModelPtr params = std::make_shared<ParamsProcessorFixedWingModel>();
             params->velocity_local = (Vector3d() << 1, 0, 0).finished();
             params->angle_stdev = 0.1;
             params->min_vel_norm = 0;
-            processor = problem->installProcessor("ProcessorFixedWingModel","processor_1", sensor, params);
-        }
-
-        FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)
-        {
-            // new frame
-            return problem->emplaceFrame(ts, x);
+            params->name = "processor_1";
+            processor = ProcessorBase::emplace<ProcessorFixedWingModel>(sensor, params);
         }
 };
 
@@ -79,7 +74,7 @@ TEST_F(ProcessorFixedWingModelTest, setup)
 TEST_F(ProcessorFixedWingModelTest, keyFrameCallback)
 {
     // new frame
-    auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+    auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
 
     // keyframecallback
     problem->keyFrameCallback(frm1, nullptr);
@@ -102,7 +97,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallback)
 TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated)
 {
     // new frame
-    auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+    auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
 
     // keyframecallback
     problem->keyFrameCallback(frm1, nullptr);
@@ -128,7 +123,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated)
 TEST_F(ProcessorFixedWingModelTest, solve_origin)
 {
     // new frame
-    auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+    auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
 
     // keyframecallback
     problem->keyFrameCallback(frm1, nullptr);
diff --git a/test/yaml/sensor_pose_2d.yaml b/test/yaml/sensor_pose_2d.yaml
index d85c3d247fda7b9f26bff9bc60f7271c2e025c53..10c6003e44afdd3a6d3d5762f3318be380fffc35 100644
--- a/test/yaml/sensor_pose_2d.yaml
+++ b/test/yaml/sensor_pose_2d.yaml
@@ -1,3 +1,4 @@
+name: a cool sensor pose 2d
 states:
   P:
     mode: fix
diff --git a/test/yaml/sensor_pose_3d.yaml b/test/yaml/sensor_pose_3d.yaml
index f2acfab188d82beef19a8ec507698ac1b038d19b..249af7bfb925cc97f943e6136b39c2dc08b5b1d3 100644
--- a/test/yaml/sensor_pose_3d.yaml
+++ b/test/yaml/sensor_pose_3d.yaml
@@ -1,3 +1,4 @@
+name: a cool sensor pose 3d
 states:
   P:
     mode: fix