diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4f6d43d2d4c233fef99abf227ec9c41e058f9c84..5765ab7eadbe19ab1479b8dc5c2df49aa7edde26 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -97,16 +97,16 @@ ENDIF()
 
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
-# option(BUILD_EXAMPLES "Build examples" OFF)
+# option(BUILD_DEMOS "Build examples" OFF)
 set(BUILD_TESTS true)
-set(BUILD_EXAMPLES false)
+set(BUILD_DEMOS false)
 
 # Does this has any other interest
 # but for the examples ?
 # yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
+IF(BUILD_DEMOS OR BUILD_TESTS)
   set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
+ENDIF(BUILD_DEMOS OR BUILD_TESTS)
 
 
 #find dependencies.
@@ -115,57 +115,10 @@ FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
 
 FIND_PACKAGE(Threads REQUIRED)
 
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
-    MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
-	FIND_PACKAGE(GLUT REQUIRED)
-	FIND_PACKAGE(pose_state_time REQUIRED)
-  MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
-  MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
+FIND_PACKAGE(Ceres REQUIRED) #Ceres is not required
 
 # YAML with yaml-cpp
 FIND_PACKAGE(YamlCpp REQUIRED)
-IF(YAMLCPP_FOUND)
-    MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-ELSEIF(YAMLCPP_FOUND)
-    MESSAGE("yaml-cpp Library NOT FOUND!")
-ENDIF(YAMLCPP_FOUND)
-
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
-    MESSAGE("glog Library FOUND: glog related sources will be built.")
-    MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
-    MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
-    MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
-FIND_PATH(
-    Suitesparse_INCLUDE_DIRS
-    NAMES SuiteSparse_config.h
-    PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
-   SET(Suitesparse_FOUND TRUE)
-   MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
-   SET(Suitesparse_FOUND FALSE)
-   MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
 
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal)
@@ -184,10 +137,11 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
 message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
 message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
 include_directories("${PROJECT_BINARY_DIR}/conf")
+
 # include spdlog (logging library)
 FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
 IF (SPDLOG_INCLUDE_DIR)
-  INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
+  # INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
   MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
 ELSE (SPDLOG_INCLUDE_DIR)
  MESSAGE(FATAL_ERROR "Could not find spdlog")
@@ -196,32 +150,10 @@ ENDIF (SPDLOG_INCLUDE_DIR)
 INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES("include")
 INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-
-IF(Ceres_FOUND)
-    INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
-    INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-# cereal
-IF(cereal_FOUND)
-    INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-
-IF(Suitesparse_FOUND)
-    INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(GLOG_FOUND)
-INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
-
+INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 
 #HEADERS
 
-
 SET(HDRS_COMMON
   include/core/common/factory.h
   include/core/common/node_base.h
@@ -513,23 +445,21 @@ IF (Ceres_FOUND)
     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
 ENDIF(Ceres_FOUND)
 
-IF (GLOG_FOUND)
-    TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
+# IF (GLOG_FOUND)
+#     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
+# ENDIF (GLOG_FOUND)
 #check if this is done correctly
 
-IF (GLOG_FOUND)
-    IF(BUILD_TESTS)
-        MESSAGE("Building tests.")
-        add_subdirectory(test)
-    ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
+IF(BUILD_TESTS)
+  MESSAGE("Building tests.")
+  add_subdirectory(test)
+ENDIF(BUILD_TESTS)
 
-IF(BUILD_EXAMPLES)
-  #Build examples
+IF(BUILD_DEMOS)
+  #Build demos
   MESSAGE("Building demos.")
   ADD_SUBDIRECTORY(demos)
-ENDIF(BUILD_EXAMPLES)
+ENDIF(BUILD_DEMOS)
 
 
 #install library
@@ -593,10 +523,14 @@ INSTALL(FILES ${PROJECT_NAME}.found
 configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
                "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
 
+configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
+               "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY)
+
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
   DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
 
 INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
+INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
 
 INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
 
diff --git a/PluginsInfo.md b/PluginsInfo.md
index 822109172503bf13012fb4a5b333459f7a1b8e78..33e19f05fa1e96f0699f16b16fd4e13fc6a80240 100644
--- a/PluginsInfo.md
+++ b/PluginsInfo.md
@@ -5,8 +5,8 @@
 ## Installing wolf(core)
 
 ```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/core.git
-cd core
+git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+cd wolf 
 mkdir build & cd build
 cmake ..
 make
@@ -32,8 +32,8 @@ If you want to use the core, you just need to have it installed and in your CMak
 `find_package(wolf REQUIRED)`.
 
 If wolf is indeed installed, this will define two variables
-`${wolf_INCLUDE_DIR}` which will contain the path to the wolf include directory
-and `${wolf_LIBRARY}` which will contain the path to the wolf library.
+`${wolf_INCLUDE_DIRS}` which will contain the path to the wolf include directory
+and `${wolf_LIBRARIES}` which will contain the path to the wolf library.
 
 ## Using wolf plugins
 
@@ -41,13 +41,13 @@ If you also want to use some wolf plugin, you just follow the same procedure, ch
 `find_package(wolf<plugin name> REQUIRED)`.
 
 If the pluging is indeed installed, this will define two variables
-`${wolf<plugin name>_INCLUDE_DIR}` which will contain the path to the plugin's include directory
-and `${wolf<plugin name>_LIBRARY}` which will contain the path to the plugin's library.
+`${wolf<plugin name>_INCLUDE_DIRS}` which will contain the path to the plugin's include directory
+and `${wolf<plugin name>_LIBRARIES}` which will contain the path to the plugin's library.
 
-As an example, suppose that we want to use the _vision_ plugin. First, we will clone and install it
+As an example, suppose that we want to use the _laser_ plugin. First, we will clone and install it
 ```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git
-cd vision
+git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/laser.git
+cd laser 
 mkdir build && cd build
 cmake ..
 make
@@ -56,15 +56,12 @@ sudo make install
 
 Then, in the CMakeLists.txt of the application we are developing we will put the following line
 ```
-find_package(wolfvision REQUIRED)
+find_package(wolflaser REQUIRED)
 ```
 if the plugin has been correctly installed, and thus find_package succeeds, then it will define two variables
 
-- ${wolfvision_INCLUDE_DIR} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_vision`
-- ${wolfvision_LIBRARY} which is the path to the compiled (& linked) library. It should have the value `/usr/local/lib/iri-algorithms/libwolfvision.so`
-
-**IMPORTANT NOTE** For the time being, each plugin is only responsible for finding their own includes and library. This means that for instance wolfvision, which obviously requires
-the _core_ plugin, will not find its own dependencies. It is the responsibility of the programmer to do `find_package(wolf REQUIRED)` to get the includes and library. In the future this will change and each plugin will be responsible for finding all the necessary dependencies.
+- ${wolflaser_INCLUDE_DIRS} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_laser;/usr/local/include/iri-algorithms` 
+- ${wolfvision_LIBRARIES} which is the path to the required libraries. It should have the value `/usr/local/lib/iri-algorithms/libwolflaser.so;/usr/local/lib/iri-algorithms/liblaser_scan_utils.so` 
 
 # Creating your plugin
 
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
index a72b121bde57c392cf9f13fe4a5ed73e309ddf98..90e622c1b964cfb78b1664b1ed9794f8004bff47 100644
--- a/cmake_modules/wolfConfig.cmake
+++ b/cmake_modules/wolfConfig.cmake
@@ -1,33 +1,33 @@
 #edit the following line to add the librarie's header files
 FIND_PATH(
-    wolf_INCLUDE_DIR
+    wolf_INCLUDE_DIRS
     NAMES wolf.found
     PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
-IF(wolf_INCLUDE_DIR)
-  MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIR}")
-ELSE(wolf_INCLUDE_DIR)
+IF(wolf_INCLUDE_DIRS)
+  MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}")
+ELSE(wolf_INCLUDE_DIRS)
   MESSAGE("Couldn't find wolf include dirs")
-ENDIF(wolf_INCLUDE_DIR)
+ENDIF(wolf_INCLUDE_DIRS)
 
 FIND_LIBRARY(
-    wolf_LIBRARY
+    wolf_LIBRARIES
     NAMES libwolf.so libwolf.dylib
     PATHS /usr/local/lib/iri-algorithms)
-IF(wolf_LIBRARY)
-  MESSAGE("Found wolf lib: ${wolf_LIBRARY}")
-ELSE(wolf_LIBRARY)
+IF(wolf_LIBRARIES)
+  MESSAGE("Found wolf lib: ${wolf_LIBRARIES}")
+ELSE(wolf_LIBRARIES)
   MESSAGE("Couldn't find wolf lib")
-ENDIF(wolf_LIBRARY)
+ENDIF(wolf_LIBRARIES)
 
-IF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
+IF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
    SET(wolf_FOUND TRUE)
- ELSE(wolf_INCLUDE_DIR AND wolf_LIBRARY)
+ ELSE(wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
    set(wolf_FOUND FALSE)
-ENDIF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
+ENDIF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
 
 IF (wolf_FOUND)
    IF (NOT wolf_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf: ${wolf_LIBRARY}")
+      MESSAGE(STATUS "Found wolf: ${wolf_LIBRARIES}")
    ENDIF (NOT wolf_FIND_QUIETLY)
 ELSE (wolf_FOUND)
    IF (wolf_FIND_REQUIRED)
@@ -38,7 +38,7 @@ ENDIF (wolf_FOUND)
 
 macro(wolf_report_not_found REASON_MSG)
   set(wolf_FOUND FALSE)
-  unset(wolf_INCLUDE_DIR)
+  unset(wolf_INCLUDE_DIRS)
   unset(wolf_LIBRARIES)
 
   # Reset the CMake module path to its state when this script was called.
@@ -62,4 +62,31 @@ if(NOT wolf_FOUND)
   wolf_report_not_found("Something went wrong while setting up wolf.")
 endif(NOT wolf_FOUND)
 # Set the include directories for wolf (itself).
-set(wolf_FOUND TRUE)
\ No newline at end of file
+set(wolf_FOUND TRUE)
+
+# Now we gather all the required dependencies for Wolf
+
+get_filename_component(WOLF_CURRENT_CONFIG_DIR
+  "${CMAKE_CURRENT_LIST_FILE}" PATH)
+
+SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH})
+LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLF_CURRENT_CONFIG_DIR})
+
+FIND_PACKAGE(Threads REQUIRED)
+list(APPEND wolf_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
+
+FIND_PACKAGE(Ceres REQUIRED)
+list(APPEND wolf_INCLUDE_DIRS ${CERES_INCLUDE_DIRS})
+list(APPEND wolf_LIBRARIES ${CERES_LIBRARIES})
+
+# YAML with yaml-cpp
+FIND_PACKAGE(YamlCpp REQUIRED)
+list(APPEND wolf_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS})
+list(APPEND wolf_LIBRARIES ${YAMLCPP_LIBRARY})
+
+if(NOT Eigen_FOUND)
+  FIND_PACKAGE(Eigen3 REQUIRED)
+  list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+endif()
+
+SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
\ No newline at end of file
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
index dc3608694f177138711f769ceadb7ef004665ad4..f0009526c6f9fcb8a61ae84916690a62190e32c6 100644
--- a/doc/doxygen.conf
+++ b/doc/doxygen.conf
@@ -429,7 +429,7 @@ EXTRACT_PACKAGE        = NO
 # included in the documentation.
 # The default value is: NO.
 
-EXTRACT_STATIC         = NO
+EXTRACT_STATIC         = YES 
 
 # If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
 # locally in source files will be included in the documentation. If set to NO
@@ -756,7 +756,8 @@ WARN_LOGFILE           =
 # Note: If this tag is empty the current directory is searched.
 
 INPUT                  = ../doc/main.dox \
-                         ../src
+                         ../src \
+                         ../include/core
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index b75801bde13cff3a003e0ba746bec5fb864bb813..cd7ba019179d7bfb7aa61723073028ccc3b17024 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -25,15 +25,15 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
 
     // 1. get KF
     FrameBasePtr kf(nullptr);
-    if ( !kf_pack_buffer_.empty() )
+    if ( !buffer_pack_kf_.empty() )
     {
         // KeyFrame Callback received
-        PackKeyFramePtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
+        PackKeyFramePtr pack = buffer_pack_kf_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
 
         if (pack!=nullptr)
             kf = pack->key_frame;
 
-        kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() );
+        buffer_pack_kf_.removeUpTo( _capture->getTimeStamp() );
 
         assert( kf && "Callback KF is not close enough to _capture!");
     }
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 8d251380532bfc842028405d1bf5d8f08299401f..23f3d0975565dfd2d9f123d4b78c291925bba810 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -83,6 +83,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         bool isKeyOrAux() const;
 
         // set type
+        void setNonEstimated();
         void setKey();
         void setAux();
 
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 6152670ea86394f733230805ecf0708f6b243932..6f37d56a539008cc34ba37b6227756d392900677 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -63,7 +63,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
     public:
         static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
-        static ProblemPtr autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file);
+        static ProblemPtr autoSetup(paramsServer &_server);
         virtual ~Problem();
 
         // Properties -----------------------------------------
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 72912c21db54fbc92baea6a06e891f3a2df975f0..412d3b3289d9f2ef3a00151c71d2da66dde94649 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -40,14 +40,14 @@ WOLF_PTR_TYPEDEFS(PackKeyFrame);
  *
  * Object and functions to manage a buffer of KFPack objects.
  */
-class PackKeyFrameBuffer
+class BufferPackKeyFrame
 {
     public:
 
         typedef std::map<TimeStamp,PackKeyFramePtr>::iterator Iterator; // buffer iterator
 
-        PackKeyFrameBuffer(void);
-        ~PackKeyFrameBuffer(void);
+        BufferPackKeyFrame(void);
+        ~BufferPackKeyFrame(void);
 
         /**\brief Select a Pack from the buffer
          *
@@ -142,11 +142,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
     protected:
         unsigned int processor_id_;
         ProcessorParamsBasePtr params_;
-        PackKeyFrameBuffer kf_pack_buffer_;
+        BufferPackKeyFrame buffer_pack_kf_;
 
     private:
         SensorBaseWPtr sensor_ptr_;
-
         static unsigned int processor_id_count_;
 
     public:
@@ -284,27 +283,27 @@ inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
     params_->time_tolerance = _time_tolerance;
 }
 
-inline PackKeyFrameBuffer::PackKeyFrameBuffer(void)
+inline BufferPackKeyFrame::BufferPackKeyFrame(void)
 {
 
 }
 
-inline PackKeyFrameBuffer::~PackKeyFrameBuffer(void)
+inline BufferPackKeyFrame::~BufferPackKeyFrame(void)
 {
 
 }
 
-inline void PackKeyFrameBuffer::clear()
+inline void BufferPackKeyFrame::clear()
 {
     container_.clear();
 }
 
-inline bool PackKeyFrameBuffer::empty()
+inline bool BufferPackKeyFrame::empty()
 {
     return container_.empty();
 }
 
-inline SizeStd PackKeyFrameBuffer::size(void)
+inline SizeStd BufferPackKeyFrame::size(void)
 {
     return container_.size();
 }
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index 2d216b24d5d1cdc773394e461e2792ec3ca9c510..f539a4cab26725b612f0b17a9b4fbe1af5d65cdd 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -160,17 +160,17 @@ class ProcessorTrackerLandmark : public ProcessorTracker
                                                const CaptureBasePtr& _capture,
                                                FeatureBasePtrList& _features_out) = 0;
 
-        /** \brief Emplace a landmark for each new feature of new_features_last_
+        /** \brief Emplaces a landmark for each new feature of new_features_last_
          **/
         virtual void emplaceNewLandmarks();
 
-        /** \brief Emplace one landmark
+        /** \brief Emplaces one landmark
          *
          * Implement in derived classes to build the type of landmark you need for this tracker.
          */
         virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
 
-        /** \brief Emplace a new factor
+        /** \brief Emplaces a new factor
          * \param _feature_ptr pointer to the Feature to constrain
          * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
          *
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index ca7e71d1a7de39fbb496a3f9eb00ab276815ed28..2615ad411adbe9c32efea2bf543cb24657e5ed97 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -63,12 +63,12 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> >  TrackMatches; // mat
  *
  * these fields of FeatureBase are initialized each time a feature is added to the track matrix:
  *
- *      add(Cap, track_id, f) will set f.capture_ptr = C and f.traci_id = traci_id.
+ *      add(Cap, track_id, f) will set f.capture_ptr = C and f.track_id = track_id.
  *
  * so e.g. given a feature f,
  *
  *      getTrack   (f->trackId()) ;       // returns all the track where feature f is.
- *      getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f.
+ *      getSnapshot(f->getCapture()) ;    // returns all the features in the same capture of f.
  *
  */
 
@@ -98,15 +98,23 @@ class TrackMatrix
         FeatureBasePtr  feature     (const SizeStd& _track_id, CaptureBasePtr _cap);
         CaptureBasePtr  firstCapture(const SizeStd& _track_id);
 
+        // tracks across captures that belong to keyframe
+//        SizeStd         numKeyframeTracks();
+        Track           trackAtKeyframes(size_t _track_id);
+//        bool            markKeyframe(CaptureBasePtr _capture);
+//        bool            unmarkKeyframe(CaptureBasePtr _capture);
+
     private:
 
         static SizeStd track_id_count_;
 
-        // Along track: maps of Feature pointers indexed by time stamp.
+        // tracks across all Captures
         map<SizeStd, Track > tracks_;       // map indexed by track_Id   of ( maps indexed by TimeStamp  of ( features ) )
+//        // tracks across captures that belong to keyframe
+//        map<size_t, Track > tracks_kf_;    // map indexed by track_Id   of ( maps indexed by TimeStamp  of ( features ) )
 
         // Across track: maps of Feature pointers indexed by track_Id.
-        map<SizeStd, Snapshot > snapshots_; // map indexed by capture_Id of ( maps indexed by track_Id of ( features ) )
+        map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
 };
 
 } /* namespace wolf */
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 96e64575a653cef6f9f01d0c0d9b372e0540a170..ffb955d456f515aa0871a54eea4de84df94af187 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -8,11 +8,21 @@
 #include <iostream>
 #include <array>
 
+/**
+   @file
+ */
 namespace utils{
+    //Typically we want to convert from/to list-type structures. In order to be general
+    //we define a list type which is used throughout the converter. In this case this type
+    //is implemented with std::vector
     template <typename A>
     using list = std::vector<A>;
     // template <typename A>
     // class toString<A>{};
+    /** @Brief Splits a comma-separated string into its pieces
+    * @param val is just the string of comma separated values
+    * @return <b>{std::vector<std::string>}</b> vector whose i-th component is the i-th comma separated value
+    */
     static inline std::vector<std::string> splitter(std::string val){
         std::vector<std::string> cont = std::vector<std::string>();
         std::stringstream ss(val);
@@ -22,6 +32,11 @@ namespace utils{
         }
         return cont;
     }
+    /** @Brief Returns all the substrings of @val that match @exp
+     * @param val String to be matched
+     * @param exp Regular expression
+    * @return <b>{std::vector<std::string>}</b> Collection of matching subtrings
+    */
     static inline std::vector<std::string> getMatches(std::string val, std::regex exp){
         std::smatch res;
         auto v = std::vector<std::string>();
@@ -32,10 +47,10 @@ namespace utils{
         }
         return v;
     }
-    static inline std::vector<std::string> pairSplitter(std::string val){
-        std::regex exp("(\\{[^\\{:]:.*?\\})");
-        return getMatches(val, exp);
-    }
+    /** @Brief Given a string representation of a matrix extracts the dimensions and the values
+    * @param matrix is a string either of the form "[[N,M],[a1,a2,a3,...]]" or "[a1,a2,a3,...]"
+    * @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...])
+    */
     static inline std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix){
         std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]");
         std::regex rgxStatic("\\[((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]");
@@ -52,10 +67,21 @@ namespace utils{
         }
         return values;
     }
+    /** @Brief Splits a dictionary-like string of the form {k1:v1},{k2:v2},... It is tightly coupled with splitMapStringRepresentation
+    * @param val is just a dictionary-like string
+    * @return <b>{std::vector<std::string>}</b> Collection of the strings of the form {k_i:v_i}
+    */
+    static inline std::vector<std::string> pairSplitter(std::string val){
+      std::regex exp("\\{[^\\{:]:.*?\\}");
+      return getMatches(val, exp);
+    }
+  /** @Brief Splits a dictionary-like string of the form [{k1:v1},{k2:v2},...]
+    * @param str_map just a dictionary-like string
+    * @return <b>{std::string}</b> String {k1:v1},{k2:v2},... (notice the removed brackets)
+    */
     static inline std::string splitMapStringRepresentation(std::string str_map){
         std::smatch mmatches;
         std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
-        // auto v = std::vector<std::string>();
         std::string result = "";
         if(std::regex_match(str_map, mmatches, rgxM)) {
             // v = splitter(mmatches[1].str());
@@ -71,7 +97,7 @@ namespace wolf{
 template<typename T>
 struct converter{
     static T convert(std::string val){
-        assert(1 == 0 && "There is no general convert for arbitrary T !!!");
+      throw std::runtime_error("There is no general convert for arbitrary T !!! String provided: "+ val);
     }
 };
 template<typename A>
@@ -81,7 +107,7 @@ struct converter<utils::list<A>>{
         utils::list<A> result = utils::list<A>();
         if(std::regex_match(val, rgxP)) {
             std::string aux = val.substr(1,val.size()-2);
-            auto l = utils::getMatches(aux, std::regex("([^,]+),?"));
+            auto l = utils::getMatches(aux, std::regex("([^,]+)"));
             for(auto it : l){
                 result.push_back(converter<A>::convert(it));
             }
@@ -134,8 +160,7 @@ struct converter<std::string>{
     }
     template<typename T>
     static std::string convert(T val){
-        assert(1 == 0 && "There is no general convert to string for arbitrary T !!!");
-        return "";
+        throw std::runtime_error("There is no general convert to string for arbitrary T !!! String provided: " + val);
     }
     static std::string convert(int val){
         return std::to_string(val);
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index 78ff43caa595ff4f8db37b0b860b3aef4ebf66fd..c721a163ac59abb487cfd01edbd3235f95d875d7 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -1,6 +1,7 @@
 #ifndef PARSER_YAML_HPP
 #define PARSER_YAML_HPP
 #include "yaml-cpp/yaml.h"
+#include "core/utils/converter.h"
 #include <vector>
 #include <regex>
 #include <map>
@@ -8,17 +9,20 @@
 #include <algorithm>
 #include <numeric>
 
-using namespace std;
 namespace {
     // std::string remove_ws( const std::string& str ){
     //     std::string str_no_ws ;
     //     for( char c : str ) if( !std::isspace(c) ) str_no_ws += c ;
     //     return str_no_ws ;
     // }
-    string parseSequence(YAML::Node n){
+    /** @Brief Generates a std::string [v1,v2,v3,...] representing the YAML sequence node
+    * @param n a YAML::Node
+    * @return <b>{std::string}</b> [v1,v2,v3,...]
+    */
+    std::string parseSequence(YAML::Node n){
         assert(n.Type() != YAML::NodeType::Map && "Trying to parse as a Sequence a Map node");
         if(n.Type() == YAML::NodeType::Scalar) return n.Scalar();
-        string aux = "[";
+        std::string aux = "[";
         bool first = true;
         for(auto it : n){
             if(first) {
@@ -31,15 +35,19 @@ namespace {
         aux = aux + "]";
         return aux;
     }
+    /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
+    * @param _map just a std::map<std::string,std::string>
+    * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
+    */
     std::string mapToString(std::map<std::string,std::string> _map){
         std::string result = "";
-        auto v = std::vector<string>();
-        std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<string,string> p){return "{" + p.first + ":" + p.second + "}";});
-        auto concat = [](string ac, string str)-> string {
+        auto v = std::vector<std::string>();
+        std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
+        auto concat = [](std::string ac, std::string str)-> std::string {
                           return ac + str + ",";
                       };
-        string aux = "";
-        string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
+        std::string aux = "";
+        std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
         if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
         else accumulated = "";
         return "[" + accumulated + "]";
@@ -47,134 +55,147 @@ namespace {
 }
 class parserYAML {
     struct ParamsInitSensor{
-        string _type;
-        string _name;
+        std::string _type;
+        std::string _name;
         YAML::Node n;
     };
     struct ParamsInitProcessor{
-        string _type;
-        string _name;
-        string _name_assoc_sensor;
+        std::string _type;
+        std::string _name;
+        std::string _name_assoc_sensor;
         YAML::Node n;
     };
-    map<string, string> _params;
-    string _active_name;
-    vector<ParamsInitSensor> _paramsSens;
-    vector<ParamsInitProcessor> _paramsProc;
-    vector<string> _files;
-    string _file;
+    std::map<std::string, std::string> _params;
+    std::string _active_name;
+    std::vector<ParamsInitSensor> _paramsSens;
+    std::vector<ParamsInitProcessor> _paramsProc;
+    std::vector<std::string> _files;
+    std::string _file;
     bool _relative_path;
-    string _path_root;
-    vector<array<string, 3>> _callbacks;
+    std::string _path_root;
+    std::vector<std::array<std::string, 3>> _callbacks;
+    YAML::Node problem;
+    std::string generatePath(std::string);
 public:
     parserYAML(){
-        _params = map<string, string>();
+        _params = std::map<std::string, std::string>();
         _active_name = "";
-        _paramsSens = vector<ParamsInitSensor>();
-        _paramsProc = vector<ParamsInitProcessor>();
+        _paramsSens = std::vector<ParamsInitSensor>();
+        _paramsProc = std::vector<ParamsInitProcessor>();
         _file = "";
-        _files = vector<string>();
+        _files = std::vector<std::string>();
         _path_root = "";
         _relative_path = false;
-        _callbacks = vector<array<string, 3>>();
+        _callbacks = std::vector<std::array<std::string, 3>>();
     }
-    parserYAML(string file){
-        _params = map<string, string>();
+    parserYAML(std::string file){
+        _params = std::map<std::string, std::string>();
         _active_name = "";
-        _paramsSens = vector<ParamsInitSensor>();
-        _paramsProc = vector<ParamsInitProcessor>();
-        _files = vector<string>();
+        _paramsSens = std::vector<ParamsInitSensor>();
+        _paramsProc = std::vector<ParamsInitProcessor>();
+        _files = std::vector<std::string>();
         _file = file;
         _path_root = "";
         _relative_path = false;
-        _callbacks = vector<array<string, 3>>();
+        _callbacks = std::vector<std::array<std::string, 3>>();
     }
-    parserYAML(string file, string path_root){
-        _params = map<string, string>();
+    parserYAML(std::string file, std::string path_root){
+        _params = std::map<std::string, std::string>();
         _active_name = "";
-        _paramsSens = vector<ParamsInitSensor>();
-        _paramsProc = vector<ParamsInitProcessor>();
-        _files = vector<string>();
+        _paramsSens = std::vector<ParamsInitSensor>();
+        _paramsProc = std::vector<ParamsInitProcessor>();
+        _files = std::vector<std::string>();
         _file = file;
-        _path_root = path_root;
-        _relative_path = true;
-        _callbacks = vector<array<string, 3>>();
+        if(path_root != ""){
+          std::regex r("/$");
+          if(not std::regex_match(path_root, r)) _path_root = path_root + "/";
+          else _path_root = path_root;
+          _relative_path = true;
+        }else{
+          _relative_path = false;
+        }
+        _callbacks = std::vector<std::array<std::string, 3>>();
     }
     ~parserYAML(){
         //
     }
-    void walkTree(string file);
-    void walkTree(string file, vector<string>& tags);
-    void walkTree(string file, vector<string>& tags, string hdr);
-    void walkTreeR(YAML::Node n, vector<string>& tags, string hdr);
-    void updateActiveName(string tag);
-    void parseFirstLevel(string file);
-    string tagsToString(vector<string>& tags);
-    vector<array<string, 2>> sensorsSerialization();
-    vector<array<string, 3>> processorsSerialization();
-    vector<string> getFiles();
-    vector<array<string, 3>> getCallbacks();
-    map<string,string> getParams();
+    void walkTree(std::string file);
+    void walkTree(std::string file, std::vector<std::string>& tags);
+    void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr);
+    void walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr);
+    void updateActiveName(std::string tag);
+    void parseFirstLevel(std::string file);
+    std::string tagsToString(std::vector<std::string>& tags);
+    std::vector<std::array<std::string, 2>> sensorsSerialization();
+    std::vector<std::array<std::string, 3>> processorsSerialization();
+    std::vector<std::string> getFiles();
+    std::vector<std::array<std::string, 3>> getCallbacks();
+    std::vector<std::array<std::string, 2>> getProblem();
+    std::map<std::string,std::string> getParams();
     void parse();
-    map<string, string> fetchAsMap(YAML::Node);
+    std::map<std::string, std::string> fetchAsMap(YAML::Node);
 };
-string parserYAML::tagsToString(vector<std::string> &tags){
-    string hdr = "";
+std::string parserYAML::generatePath(std::string path){
+    std::regex r("^/.*");
+    if(std::regex_match(path, r)){
+      std::cout << "Generating " + path << std::endl;
+        return path;
+    }else{
+      std::cout << "Generating " + _path_root + path << std::endl;
+        return _path_root + path;
+    }
+}
+std::string parserYAML::tagsToString(std::vector<std::string> &tags){
+    std::string hdr = "";
     for(auto it : tags){
         hdr = hdr + "/" + it;
     }
     return hdr;
 }
-void parserYAML::walkTree(string file){
+void parserYAML::walkTree(std::string file){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
-    vector<string> hdrs = vector<string>();
+    n = YAML::LoadFile(generatePath(file));
+    std::vector<std::string> hdrs = std::vector<std::string>();
     walkTreeR(n, hdrs, "");
 }
-void parserYAML::walkTree(string file, vector<string>& tags){
+void parserYAML::walkTree(std::string file, std::vector<std::string>& tags){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     walkTreeR(n, tags, "");
 }
-void parserYAML::walkTree(string file, vector<string>& tags, string hdr){
+void parserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     walkTreeR(n, tags, hdr);
 }
-void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
+/** @Brief Recursively walks the YAML tree while filling a map with the values oarsed from the file
+* @param YAML node to be parsed
+* @param tags represents the path from the root of the YAML tree to the current node
+* @param hdr is the name of the current YAML node
+*/
+void parserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr){
     switch (n.Type()) {
     case YAML::NodeType::Scalar : {
-        regex r("^@.*");
-        if(regex_match(n.Scalar(), r)){
-            string str = n.Scalar();
-            // cout << "SUBSTR " << str.substr(1,str.size() - 1);
+        std::regex r("^@.*");
+        if(std::regex_match(n.Scalar(), r)){
+            std::string str = n.Scalar();
             walkTree(str.substr(1,str.size() - 1), tags, hdr);
         }else{
-            // std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬"));
-            // cout << "«»" << n.Scalar() << endl;
-            _params.insert(pair<string,string>(hdr, n.Scalar()));
+            _params.insert(std::pair<std::string,std::string>(hdr, n.Scalar()));
         }
         break;
     }
     case YAML::NodeType::Sequence : {
-        // cout << tags[tags.size() - 1] << "«»" << kv << endl;
-        // std::vector<double> vi = n.as<std::vector<double>>();
-        string aux = parseSequence(n);
-        _params.insert(pair<string,string>(hdr, aux));
+        std::string aux = parseSequence(n);
+        _params.insert(std::pair<std::string,std::string>(hdr, aux));
         break;
     }
     case YAML::NodeType::Map : {
         for(const auto& kv : n){
             //If the key's value starts with a $ (i.e. $key) then its value is parsed as a literal map,
             //otherwise the parser recursively parses the map.
-            regex r("^\\$.*");
-            if(not regex_match(kv.first.as<string>(), r)){
+            std::regex r("^\\$.*");
+            if(not std::regex_match(kv.first.as<std::string>(), r)){
                 /*
                 If key=="follow" then the parser will assume that the value is a path and will parse
                 the (expected) yaml file at the specified path. Note that this does not increase the header depth.
@@ -197,40 +218,43 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
                  - $follow: "@some_path"        |
                  - var: 1.2                     |
                 */
-                regex rr("follow");
-                if(not regex_match(kv.first.as<string>(), rr)) {
-                    tags.push_back(kv.first.as<string>());
-                    if(tags.size() == 2) this->updateActiveName(kv.first.as<string>());
-                    walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<string>());
+                std::regex rr("follow");
+                if(not std::regex_match(kv.first.as<std::string>(), rr)) {
+                    tags.push_back(kv.first.as<std::string>());
+                    if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>());
+                    walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>());
                     tags.pop_back();
                     if(tags.size() == 1) this->updateActiveName("");
                 }else{
-                    walkTree(kv.second.as<string>(), tags, hdr);
+                    walkTree(kv.second.as<std::string>(), tags, hdr);
                 }
             }else{
-                string key = kv.first.as<string>();
+                std::string key = kv.first.as<std::string>();
                 key = key.substr(1,key.size() - 1);
                 auto fm = fetchAsMap(kv.second);
-                _params.insert(pair<string,string>(hdr + "/" + key, mapToString(fm)));
+                _params.insert(std::pair<std::string,std::string>(hdr + "/" + key, mapToString(fm)));
             }
         }
         break;
     }
     default:
-        assert(1 == 0 && "Unsupported node Type at walkTreeR");
+        assert(1 == 0 && "Unsupported node Type at walkTreeR.");
         break;
     }
 }
-void parserYAML::updateActiveName(string tag){
+void parserYAML::updateActiveName(std::string tag){
     this->_active_name = tag;
 }
-void parserYAML::parseFirstLevel(string file){
+/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
+    are defined at the top level of the YAML file.
+ * @param file is the path to the YAML file */
+void parserYAML::parseFirstLevel(std::string file){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
+
     YAML::Node n_config = n["config"];
     assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
+    this->problem = n_config["problem"];
     for(const auto& kv : n_config["sensors"]){
         ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv};
         _paramsSens.push_back(pSensor);
@@ -243,76 +267,80 @@ void parserYAML::parseFirstLevel(string file){
         _callbacks.push_back({{kv[0].as<std::string>(), kv[1].as<std::string>(), kv[2].as<std::string>()}});
     }
     YAML::Node n_files = n["files"];
-    assert(n_files.Type() == YAML::NodeType::Sequence && "trying to parse files node but found a non-Sequence node");
     for(const auto& kv : n_files){
         _files.push_back(kv.Scalar());
     }
+    _params.insert(std::pair<std::string,std::string>("files", wolf::converter<std::string>::convert(_files)));
 }
-vector<array<string, 2>> parserYAML::sensorsSerialization(){
-    vector<array<string, 2>> aux = vector<array<string, 2>>();
+std::vector<std::array<std::string, 2>> parserYAML::sensorsSerialization(){
+    std::vector<std::array<std::string, 2>> aux = std::vector<std::array<std::string, 2>>();
     for(auto it : this->_paramsSens)
         aux.push_back({{it._type,it._name}});
     return aux;
 }
-vector<array<string, 3>> parserYAML::processorsSerialization(){
-    vector<array<string, 3>> aux = vector<array<string, 3>>();
+std::vector<std::array<std::string, 3>> parserYAML::processorsSerialization(){
+    std::vector<std::array<std::string, 3>> aux = std::vector<std::array<std::string, 3>>();
     for(auto it : this->_paramsProc)
         aux.push_back({{it._type,it._name,it._name_assoc_sensor}});
     return aux;
 }
-vector<string> parserYAML::getFiles(){
+std::vector<std::string> parserYAML::getFiles(){
     return this->_files;
 }
-vector<array<string, 3>> parserYAML::getCallbacks(){
+std::vector<std::array<std::string, 3>> parserYAML::getCallbacks(){
     return this->_callbacks;
 }
-map<string,string> parserYAML::getParams(){
-    map<string,string> rtn = _params;
+std::vector<std::array<std::string, 2>> parserYAML::getProblem(){
+    return std::vector<std::array<std::string, 2>>();
+}
+std::map<std::string,std::string> parserYAML::getParams(){
+    std::map<std::string,std::string> rtn = _params;
     return rtn;
 }
 void parserYAML::parse(){
     this->parseFirstLevel(this->_file);
+
+    if(this->problem.Type() != YAML::NodeType::Undefined){
+        std::vector<std::string> tags = std::vector<std::string>();
+        this->walkTreeR(this->problem, tags , "problem");
+    }
     for(auto it : _paramsSens){
-        vector<string> tags = vector<string>();
-        // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
+        std::vector<std::string> tags = std::vector<std::string>();
         this->walkTreeR(it.n , tags , it._name);
     }
     for(auto it : _paramsProc){
-        vector<string> tags = vector<string>();
-        // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
+        std::vector<std::string> tags = std::vector<std::string>();
         this->walkTreeR(it.n , tags , it._name);
     }
 }
-map<string, string> parserYAML::fetchAsMap(YAML::Node n){
+std::map<std::string, std::string> parserYAML::fetchAsMap(YAML::Node n){
     assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
-    auto m = map<string, string>();
+    auto m = std::map<std::string, std::string>();
     for(const auto& kv : n){
-        string key = kv.first.as<string>();
+        std::string key = kv.first.as<std::string>();
         switch (kv.second.Type()) {
         case YAML::NodeType::Scalar : {
-            string value = kv.second.Scalar();
-            m.insert(pair<string,string>(key, value));
+            std::string value = kv.second.Scalar();
+            m.insert(std::pair<std::string,std::string>(key, value));
             break;
         }
         case YAML::NodeType::Sequence : {
-            // cout << tags[tags.size() - 1] << "«»" << kv << endl;
-            // std::vector<double> vi = n.as<std::vector<double>>();
-            string aux = parseSequence(kv.second);
-            m.insert(pair<string,string>(key, aux));
+            std::string aux = parseSequence(kv.second);
+            m.insert(std::pair<std::string,std::string>(key, aux));
             break;
         }
         case YAML::NodeType::Map : {
             m = fetchAsMap(kv.second);
-            auto rtn = vector<string>();
-            std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<string,string> v){return "{" + v.first + ":" + v.second + "}";});
-            auto concat = [](string ac, string str)-> string {
+            auto rtn = std::vector<std::string>();
+            std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<std::string,std::string> v){return "{" + v.first + ":" + v.second + "}";});
+            auto concat = [](std::string ac, std::string str)-> std::string {
                               return ac + str + ",";
                           };
-            string aux = "";
-            string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat);
+            std::string aux = "";
+            std::string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat);
             if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
             else accumulated = "";
-            m.insert(pair<string,string>(key, "[" + accumulated + "]"));
+            m.insert(std::pair<std::string,std::string>(key, "[" + accumulated + "]"));
             break;
         }
         default:
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 04bbaa38fff74496753765f6fd6628dc60d6b68b..0bdb038d95318698cccbd722e81c719c03d1d97b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -155,6 +155,20 @@ void FrameBase::removeStateBlocks()
     }
 }
 
+void FrameBase::setNonEstimated()
+{
+    // unregister if previously estimated
+    if (isKeyOrAux())
+        removeStateBlocks();
+
+    type_ = NON_ESTIMATED;
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
+}
+
 void FrameBase::setKey()
 {
     // register if previously not estimated
@@ -163,8 +177,11 @@ void FrameBase::setKey()
 
     // WOLF_DEBUG("Set Key", this->id());
     type_ = KEY;
-    getTrajectory()->sortFrame(shared_from_this());
-    getTrajectory()->updateLastFrames();
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
 }
 
 void FrameBase::setAux()
@@ -174,8 +191,11 @@ void FrameBase::setAux()
 
     // WOLF_DEBUG("Set Auxiliary", this->id());
     type_ = AUXILIARY;
-    getTrajectory()->sortFrame(shared_from_this());
-    getTrajectory()->updateLastFrames();
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
 }
 
 void FrameBase::fix()
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 056db02dc4c5022e4bfb0b4d505c7533f71571da..cab9b32cd98474de4dc98e087c2f77cc005c2ea1 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -11,7 +11,6 @@
 #include "core/sensor/sensor_factory.h"
 #include "core/processor/processor_factory.h"
 #include "core/state_block/state_block.h"
-#include "core/yaml/parser_yaml.hpp"
 #include "core/utils/params_server.hpp"
 #include "core/utils/loader.hpp"
 
@@ -20,6 +19,8 @@
 
 // C++ includes
 #include <algorithm>
+#include <map>
+#include <vector>
 
 namespace wolf
 {
@@ -72,34 +73,35 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
     p->setup();
     return p->shared_from_this();
 }
-ProblemPtr Problem::autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file)
-{
-    auto p = Problem::create(_frame_structure, _dim);
-    // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/src/params.yaml";
-    parserYAML parser = parserYAML(_yaml_file);
-    parser.parse();
-    paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
+ProblemPtr Problem::autoSetup(paramsServer &_server)
+{
+    // parserYAML parser = parserYAML(_yaml_file, _root_path);
+    // parser.parse();
+    // paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
+    std::string frame_structure = _server.getParam<std::string>("problem/frame structure", "PO");
+    int dim = _server.getParam<int>("problem/dimension", "2");
+    auto p = Problem::create(frame_structure, dim);
     // cout << "PRINTING SERVER MAP" << endl;
-    // server.print();
+    // _server.print();
     // cout << "-----------------------------------" << endl;
-    auto loaders = vector<Loader*>();
-    for(auto it : parser.getFiles()) {
-        cout << "LOADING " << it << endl;
+    auto loaders = std::vector<Loader*>();
+    for(auto it : _server.getParam<std::vector<std::string>>("files")) {
+        std::cout << "LOADING " << it << std::endl;
         auto l = new LoaderRaw(it);
         l->load();
         loaders.push_back(l);
     }
     //TODO: To be fixed. This prior should be set in here, but now it is set externally.
    // setPrior(Eigen::Vector3s::Zero(), 0.1*Eigen::Matrix3s::Identity(), TimeStamp(), Scalar(0.1));
-   auto sensorMap = map<string, SensorBasePtr>();
-   auto procesorMap = map<string, ProcessorBasePtr>();
-   for(auto s : server.getSensors()){
-       cout << s._name << " " << s._type << endl;
-       sensorMap.insert(pair<string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, server)));
+    auto sensorMap = std::map<std::string, SensorBasePtr>();
+    auto procesorMap = std::map<std::string, ProcessorBasePtr>();
+   for(auto s : _server.getSensors()){
+       // cout << s._name << " " << s._type << endl;
+     sensorMap.insert(std::pair<std::string, SensorBasePtr>(s._name,p->installSensor(s._type, s._name, _server)));
    }
-   for(auto s : server.getProcessors()){
-       cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
-       procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
+   for(auto s : _server.getProcessors()){
+       // cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
+      procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(s._name,p->installProcessor(s._type, s._name, s._name_assoc_sensor, _server)));
    }
    return p;
 }
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index fb9c7f921c6869976b5d5ce01fb3ebb7ce5950a7..dd79d5b10033732892cf187a898f6a862b1f872d 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -54,7 +54,7 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _
 {
     WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
     if (_keyframe_ptr != nullptr)
-        kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other);
+        buffer_pack_kf_.add(_keyframe_ptr,_time_tol_other);
 }
 
 void ProcessorBase::remove()
@@ -96,25 +96,25 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
 }
 /////////////////////////////////////////////////////////////////////////////////////////
 
-void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp)
+void BufferPackKeyFrame::removeUpTo(const TimeStamp& _time_stamp)
 {
-    PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
+    BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp);
     container_.erase(container_.begin(), post); // erasing by range
 }
 
-void PackKeyFrameBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
+void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
 {
     TimeStamp time_stamp = _key_frame->getTimeStamp();
     PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
     container_.emplace(time_stamp, kfpack);
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
 {
     if (container_.empty())
         return nullptr;
 
-    PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
+    BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp);
 
     // remove packs corresponding to removed KFs (keeping the next iterator in post)
     while (post != container_.end() && post->second->key_frame->isRemoving())
@@ -129,7 +129,7 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
 
     if (prev_exists)
     {
-        PackKeyFrameBuffer::Iterator prev = std::prev(post);
+        BufferPackKeyFrame::Iterator prev = std::prev(post);
 
         bool prev_ok = checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
 
@@ -152,12 +152,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
 
     return nullptr;
 }
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
 {
     return selectPack(_capture->getTimeStamp(), _time_tolerance);
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
 {
     // remove packs corresponding to removed KFs
     while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
@@ -181,12 +181,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time
 
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
 {
     return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
 }
 
-void PackKeyFrameBuffer::print(void)
+void BufferPackKeyFrame::print(void)
 {
     std::cout << "[ ";
     for (auto iter : container_)
@@ -196,7 +196,7 @@ void PackKeyFrameBuffer::print(void)
     std::cout << "]" << std::endl;
 }
 
-bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
+bool BufferPackKeyFrame::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
                                       const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
 {
     Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index a24cd7ed2bb9ff2336cbfe394bb32a61a245db93..0b646d71b9e670d8d81725b2a8e80b1f02187da8 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -92,7 +92,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
 
     PackKeyFramePtr pack = computeProcessingStep();
     if (pack)
-        kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+        buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
 
     switch(processing_step_)
     {
@@ -656,11 +656,11 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
         throw std::runtime_error("ProcessorMotion received data before being initialized.");
     }
 
-    PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+    PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
 
     if (pack)
     {
-        if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+        if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
         {
             WOLF_WARN("||*||");
             WOLF_INFO(" ... It seems you missed something!");
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 7acb2066ac89a2c1655f2de957c35d39b279e961..b1c67806c6ab4a7ff2a82f0c10a53ab6a3953adf 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -51,8 +51,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
     {
         case FIRST_TIME_WITH_PACK :
         {
-            PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
-            kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+            PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
+            buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
 
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
 
@@ -96,7 +96,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         case SECOND_TIME_WITH_PACK :
         {
         	// No-break case only for debug. Next case will be executed too.
-            PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
+            PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
         } // @suppress("No break at end of case")
         case SECOND_TIME_WITHOUT_PACK :
@@ -123,8 +123,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         }
         case RUNNING_WITH_PACK :
         {
-            PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
-            kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+            PackKeyFramePtr pack = buffer_pack_kf_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
+            buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
 
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
 
@@ -173,9 +173,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
                 incoming_ptr_->link(frm);
 
-                // Set state to the keyframe
-                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-
                 // Establish factors
                 establishFactors();
 
@@ -202,9 +199,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
                 frm->addCapture(incoming_ptr_);
 
-                // Set state to the keyframe
-                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-
                 // Establish factors
                 establishFactors();
 
@@ -261,7 +255,7 @@ void ProcessorTracker::computeProcessingStep()
     {
         case FIRST_TIME :
 
-            if (kf_pack_buffer_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
+            if (buffer_pack_kf_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
                 processing_step_ = FIRST_TIME_WITH_PACK;
             else // ! last && ! pack(incoming)
                 processing_step_ = FIRST_TIME_WITHOUT_PACK;
@@ -269,7 +263,7 @@ void ProcessorTracker::computeProcessingStep()
 
         case SECOND_TIME :
 
-            if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+            if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
                 processing_step_ = SECOND_TIME_WITH_PACK;
             else
                 processing_step_ = SECOND_TIME_WITHOUT_PACK;
@@ -278,7 +272,7 @@ void ProcessorTracker::computeProcessingStep()
         case RUNNING :
         default :
 
-            if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+            if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
             {
                 if (last_ptr_->getFrame()->isKey())
                 {
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index e38d33f316d801f29e2f190932984c70e4216c83..b6c9cd176ffe3fd7468d60710c434ff29fe8e1db 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -32,8 +32,8 @@ Track TrackMatrix::track(const SizeStd& _track_id)
 
 Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
 {
-    if (_capture && snapshots_.count(_capture->id()) > 0)
-        return snapshots_.at(_capture->id());
+    if (_capture && snapshots_.count(_capture) > 0)
+        return snapshots_.at(_capture);
     else
         return Snapshot();
 }
@@ -51,7 +51,7 @@ void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr)
 
     _ftr->setTrackId(_track_id);
     tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr);
-    snapshots_[_ftr->getCapture()->id()].emplace(_track_id, _ftr);        // will create new snapshot if _cap_id   is not present
+    snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr);        // will create new snapshot if _cap_id   is not present
 }
 
 void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new)
@@ -66,10 +66,11 @@ void TrackMatrix::remove(const SizeStd& _track_id)
     {
         for (auto const& pair_time_ftr : tracks_.at(_track_id))
         {
-            SizeStd cap_id = pair_time_ftr.second->getCapture()->id();
-            snapshots_.at(cap_id).erase(_track_id);
-            if (snapshots_.at(cap_id).empty())
-                snapshots_.erase(cap_id);
+            CaptureBasePtr cap = pair_time_ftr.second->getCapture();
+            snapshots_.at(cap).erase(_track_id);
+            if (snapshots_.at(cap).empty())
+                snapshots_.erase(cap);
+
         }
 
         // Remove track
@@ -80,10 +81,10 @@ void TrackMatrix::remove(const SizeStd& _track_id)
 void TrackMatrix::remove(CaptureBasePtr _cap)
 {
     // remove snapshot features from all tracks
-    if (snapshots_.count(_cap->id()))
+    if (snapshots_.count(_cap))
     {
         TimeStamp ts = _cap->getTimeStamp();
-        for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
+        for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
         {
             SizeStd trk_id = pair_trkid_ftr.first;
             tracks_.at(trk_id).erase(ts);
@@ -92,7 +93,7 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
         }
 
         // remove snapshot
-        snapshots_.erase(_cap->id());
+        snapshots_.erase(_cap);
     }
 }
 
@@ -103,13 +104,13 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
     {
         if (auto cap = _ftr->getCapture())
         {
-            tracks_   .at(_ftr->trackId()).erase(cap->getTimeStamp());
+            tracks_    .at(_ftr->trackId()).erase(cap->getTimeStamp());
             if (tracks_.at(_ftr->trackId()).empty())
                 tracks_.erase(_ftr->trackId());
 
-            snapshots_.at(cap->id())      .erase(_ftr->trackId());
-            if (snapshots_.at(cap->id()).empty())
-                snapshots_.erase(cap->id());
+            snapshots_.at(cap)      .erase(_ftr->trackId());
+            if (snapshots_.at(cap).empty())
+                snapshots_.erase(cap);
         }
     }
 }
@@ -155,8 +156,8 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
 std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
 {
     std::list<FeatureBasePtr> lst;
-    if (snapshots_.count(_cap->id()))
-        for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
+    if (snapshots_.count(_cap))
+        for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
             lst.push_back(pair_trkid_ftr.second);
     return lst;
 }
@@ -198,4 +199,23 @@ CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id)
     return firstFeature(_track_id)->getCapture();
 }
 
+Track TrackMatrix::trackAtKeyframes(size_t _track_id)
+{
+    // We assemble a track_kf on the fly by checking each capture's frame.
+    if (tracks_.count(_track_id))
+    {
+        Track track_kf;
+        for (auto& pair_ts_ftr : tracks_.at(_track_id))
+        {
+            auto& ts  = pair_ts_ftr.first;
+            auto& ftr = pair_ts_ftr.second;
+            if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->isKey())
+                track_kf[ts] = ftr;
+        }
+        return track_kf;
+    }
+    else
+        return Track();
+}
+
 }
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 4f1bba034ee2da85ec3b06aad5ec29984de023b5..1be0920fb059789e99b16f3a84d11b3a33b49699 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -410,7 +410,6 @@ void SensorBase::setProblem(ProblemPtr _problem)
 void SensorBase::link(HardwareBasePtr _hw_ptr)
 {
     assert(this->getHardware() == nullptr && "linking an already linked sensor");
-
     if(_hw_ptr)
     {
         this->setHardware(_hw_ptr);
diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp
index b49f30a2d3e3f519dcc9e525aa8652830c809b51..d8378df4616f90af772fe967e06ba892ecb516b5 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.cpp
@@ -20,6 +20,13 @@ TEST(Converter, ParseToVector)
     ASSERT_EQ(v[8],11);
     vector<string> vs {"a","b","c"};
     ASSERT_EQ(converter<string>::convert(vs), "[a,b,c]");
+    string v2 = "[first,second,third,fourth]";
+    vector<string> vv = converter<vector<string>>::convert(v2);
+    ASSERT_EQ(vv.size(),4);
+    ASSERT_EQ(vv[0],"first");
+    ASSERT_EQ(vv[1],"second");
+    ASSERT_EQ(vv[2],"third");
+    ASSERT_EQ(vv[3],"fourth");
 
 }
 TEST(Converter, ParseToEigenMatrix)
@@ -42,7 +49,11 @@ TEST(Converter, ParseToMap)
     map<string, vector<int>> m = {{"x",vector<int>{1,2}}, {"y",vector<int>{}}, {"z",vector<int>{3}}};
     ASSERT_EQ(converter<string>::convert(m), "[{x:[1,2]},{y:[]},{z:[3]}]");
 }
-
+TEST(Converter, noGeneralConvert)
+{
+  class DUMMY{};
+  EXPECT_THROW(([=]{converter<DUMMY>::convert("Should fail");}()), std::runtime_error);
+}
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 6597f0f53207c98759e882a96221a2ac0fcb00d0..1b1f1ffb61ded8028dfbe7814f6f813f76793334 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -26,7 +26,7 @@ class BufferPackKeyFrameTest : public testing::Test
 {
     public:
 
-        PackKeyFrameBuffer pack_kf_buffer;
+        BufferPackKeyFrame pack_kf_buffer;
         FrameBasePtr f10, f20, f21, f28;
         Scalar tt10, tt20, tt21, tt28;
         TimeStamp timestamp;
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index 3e83f8f89955a57342a0ec5247afb63303de69ce..dc5f5a5aa40962abba31b3e21ccdaa3aeb2d928a 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -18,7 +18,7 @@ parserYAML parse(string _file, string _path_root)
 
 TEST(ParamsServer, Default)
 {
-  auto parser = parse("/test/yaml/params1.yaml", wolf_root);
+  auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
   paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization());
   EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6);
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 97451a6060ccd3adb43fd990c929c9cfee7b8dbc..d3ba3a40587e1e732cccf4d1a6c0d39e58a0207f 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -17,7 +17,7 @@ parserYAML parse(string _file, string _path_root)
 
 TEST(ParserYAML, RegularParse)
 {
-  auto parser = parse("/test/yaml/params1.yaml", wolf_root);
+  auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
   // for(auto it : params)
   //   cout << it.first << " %% " << it.second << endl;
@@ -26,16 +26,23 @@ TEST(ParserYAML, RegularParse)
 }
 TEST(ParserYAML, ParseMap)
 {
-  auto parser = parse("/test/yaml/params2.yaml", wolf_root);
+  auto parser = parse("test/yaml/params2.yaml", wolf_root);
   auto params = parser.getParams();
   EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
 }
 TEST(ParserYAML, JumpFile)
 {
-  auto parser = parse("/test/yaml/params3.yaml", wolf_root);
+  auto parser = parse("test/yaml/params3.yaml", wolf_root);
   auto params = parser.getParams();
-  EXPECT_EQ(params["my_proc_test/max_buff_length"], "100");
-  EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false");
+  EXPECT_EQ(params["my_proc_test/extern params/max_buff_length"], "100");
+  EXPECT_EQ(params["my_proc_test/extern params/voting_active"], "false");
+}
+TEST(ParserYAML, ProblemConfig)
+{
+  auto parser = parse("test/yaml/params2.yaml", wolf_root);
+  auto params = parser.getParams();
+  EXPECT_EQ(params["problem/frame structure"], "POV");
+  EXPECT_EQ(params["problem/dimension"], "2");
 }
 int main(int argc, char **argv)
 {
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 83ffdbd5eb3a630e66d9d5914957ad83ffa0e308..8d7cafc5f699abc2f0fa9d275f57d5b4b3677821 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -19,6 +19,7 @@ class TrackMatrixTest : public testing::Test
         Eigen::Vector2s m;
         Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01;
 
+        FrameBasePtr   F0, F1, F2, F3, F4;
         CaptureBasePtr C0, C1, C2, C3, C4;
         FeatureBasePtr f0, f1, f2, f3, f4;
 
@@ -32,6 +33,14 @@ class TrackMatrixTest : public testing::Test
             C3 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 3.0);
             C4 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 4.0);
 
+            // unlinked frames
+            // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
+            F0 = FrameBase::emplace<FrameBase>(nullptr, 0.0, nullptr);
+            F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, nullptr);
+            F2 = FrameBase::emplace<FrameBase>(nullptr, 2.0, nullptr);
+            F3 = FrameBase::emplace<FrameBase>(nullptr, 3.0, nullptr);
+            F4 = FrameBase::emplace<FrameBase>(nullptr, 4.0, nullptr);
+
             // unlinked features
             // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
             f0 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
@@ -39,6 +48,17 @@ class TrackMatrixTest : public testing::Test
             f2 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
             f3 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
             f4 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
+
+            // F0 and F4 are keyframes
+            F0->setKey();
+            F4->setKey();
+
+            // link captures
+            C0->link(F0);
+            C1->link(F1);
+            C2->link(F2);
+            C3->link(F3);
+            C4->link(F4);
         }
 };
 
@@ -46,12 +66,20 @@ TEST_F(TrackMatrixTest, newTrack)
 {
     f0->link(C0);
     f1->link(C1);
+    f2->link(C1);
+    f3->link(C1);
 
     track_matrix.newTrack(f0);
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
 
     track_matrix.newTrack(f1);
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+
+    track_matrix.newTrack(f2);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3);
+
+    track_matrix.newTrack(f3);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4);
 }
 
 TEST_F(TrackMatrixTest, add)
@@ -61,17 +89,19 @@ TEST_F(TrackMatrixTest, add)
     f2->link(C2);
 
     track_matrix.newTrack(f0);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
 
     track_matrix.add(f0->trackId(), f1);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1        snapshots
      *
      *  f0---f1        trk 0
      */
     ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
     ASSERT_EQ(f1->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
 
     track_matrix.add(f0->trackId(), f2);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1   C2   snapshots
      *
      *  f0---f1---f2   trk 0
      */
@@ -84,11 +114,12 @@ TEST_F(TrackMatrixTest, add2)
     f0->link(C0);
     f1->link(C1);
     f2->link(C2);
+    f3->link(C1);
 
     track_matrix.newTrack(f0);
 
     track_matrix.add(f0, f1);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1        snapshots
      *
      *  f0---f1        trk 0
      */
@@ -96,12 +127,23 @@ TEST_F(TrackMatrixTest, add2)
     ASSERT_EQ(f1->trackId(), f0->trackId());
 
     track_matrix.add(f1, f2);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1   C2   snapshots
      *
      *  f0---f1---f2   trk 0
      */
     ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
     ASSERT_EQ(f2->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+
+    track_matrix.newTrack(f3);
+    /* KC0   C1   C2   snapshots
+     *
+     *  f0---f1---f2   trk 0
+     *       f3        trk 1
+     */
+    ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1);
+    ASSERT_NE(f3->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
 }
 
 TEST_F(TrackMatrixTest, numTracks)
@@ -205,8 +247,18 @@ TEST_F(TrackMatrixTest, remove_ftr)
     ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1);
 
     track_matrix.remove(f1);
+    /*  C0   C1   C2   snapshots
+     *
+     *  f2             trk 1
+     */
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
     ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
+
+    track_matrix.remove(f2);
+    /*  C0   C1   C2   snapshots
+     *
+     */
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
 }
 
 TEST_F(TrackMatrixTest, remove_trk)
@@ -403,6 +455,36 @@ TEST_F(TrackMatrixTest, matches)
     ASSERT_EQ(pairs.size(), (unsigned int) 0);
 }
 
+TEST_F(TrackMatrixTest, trackAtKeyframes)
+{
+    f0->link(C0);
+    f1->link(C1);
+    f2->link(C2);
+    f3->link(C1);
+    f4->link(C4);
+
+    track_matrix.newTrack(f0);
+    track_matrix.add(f0->trackId(), f1);
+    track_matrix.add(f0->trackId(), f2);
+    track_matrix.add(f0->trackId(), f4);
+    track_matrix.newTrack(f3);
+    /* KC0   C1   C2   C3  KC4 snapshots
+     *
+     *  f0---f1---f2--------f4 trk 0
+     *       |
+     *       f3                trk 1
+     */
+
+    wolf::Track trk_kf_0 = track_matrix.trackAtKeyframes(f0->trackId());
+    ASSERT_EQ(trk_kf_0.size(), 2);
+    ASSERT_EQ(trk_kf_0[0.0], f0);
+    ASSERT_EQ(trk_kf_0[4.0], f4);
+
+    wolf::Track trk_kf_1 = track_matrix.trackAtKeyframes(f3->trackId());
+    ASSERT_EQ(trk_kf_1.size(), 0);
+}
+
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
diff --git a/test/params1.yaml b/test/params1.yaml
deleted file mode 100644
index 4ad74198329b61d86b023f17f9a7016224d5489a..0000000000000000000000000000000000000000
--- a/test/params1.yaml
+++ /dev/null
@@ -1 +0,0 @@
-      follow: "/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/params3.yaml b/test/params3.yaml
deleted file mode 100644
index 78489d218fad394f90364a1e1d758daa0270e7cd..0000000000000000000000000000000000000000
--- a/test/params3.yaml
+++ /dev/null
@@ -1 +0,0 @@
-      jump: "@/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
index 940d3e5854a5ec612f5223251243e643c7bef79b..d7d066b53a333d6beb27b20d7eef7468b29f505e 100644
--- a/test/yaml/params1.yaml
+++ b/test/yaml/params1.yaml
@@ -1,4 +1,7 @@
 config:
+  problem:
+    frame structure: "POV"
+    dimension: 3
   sensors: 
     -
       type: "ODOM 2D"
@@ -24,7 +27,7 @@ config:
       type: "ODOM 2D"
       name: "my_proc_test"
       sensor name: "odom"
-      follow: "/test/yaml/params3.1.yaml"
+      follow: "test/yaml/params3.1.yaml"
 files:
   - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
   - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml
index d58014fbabc36387be1a96cc4244cff954ac2820..7830e8cb85f9fef0acea46e81fa293ed7528fc5c 100644
--- a/test/yaml/params2.yaml
+++ b/test/yaml/params2.yaml
@@ -1,4 +1,7 @@
 config:
+  problem:
+    frame structure: "POV"
+    dimension: 2
   sensors: 
     -
       type: "ODOM 2D"
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
index ac82cf0dd818e6c6dec00c61ff86b75c38a3fc11..3e0d7a40f9fce04d9f6976518fe3bb900dabdbf9 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -1,31 +1,19 @@
 config:
-  sensors: 
+  # problem:
+  #   frame structure: "POV"
+  #   dimension: 2
+  sensors:
     -
       type: "ODOM 2D"
       name: "odom"
       intrinsic:
         k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1 
+        k_rot_to_rot: 0.1
       extrinsic:
         pos: [1,2,3]
-    -
-      type: "RANGE BEARING"
-      name: "rb"
   processors:
-    -
-      type: "ODOM 2D"
-      name: "processor1"
-      sensor name: "odom"
-    -
-      type: "RANGE BEARING"
-      name: "rb_processor"
-      sensor name: "rb"
     -
       type: "ODOM 2D"
       name: "my_proc_test"
       sensor name: "odom"
-      follow: "/test/yaml/params3.1.yaml"
-      jump: "@/test/yaml/params3.1.yaml"
-files:
-  - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
-  - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
+      extern params: "@test/yaml/params3.1.yaml"
\ No newline at end of file